// Calculates the distance between two GPS lat/lon coordinates // returns units: KM // From: http://www.movable-type.co.uk/scripts/latlong.html sll DistanceBetweenSLL(sll lat1, sll lon1, sll lat2, sll lon2) { sll R = int2sll(6371); // kilometres sll lat1_rad = slldeg2rad(lat1); sll lon1_rad = slldeg2rad(lon1); sll lat2_rad = slldeg2rad(lat2); sll lon2_rad = slldeg2rad(lon2); sll dist_lat = sllsub(lat2_rad, lat1_rad); sll dist_lon = sllsub(lon2_rad, lon1_rad); dist_lat = slldiv2(dist_lat); dist_lon = slldiv2(dist_lon); dist_lat = sllsin(dist_lat); dist_lon = sllsin(dist_lon); sll a = sllmul(dist_lat, dist_lat); sll b = sllmul(dist_lon, dist_lon); sll c = sllmul(sllcos(lat1_rad), sllcos(lat2_rad)); sll d = slladd(a, sllmul(b,c)); sll d_sqrt = sllsqrt(d); sll d_sqrt_1 = sllsqrt(sllsub(CONST_1,d)); sll result = sllmul2(sllatan2(d_sqrt, d_sqrt_1)); return sllmul(R,result); }
int gl_V3_Norm(V3 *a) { GLfloat n; n=sllsqrt(slladd(slladd(sllmul(a->X,a->X), sllmul(a->Y,a->Y)), sllmul(a->Z,a->Z))); if (sllvalue(n)==sllvalue(int2sll(0))) return 1; a->X=slldiv(a->X, n); a->Y=slldiv(a->Y, n); a->Z=slldiv(a->Z, n); return 0; }
void glopRotate(GLContext *c,GLParam *p) { #define SLL_M_PI dbl2sll(M_PI) M4 m; GLfloat u[3]; GLfloat angle; int dir_code; angle = slldiv(sllmul(p[1].f, SLL_M_PI), int2sll(180)); u[0]=p[2].f; u[1]=p[3].f; u[2]=p[4].f; /* simple case detection */ dir_code = ((sllvalue(u[0]) != sllvalue(int2sll(0)))<<2) | ((sllvalue(u[1]) != sllvalue(int2sll(0)))<<1) | (sllvalue(u[2]) != sllvalue(int2sll(0))); switch(dir_code) { case 0: gl_M4_Id(&m); break; case 4: if (sllvalue(u[0]) < sllvalue(int2sll(0))) angle=sllneg(angle); gl_M4_Rotate(&m,angle,0); break; case 2: if (sllvalue(u[1]) < sllvalue(int2sll(0))) angle=sllneg(angle); gl_M4_Rotate(&m,angle,1); break; case 1: if (sllvalue(u[2]) < sllvalue(int2sll(0))) angle=sllneg(angle); gl_M4_Rotate(&m,angle,2); break; default: { GLfloat cost, sint; /* normalize vector */ GLfloat len = slladd( slladd( sllmul(u[0],u[0]), sllmul(u[1],u[1]) ), sllmul(u[2],u[2])); if (sllvalue(len) == sllvalue(int2sll(0))) return; len = slldiv(int2sll(1), sllsqrt(len)); u[0] = sllmul(u[0], len); u[1] = sllmul(u[1], len); u[2] = sllmul(u[2], len); /* store cos and sin values */ cost=sllcos(angle); sint=sllsin(angle); /* fill in the values */ m.m[3][0]=m.m[3][1]=m.m[3][2]= m.m[0][3]=m.m[1][3]=m.m[2][3]=int2sll(0); m.m[3][3]=int2sll(1); /* do the math */ m.m[0][0]=slladd(sllmul(u[0],u[0]), sllmul(cost, sllsub(int2sll(1), sllmul(u[0],u[0])))); m.m[1][0]=sllsub(sllmul(sllmul(u[0],u[1]), sllsub(int2sll(1), cost)), sllmul(u[2],sint)); m.m[2][0]=slladd(sllmul(sllmul(u[2],u[0]), sllsub(int2sll(1), cost)), sllmul(u[1],sint)); m.m[0][1]=slladd(sllmul(sllmul(u[0],u[1]), sllsub(int2sll(1), cost)), sllmul(u[2],sint)); m.m[1][1]=slladd(sllmul(u[1],u[1]), sllmul(cost, sllsub(int2sll(1), sllmul(u[1],u[1])))); m.m[2][1]=sllsub(sllmul(sllmul(u[1],u[2]), sllsub(int2sll(1), cost)), sllmul(u[0],sint)); m.m[0][2]=sllsub(sllmul(sllmul(u[2],u[0]), sllsub(int2sll(1), cost)), sllmul(u[1],sint)); m.m[1][2]=slladd(sllmul(sllmul(u[1],u[2]), sllsub(int2sll(1), cost)), sllmul(u[0],sint)); m.m[2][2]=slladd(sllmul(u[2],u[2]), sllmul(cost, sllsub(int2sll(1), sllmul(u[2],u[2])))); } } gl_M4_MulLeft(c->matrix_stack_ptr[c->matrix_mode],&m); gl_matrix_update(c); }