{
#endif
-float clamp(float v);
int getAngleChange(float *R, float *prevR, float *angleChange);
int quatToMatrix(float *quat, float *R);
-int matrixToQuat(float *mat, float *q);
int getRotationMatrix(float *accel, float *geo, float *R, float *I);
int remapCoordinateSystem(float *inR, int X, int Y, float *outR);
-int getDeclination(float *decl);
-int getInclination(float *incl);
int setCoordinate(float latitude, float longitude, float altitude, float *declination, float *inclination, int option);
#ifdef __cplusplus
#include <math.h>
#include <stdlib.h>
-float clamp(float v)
-{
- return (v < 0) ? 0.0 : v;
-}
-
int getAngleChange(float *R, float *prevR, float *angleChange)
{
if (R == NULL || prevR == NULL || angleChange == NULL)
return 0;
}
-int matrixToQuat(float *mat, float *q)
-{
- if (q == NULL || mat == NULL)
- return -1;
-
- const float Hx = mat[0];
- const float My = mat[4];
- const float Az = mat[8];
- q[0] = sqrtf(clamp(Hx - My - Az + 1) * 0.25f);
- q[1] = sqrtf(clamp(-Hx + My - Az + 1) * 0.25f);
- q[2] = sqrtf(clamp(-Hx - My + Az + 1) * 0.25f);
- q[3] = sqrtf(clamp(Hx + My + Az + 1) * 0.25f);
- q[0] = copysignf(q[0], mat[7] - mat[5]);
- q[1] = copysignf(q[1], mat[2] - mat[6]);
- q[2] = copysignf(q[2], mat[3] - mat[1]);
-
- return 0;
-}
-
int getRotationMatrix(float *accel, float *geo, float *R, float *I)
{
if (accel == NULL || geo == NULL || R == NULL || I == NULL)
return ((f1 - precision) < f2) && ((f1 + precision) > f2);
}
-int getDeclination(float *decl)
-{
- if (decl == NULL)
- return -1;
-
- *decl = g_declination;
-
- return 0;
-}
-
-int getInclination(float *incl)
-{
- if (incl == NULL)
- return -1;
-
- *incl = g_inclination;
-
- return 0;
-}
-
int setCoordinate(float latitude, float longitude, float altitude, float *declination, float *inclination, int option)
{
float dec, dip, ti, gv;