39 #define SMALLQ0 0.01F // limit of quaternion scalar component requiring special algorithm
40 #define CORRUPTQUAT 0.001F // threshold for deciding rotation quaternion is corrupt
41 #define SMALLMODULUS 0.01F // limit where rounding errors may appear
57 fmodGxyz = fmodGyz + fGs[
CHX] * fGs[
CHX];
85 fmodGyz = sqrtf(fmodGyz);
86 fmodGxyz = sqrtf(fmodGxyz);
87 frecipmodGxyz = 1.0F / fmodGxyz;
88 ftmp = fmodGxyz / fmodGyz;
91 for (i =
CHX; i <=
CHZ; i++)
93 fR[i][
CHZ] = fGs[i] * frecipmodGxyz;
97 fR[
CHX][
CHX] = fmodGyz * frecipmodGxyz;
132 fmodGxyz = fmodGxz + fGs[
CHY] * fGs[
CHY];
135 if (fmodGxyz == 0.0F)
146 if (fGs[
CHY] >= 0.0F)
160 fmodGxz = sqrtf(fmodGxz);
161 fmodGxyz = sqrtf(fmodGxyz);
162 frecipmodGxyz = 1.0F / fmodGxyz;
163 ftmp = fmodGxyz / fmodGxz;
170 for (i =
CHX; i <=
CHZ; i++)
172 fR[i][
CHZ] = -fGs[i] * frecipmodGxyz;
182 fR[
CHY][
CHY] = -fmodGxz * frecipmodGxyz;
199 fmodBxy = sqrtf(fBc[
CHX] * fBc[
CHX] + fBc[
CHY] * fBc[
CHY]);
227 fmodBxy = sqrtf(fBc[
CHX] * fBc[
CHX] + fBc[
CHY] * fBc[
CHY]);
258 void feCompassNED(
float fR[][3],
float *pfDelta,
float fBc[],
float fGs[])
271 for (i =
CHX; i <=
CHZ; i++)
293 if (!((fmod[CHX] == 0.0F) || (fmod[CHY] == 0.0F) || (fmod[CHZ] == 0.0F)))
296 for (j = CHX; j <=
CHZ; j++)
298 ftmp = 1.0F / fmod[j];
300 for (i = CHX; i <=
CHZ; i++)
315 fmodBc = sqrtf(fBc[CHX] * fBc[CHX] + fBc[CHY] * fBc[CHY] + fBc[CHZ] * fBc[CHZ]);
317 if (!((fmod[CHZ] == 0.0F) || (fmodBc == 0.0F)))
319 *pfDelta =
fasin_deg(fGsdotBc / (fmod[CHZ] * fmodBc));
339 for (i =
CHX; i <=
CHZ; i++)
361 if (!((fmod[CHX] == 0.0F) || (fmod[CHY] == 0.0F) || (fmod[CHZ] == 0.0F)))
364 for (j = CHX; j <=
CHZ; j++)
366 ftmp = 1.0F / fmod[j];
368 for (i = CHX; i <=
CHZ; i++)
383 fmodBc = sqrtf(fBc[CHX] * fBc[CHX] + fBc[CHY] * fBc[CHY] + fBc[CHZ] * fBc[CHZ]);
385 if (!((fmod[CHZ] == 0.0F) || (fmodBc == 0.0F)))
387 *pfDelta = -
fasin_deg(fGsdotBc / (fmod[CHZ] * fmodBc));
407 for (i =
CHX; i <=
CHZ; i++)
409 fR[i][
CHZ] = -fGs[i];
429 if (!((fmod[CHX] == 0.0F) || (fmod[CHY] == 0.0F) || (fmod[CHZ] == 0.0F)))
432 for (j = CHX; j <=
CHZ; j++)
434 ftmp = 1.0F / fmod[j];
436 for (i = CHX; i <=
CHZ; i++)
451 fmodBc = sqrtf(fBc[CHX] * fBc[CHX] + fBc[CHY] * fBc[CHY] + fBc[CHZ] * fBc[CHZ]);
453 if (!((fmod[CHZ] == 0.0F) || (fmodBc == 0.0F)))
455 *pfDelta =
fasin_deg(fGsdotBc / (fmod[CHZ] * fmodBc));
464 float *pfDelta6DOF,
float fBc[],
float fGs[],
float *pfQvBQd,
float *pfQvGQa)
478 float famOvermodBccosDelta;
479 float famOvermodBcsinDelta;
486 fmodGs = sqrtf(fmodGsSq);
487 fmodBc = sqrtf(fmodBcSq);
488 if ((fmodGs == 0.0F) || (fmodBc == 0.0F) || (fB == 0.0))
491 pfq->
q1 = pfq->
q2 = pfq->
q3 = 0.0F;
496 *pfQvGQa = fabsf(fmodGsSq - 1.0F);
497 *pfQvBQd = fabsf(fmodBcSq - fB * fB);
498 fag = *pfQvBQd / (fB * fB * *pfQvGQa + *pfQvBQd);
502 fagOvermodGs = fag / fmodGs;
503 famOvermodBc = fam / fmodBc;
504 famOvermodBccosDelta = famOvermodBc * fcosDelta;
505 famOvermodBcsinDelta = famOvermodBc * fsinDelta;
509 *pfDelta6DOF =
fasin_deg((fGsdotBc) / (fmodGs * fmodBc));
512 fK[0][0] = fK[3][3] = fagOvermodGs * fGs[
CHZ];
513 fK[1][1] = fK[2][2] = -fK[0][0];
514 fK[0][1] = fK[2][3] = fagOvermodGs * fGs[
CHY];
515 fK[1][3] = fagOvermodGs * fGs[
CHX];
516 fK[0][2] = -fK[1][3];
519 ftmp = famOvermodBcsinDelta * fBc[
CHY];
522 fK[1][2] = famOvermodBccosDelta * fBc[
CHY];
523 fK[0][3] = -fK[1][2];
524 ftmp = famOvermodBccosDelta * fBc[
CHX];
529 ftmp = famOvermodBcsinDelta * fBc[
CHZ];
534 ftmp = famOvermodBccosDelta * fBc[
CHZ];
537 ftmp = famOvermodBcsinDelta * fBc[
CHX];
554 if (eigval[1] > eigval[i]) i = 1;
555 if (eigval[2] > eigval[i]) i = 2;
556 if (eigval[3] > eigval[i]) i = 3;
557 pfq->
q0 = eigvec[0][i];
558 pfq->
q1 = eigvec[1][i];
559 pfq->
q2 = eigvec[2][i];
560 pfq->
q3 = eigvec[3][i];
577 float *pfDelta6DOF,
float fBc[],
float fGs[],
float *pfQvBQd,
float *pfQvGQa)
591 float famOvermodBccosDelta;
592 float famOvermodBcsinDelta;
599 fmodGs = sqrtf(fmodGsSq);
600 fmodBc = sqrtf(fmodBcSq);
601 if ((fmodGs == 0.0F) || (fmodBc == 0.0F) || (fB == 0.0))
604 pfq->
q1 = pfq->
q2 = pfq->
q3 = 0.0F;
609 *pfQvGQa = fabsf(fmodGsSq - 1.0F);
610 *pfQvBQd = fabsf(fmodBcSq - fB * fB);
611 fag = *pfQvBQd / (fB * fB * *pfQvGQa + *pfQvBQd);
615 fagOvermodGs = fag / fmodGs;
616 famOvermodBc = fam / fmodBc;
617 famOvermodBccosDelta = famOvermodBc * fcosDelta;
618 famOvermodBcsinDelta = famOvermodBc * fsinDelta;
622 *pfDelta6DOF = -
fasin_deg((fGsdotBc) / (fmodGs * fmodBc));
625 fK[0][0] = fK[3][3] = fagOvermodGs * fGs[
CHZ];
626 fK[1][1] = fK[2][2] = -fK[0][0];
627 fK[0][1] = fK[2][3] = fagOvermodGs * fGs[
CHY];
628 fK[1][3] = fagOvermodGs * fGs[
CHX];
629 fK[0][2] = -fK[1][3];
632 ftmp = famOvermodBcsinDelta * fBc[
CHX];
635 fK[0][3] = fK[1][2] = famOvermodBccosDelta * fBc[
CHX];
636 ftmp = famOvermodBccosDelta * fBc[
CHY];
641 ftmp = famOvermodBcsinDelta * fBc[
CHZ];
646 ftmp = famOvermodBccosDelta * fBc[
CHZ];
649 ftmp = famOvermodBcsinDelta * fBc[
CHY];
666 if (eigval[1] > eigval[i]) i = 1;
667 if (eigval[2] > eigval[i]) i = 2;
668 if (eigval[3] > eigval[i]) i = 3;
669 pfq->
q0 = eigvec[0][i];
670 pfq->
q1 = eigvec[1][i];
671 pfq->
q2 = eigvec[2][i];
672 pfq->
q3 = eigvec[3][i];
689 float *pfDelta6DOF,
float fBc[],
float fGs[],
float *pfQvBQd,
float *pfQvGQa)
703 float famOvermodBccosDelta;
704 float famOvermodBcsinDelta;
711 fmodGs = sqrtf(fmodGsSq);
712 fmodBc = sqrtf(fmodBcSq);
713 if ((fmodGs == 0.0F) || (fmodBc == 0.0F) || (fB == 0.0))
716 pfq->
q1 = pfq->
q2 = pfq->
q3 = 0.0F;
721 *pfQvGQa = fabsf(fmodGsSq - 1.0F);
722 *pfQvBQd = fabsf(fmodBcSq - fB * fB);
723 fag = *pfQvBQd / (fB * fB * *pfQvGQa + *pfQvBQd);
727 fagOvermodGs = fag / fmodGs;
728 famOvermodBc = fam / fmodBc;
729 famOvermodBccosDelta = famOvermodBc * fcosDelta;
730 famOvermodBcsinDelta = famOvermodBc * fsinDelta;
734 *pfDelta6DOF =
fasin_deg((fGsdotBc) / (fmodGs * fmodBc));
737 fK[0][0] = fK[3][3] = -fagOvermodGs * fGs[
CHZ];
738 fK[1][1] = fK[2][2] = -fK[0][0];
739 fK[0][1] = fK[2][3] = -fagOvermodGs * fGs[
CHY];
740 fK[1][3] = -fagOvermodGs * fGs[
CHX];
741 fK[0][2] = -fK[1][3];
744 ftmp = famOvermodBcsinDelta * fBc[
CHX];
747 fK[0][3] = fK[1][2] = famOvermodBccosDelta * fBc[
CHX];
748 ftmp = famOvermodBccosDelta * fBc[
CHY];
753 ftmp = famOvermodBcsinDelta * fBc[
CHZ];
758 ftmp = famOvermodBccosDelta * fBc[
CHZ];
761 ftmp = famOvermodBcsinDelta * fBc[
CHY];
778 if (eigval[1] > eigval[i]) i = 1;
779 if (eigval[2] > eigval[i]) i = 2;
780 if (eigval[3] > eigval[i]) i = 3;
781 pfq->
q0 = eigvec[0][i];
782 pfq->
q1 = eigvec[1][i];
783 pfq->
q2 = eigvec[2][i];
784 pfq->
q3 = eigvec[3][i];
800 float *pfRhoDeg,
float *pfChiDeg)
809 if (*pfPhiDeg == 180.0F)
815 if (*pfTheDeg == 90.0F)
818 *pfPsiDeg =
fatan2_deg(R[CHZ][
CHY], R[CHY][CHY]) + *pfPhiDeg;
820 else if (*pfTheDeg == -90.0F)
823 *pfPsiDeg =
fatan2_deg(-R[CHZ][
CHY], R[CHY][CHY]) - *pfPhiDeg;
832 if (*pfPsiDeg < 0.0F)
838 if (*pfPsiDeg >= 360.0F)
844 *pfRhoDeg = *pfPsiDeg;
854 float *pfRhoDeg,
float *pfChiDeg)
863 if (*pfTheDeg == 180.0F)
869 if (*pfPhiDeg == 90.0F)
874 else if (*pfPhiDeg == -90.0F)
886 if (*pfPsiDeg < 0.0F)
892 if (*pfPsiDeg >= 360.0F)
899 *pfRhoDeg = *pfPsiDeg;
909 float *pfRhoDeg,
float *pfChiDeg)
936 if (R[CHZ][CHZ] < 0.0F)
939 *pfTheDeg = 180.0F - *pfTheDeg;
943 if (*pfTheDeg >= 180.0F)
949 if (*pfTheDeg == 90.0F)
954 else if (*pfTheDeg == -90.0F)
965 if (fabsf(*pfTheDeg) >= 90.0F)
972 if (*pfPsiDeg < 0.0F)
978 if (*pfPsiDeg >= 360.0F)
984 *pfRhoDeg = 360.0F - *pfPsiDeg;
987 if (*pfRhoDeg >= 360.0F)
1010 fetadeg = fscaling * sqrtf(rvecdeg[
CHX] * rvecdeg[
CHX] + rvecdeg[
CHY] * rvecdeg[
CHY] + rvecdeg[
CHZ] * rvecdeg[
CHZ]);
1012 fetarad2 = fetarad * fetarad;
1016 if (fetarad2 <= 0.02F)
1019 sinhalfeta = fetarad * (0.5F -
ONEOVER48 * fetarad2);
1021 else if (fetarad2 <= 0.06F)
1025 fetarad4 = fetarad2 * fetarad2;
1031 sinhalfeta = (float)sinf(0.5F * fetarad);
1035 if (fetadeg != 0.0F)
1038 ftmp = fscaling * sinhalfeta / fetadeg;
1039 pq->
q1 = rvecdeg[
CHX] * ftmp;
1040 pq->
q2 = rvecdeg[
CHY] * ftmp;
1041 pq->
q3 = rvecdeg[
CHZ] * ftmp;
1046 pq->
q1 = pq->
q2 = pq->
q3 = 0.0F;
1051 fvecsq = pq->
q1 * pq->
q1 + pq->
q2 * pq->
q2 + pq->
q3 * pq->
q3;
1055 pq->
q0 = sqrtf(1.0F - fvecsq);
1078 pq->
q0 = sqrtf(fabsf(fq0sq));
1084 recip4q0 = 0.25F / pq->
q0;
1094 pq->
q1 = sqrtf(fabsf(0.5F * (1.0F + R[
CHX][
CHX]) - fq0sq));
1095 pq->
q2 = sqrtf(fabsf(0.5F * (1.0F + R[
CHY][
CHY]) - fq0sq));
1096 pq->
q3 = sqrtf(fabsf(0.5F * (1.0F + R[
CHZ][
CHZ]) - fq0sq));
1099 if ((R[CHY][CHZ] - R[CHZ][CHY]) < 0.0F) pq->
q1 = -pq->
q1;
1100 if ((R[CHZ][CHX] - R[CHX][CHZ]) < 0.0F) pq->
q2 = -pq->
q2;
1101 if ((R[CHX][CHY] - R[CHY][CHX]) < 0.0F) pq->
q3 = -pq->
q3;
1111 float f2q0q0, f2q0q1, f2q0q2, f2q0q3;
1112 float f2q1q1, f2q1q2, f2q1q3;
1113 float f2q2q2, f2q2q3;
1117 f2q = 2.0F * pq->
q0;
1118 f2q0q0 = f2q * pq->
q0;
1119 f2q0q1 = f2q * pq->
q1;
1120 f2q0q2 = f2q * pq->
q2;
1121 f2q0q3 = f2q * pq->
q3;
1123 f2q = 2.0F * pq->
q1;
1124 f2q1q1 = f2q * pq->
q1;
1125 f2q1q2 = f2q * pq->
q2;
1126 f2q1q3 = f2q * pq->
q3;
1128 f2q = 2.0F * pq->
q2;
1129 f2q2q2 = f2q * pq->
q2;
1130 f2q2q3 = f2q * pq->
q3;
1131 f2q3q3 = 2.0F * pq->
q3 * pq->
q3;
1134 R[
CHX][
CHX] = f2q0q0 + f2q1q1 - 1.0F;
1135 R[
CHX][
CHY] = f2q1q2 + f2q0q3;
1136 R[
CHX][
CHZ] = f2q1q3 - f2q0q2;
1137 R[
CHY][
CHX] = f2q1q2 - f2q0q3;
1138 R[
CHY][
CHY] = f2q0q0 + f2q2q2 - 1.0F;
1139 R[
CHY][
CHZ] = f2q2q3 + f2q0q1;
1140 R[
CHZ][
CHX] = f2q1q3 + f2q0q2;
1141 R[
CHZ][
CHY] = f2q2q3 - f2q0q1;
1142 R[
CHZ][
CHZ] = f2q0q0 + f2q3q3 - 1.0F;
1156 if ((pq->
q0 >= 1.0F) || (pq->
q0 <= -1.0F))
1165 fetarad = 2.0F * acosf(pq->
q0);
1170 if (fetadeg >= 180.0F)
1177 sinhalfeta = (float)sinf(0.5F * fetarad);
1180 if (sinhalfeta == 0.0F)
1183 rvecdeg[
CHX] = rvecdeg[
CHY] = rvecdeg[
CHZ] = 0.0F;
1188 ftmp = fetadeg / sinhalfeta;
1189 rvecdeg[
CHX] = pq->
q1 * ftmp;
1190 rvecdeg[
CHY] = pq->
q2 * ftmp;
1191 rvecdeg[
CHZ] = pq->
q3 * ftmp;
1207 if (fdeltaq.
q0 < 0.0F)
1209 fdeltaq.
q0 = -fdeltaq.
q0;
1210 fdeltaq.
q1 = -fdeltaq.
q1;
1211 fdeltaq.
q2 = -fdeltaq.
q2;
1212 fdeltaq.
q3 = -fdeltaq.
q3;
1217 ftmp = flpf + (1.0F - flpf) * (1.0F - fdeltaq.
q0);
1225 ftmp = fdeltaq.
q1 * fdeltaq.
q1 + fdeltaq.
q2 * fdeltaq.
q2 + fdeltaq.
q3 * fdeltaq.
q3;
1229 fdeltaq.
q0 = sqrtf(1.0F - ftmp);
1239 ftmp = 1.0F / fdeltat;
1240 fOmega[
CHX] = rvecdeg[
CHX] * ftmp;
1241 fOmega[
CHY] = rvecdeg[
CHY] * ftmp;
1242 fOmega[
CHZ] = rvecdeg[
CHZ] * ftmp;
1257 pqA->
q0 = pqB->
q0 * pqC->
q0 - pqB->
q1 * pqC->
q1 - pqB->
q2 * pqC->
q2 - pqB->
q3 * pqC->
q3;
1258 pqA->
q1 = pqB->
q0 * pqC->
q1 + pqB->
q1 * pqC->
q0 + pqB->
q2 * pqC->
q3 - pqB->
q3 * pqC->
q2;
1259 pqA->
q2 = pqB->
q0 * pqC->
q2 - pqB->
q1 * pqC->
q3 + pqB->
q2 * pqC->
q0 + pqB->
q3 * pqC->
q1;
1260 pqA->
q3 = pqB->
q0 * pqC->
q3 + pqB->
q1 * pqC->
q2 - pqB->
q2 * pqC->
q1 + pqB->
q3 * pqC->
q0;
1271 qProd.
q0 = pqA->
q0 * pqB->
q0 - pqA->
q1 * pqB->
q1 - pqA->
q2 * pqB->
q2 - pqA->
q3 * pqB->
q3;
1272 qProd.
q1 = pqA->
q0 * pqB->
q1 + pqA->
q1 * pqB->
q0 + pqA->
q2 * pqB->
q3 - pqA->
q3 * pqB->
q2;
1273 qProd.
q2 = pqA->
q0 * pqB->
q2 - pqA->
q1 * pqB->
q3 + pqA->
q2 * pqB->
q0 + pqA->
q3 * pqB->
q1;
1274 qProd.
q3 = pqA->
q0 * pqB->
q3 + pqA->
q1 * pqB->
q2 - pqA->
q2 * pqB->
q1 + pqA->
q3 * pqB->
q0;
1287 qProd.
q0 = pqA->
q0 * pqB->q0 + pqA->
q1 * pqB->q1 + pqA->
q2 * pqB->q2 + pqA->
q3 * pqB->q3;
1288 qProd.
q1 = pqA->
q0 * pqB->q1 - pqA->
q1 * pqB->q0 - pqA->
q2 * pqB->q3 + pqA->
q3 * pqB->q2;
1289 qProd.
q2 = pqA->
q0 * pqB->q2 + pqA->
q1 * pqB->q3 - pqA->
q2 * pqB->q0 - pqA->
q3 * pqB->q1;
1290 qProd.
q3 = pqA->
q0 * pqB->q3 - pqA->
q1 * pqB->q2 + pqA->
q2 * pqB->q1 - pqA->
q3 * pqB->q0;
1301 fNorm = sqrtf(pqA->
q0 * pqA->
q0 + pqA->
q1 * pqA->
q1 + pqA->
q2 * pqA->
q2 + pqA->
q3 * pqA->
q3);
1305 fNorm = 1.0F / fNorm;
1315 pqA->
q1 = pqA->
q2 = pqA->
q3 = 0.0F;
1334 pqA->
q1 = pqA->
q2 = pqA->
q3 = 0.0F;
1344 float fsqrt1plusudotv;
1348 fsqrt1plusudotv = sqrtf(fabsf(1.0F + fu[
CHX] * fv[
CHX] + fu[
CHY] * fv[
CHY] + fu[
CHZ] * fv[
CHZ]));
1357 if (fsqrt1plusudotv != 0.0F)
1361 pfq->
q1 = -fuxv[
CHX] * ftmp;
1362 pfq->
q2 = -fuxv[
CHY] * ftmp;
1363 pfq->
q3 = -fuxv[
CHZ] * ftmp;
1373 ftmp = sqrtf(fabsf(pfq->
q1 * pfq->
q1 + pfq->
q2 * pfq->
q2 + pfq->
q3 * pfq->
q3));
void f3DOFMagnetometerMatrixNED(float fR[][3], float fBc[])
void fLeastSquareseCompassWin8(struct fquaternion *pfq, float fB, float fDelta, float fsinDelta, float fcosDelta, float *pfDelta6DOF, float fBc[], float fGs[], float *pfQvBQd, float *pfQvGQa)
void fAndroidAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg)
void fveqconjgquq(struct fquaternion *pfq, float fu[], float fv[])
void fNEDAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg)
void fqAeq1(struct fquaternion *pqA)
void feCompassNED(float fR[][3], float *pfDelta, float fBc[], float fGs[])
void f3DOFTiltNED(float fR[][3], float fGs[])
void f3DOFMagnetometerMatrixWin8(float fR[][3], float fBc[])
void fRotationVectorDegFromQuaternion(struct fquaternion *pq, float rvecdeg[])
void fLPFOrientationQuaternion(struct fquaternion *pq, struct fquaternion *pLPq, float flpf, float fdeltat, float fOmega[])
void fLeastSquareseCompassAndroid(struct fquaternion *pfq, float fB, float fDelta, float fsinDelta, float fcosDelta, float *pfDelta6DOF, float fBc[], float fGs[], float *pfQvBQd, float *pfQvGQa)
void qAeqBxC(struct fquaternion *pqA, const struct fquaternion *pqB, const struct fquaternion *pqC)
void feCompassWin8(float fR[][3], float *pfDelta, float fBc[], float fGs[])
void f3DOFTiltAndroid(float fR[][3], float fGs[])
void fqAeqNormqA(struct fquaternion *pqA)
void fQuaternionFromRotationVectorDeg(struct fquaternion *pq, const float rvecdeg[], float fscaling)
void qAeqAxB(struct fquaternion *pqA, const struct fquaternion *pqB)
void f3DOFTiltWin8(float fR[][3], float fGs[])
void f3x3matrixAeqI(float A[][3])
struct fquaternion qconjgAxB(const struct fquaternion *pqA, const struct fquaternion *pqB)
void eigencompute4(float A[][4], float eigval[], float eigvec[][4], int8 n)
void f3x3matrixAeqScalar(float A[][3], float Scalar)
void feCompassAndroid(float fR[][3], float *pfDelta, float fBc[], float fGs[])
void fRotationMatrixFromQuaternion(float R[][3], const struct fquaternion *pq)
void fQuaternionFromRotationMatrix(float R[][3], struct fquaternion *pq)
void fLeastSquareseCompassNED(struct fquaternion *pfq, float fB, float fDelta, float fsinDelta, float fcosDelta, float *pfDelta6DOF, float fBc[], float fGs[], float *pfQvBQd, float *pfQvGQa)
float fatan2_deg(float y, float x)
void f3DOFMagnetometerMatrixAndroid(float fR[][3], float fBc[])
void fWin8AnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg)