void vector_difference_to_euler_angles(fixedpointnum *v1, fixedpointnum *v2, fixedpointnum *euler) {
// take the difference between the two attitudes and return the euler angles between them
// find the axis of rotation and angle between the two downVectors
// the cross products of the two vectors will give us the axis of rotation from one to the other
fixedpointnum axisofrotation[3];
vector_cross_product(v1, v2, axisofrotation);
fixedpointnum axislength=lib_fp_sqrt(normalize_vector(axisofrotation));
// get the angle of rotation between the two vectors
fixedpointnum angle=lib_fp_atan2(axislength, vector_dot_product(v1, v2));
fixedpointnum unitvector[3];
unitvector[0]=0;
unitvector[1]=FIXEDPOINTONE;
unitvector[2]=0;
euler[0]=lib_fp_multiply(vector_dot_product(axisofrotation, unitvector), angle);
unitvector[0]=FIXEDPOINTONE;
unitvector[1]=0;
unitvector[2]=0;
euler[1]=lib_fp_multiply(vector_dot_product(axisofrotation, unitvector), angle);
}
fixedpointnum navigation_getdistanceandbearing(fixedpointnum lat1,fixedpointnum lon1,fixedpointnum lat2,fixedpointnum lon2,fixedpointnum *bearing)
{ // returns fixedpointnum distance in meters and bearing in fixedpointnum degrees from point 1 to point 2
fixedpointnum latdiff=lat2-lat1;
fixedpointnum londiff=lib_fp_multiply(lon2-lon1,lib_fp_cosine(lat1>>LATLONGEXTRASHIFT));
*bearing = FIXEDPOINT90 + lib_fp_atan2(-latdiff, londiff);
if (*bearing >FIXEDPOINT180) *bearing -= FIXEDPOINT360;
// distance is 111319 meters per degree. This factor needs to be shifted 16 to make it a fixedpointnum.
// Since lat and lon are already shifted by 6,
// we will shift by 10 more total. Shift lat and long by 8 and the constant by 2 to get the extra 10.
// The squaring will overflow our fixedpointnum at distances greater than 1000 meters or so, so test for size and shift accordingly
if (lib_fp_abs(latdiff)+lib_fp_abs(londiff)>40000L)
{ // for big distances, don't shift lat and long. Instead shift the constant by 10.
// this will get us to 32 kilometers at which point our fixedpoingnum can't hold a larger distance.
return(lib_fp_multiply(lib_fp_sqrt(lib_fp_multiply(latdiff,latdiff)+lib_fp_multiply(londiff,londiff)),113990656L));
}
else
{
latdiff=latdiff<<8;
londiff=londiff<<8;
return(lib_fp_multiply(lib_fp_sqrt(lib_fp_multiply(latdiff,latdiff)+lib_fp_multiply(londiff,londiff)),445276L));
}
}
