40 #define SMALLQ0 0.01F // limit of quaternion scalar component requiring special algorithm
41 #define CORRUPTQUAT 0.001F // threshold for deciding rotation quaternion is corrupt
42 #define SMALLMODULUS 0.01F // limit where rounding errors may appear
44 #define ONEOVER48 0.02083333333F // 1 / 48
45 #define ONEOVER3840 0.0002604166667F // 1 / 3840
60 fmodGyz = fGp[
Y] * fGp[
Y] + fGp[
Z] * fGp[
Z];
61 fmodGxyz = fmodGyz + fGp[
X] * fGp[
X];
89 fmodGyz = sqrtf(fmodGyz);
90 fmodGxyz = sqrtf(fmodGxyz);
91 frecipmodGxyz = 1.0F / fmodGxyz;
92 ftmp = fmodGxyz / fmodGyz;
95 for (i =
X; i <=
Z; i++)
97 fR[i][
Z] = fGp[i] * frecipmodGxyz;
101 fR[
X][
X] = fmodGyz * frecipmodGxyz;
102 fR[
Y][
X] = -fR[
X][
Z] * fR[
Y][
Z] * ftmp;
103 fR[
Z][
X] = -fR[
X][
Z] * fR[
Z][
Z] * ftmp;
107 fR[
Y][
Y] = fR[
Z][
Z] * ftmp;
108 fR[
Z][
Y] = -fR[
Y][
Z] * ftmp;
135 fmodGxz = fGp[
X] * fGp[
X] + fGp[
Z] * fGp[
Z];
136 fmodGxyz = fmodGxz + fGp[
Y] * fGp[
Y];
139 if (fmodGxyz == 0.0F)
164 fmodGxz = sqrtf(fmodGxz);
165 fmodGxyz = sqrtf(fmodGxyz);
166 frecipmodGxyz = 1.0F / fmodGxyz;
167 ftmp = fmodGxyz / fmodGxz;
174 for (i =
X; i <=
Z; i++)
176 fR[i][
Z] = -fGp[i] * frecipmodGxyz;
180 fR[
X][
X] = -fR[
Z][
Z] * ftmp;
182 fR[
Z][
X] = fR[
X][
Z] * ftmp;;
185 fR[
X][
Y] = fR[
X][
Z] * fR[
Y][
Z] * ftmp;
186 fR[
Y][
Y] = -fmodGxz * frecipmodGxyz;
189 fR[
Y][
Y] = -fR[
Y][
Y];
191 fR[
Z][
Y] = fR[
Y][
Z] * fR[
Z][
Z] * ftmp;
203 fmodBxy = sqrtf(fBc[
X] * fBc[
X] + fBc[
Y] * fBc[
Y]);
213 fR[
Z][
X] = fR[
Z][
Y] = fR[
X][
Z] = fR[
Y][
Z] = 0.0F;
217 fR[
X][
X] = fR[
Y][
Y] = fBc[
X] / fmodBxy;
218 fR[
Y][
X] = fBc[
Y] / fmodBxy;
219 fR[
X][
Y] = -fR[
Y][
X];
231 fmodBxy = sqrtf(fBc[
X] * fBc[
X] + fBc[
Y] * fBc[
Y]);
241 fR[
Z][
X] = fR[
Z][
Y] = fR[
X][
Z] = fR[
Y][
Z] = 0.0F;
245 fR[
X][
X] = fR[
Y][
Y] = fBc[
Y] / fmodBxy;
246 fR[
X][
Y] = fBc[
X] / fmodBxy;
247 fR[
Y][
X] = -fR[
X][
Y];
262 void feCompassNED(
float fR[][3],
float *pfDelta,
float fBc[],
float fGp[])
275 for (i =
X; i <=
Z; i++)
282 fR[
X][
Y] = fR[
Y][
Z] * fR[
Z][
X] - fR[
Z][
Z] * fR[
Y][
X];
283 fR[
Y][
Y] = fR[
Z][
Z] * fR[
X][
X] - fR[
X][
Z] * fR[
Z][
X];
284 fR[
Z][
Y] = fR[
X][
Z] * fR[
Y][
X] - fR[
Y][
Z] * fR[
X][
X];
287 fR[
X][
X] = fR[
Y][
Y] * fR[
Z][
Z] - fR[
Z][
Y] * fR[
Y][
Z];
288 fR[
Y][
X] = fR[
Z][
Y] * fR[
X][
Z] - fR[
X][
Y] * fR[
Z][
Z];
289 fR[
Z][
X] = fR[
X][
Y] * fR[
Y][
Z] - fR[
Y][
Y] * fR[
X][
Z];
292 fmod[
X] = sqrtf(fR[
X][
X] * fR[
X][
X] + fR[
Y][
X] * fR[
Y][
X] + fR[
Z][
X] * fR[
Z][
X]);
293 fmod[
Y] = sqrtf(fR[X][
Y] * fR[X][
Y] + fR[
Y][
Y] * fR[
Y][
Y] + fR[
Z][
Y] * fR[
Z][
Y]);
294 fmod[
Z] = sqrtf(fR[X][
Z] * fR[X][
Z] + fR[Y][
Z] * fR[Y][
Z] + fR[
Z][
Z] * fR[
Z][
Z]);
297 if (!((fmod[X] == 0.0F) || (fmod[Y] == 0.0F) || (fmod[Z] == 0.0F)))
300 for (j = X; j <=
Z; j++)
302 ftmp = 1.0F / fmod[j];
304 for (i = X; i <=
Z; i++)
319 fmodBc = sqrtf(fBc[X] * fBc[X] + fBc[Y] * fBc[Y] + fBc[Z] * fBc[Z]);
320 fGdotBc = fGp[
X] * fBc[
X] + fGp[
Y] * fBc[
Y] + fGp[
Z] * fBc[
Z];
321 if (!((fmod[Z] == 0.0F) || (fmodBc == 0.0F)))
323 *pfDelta =
fasin_deg(fGdotBc / (fmod[Z] * fmodBc));
343 for (i =
X; i <=
Z; i++)
350 fR[
X][
X] = fR[
Y][
Y] * fR[
Z][
Z] - fR[
Z][
Y] * fR[
Y][
Z];
351 fR[
Y][
X] = fR[
Z][
Y] * fR[
X][
Z] - fR[
X][
Y] * fR[
Z][
Z];
352 fR[
Z][
X] = fR[
X][
Y] * fR[
Y][
Z] - fR[
Y][
Y] * fR[
X][
Z];
355 fR[
X][
Y] = fR[
Y][
Z] * fR[
Z][
X] - fR[
Z][
Z] * fR[
Y][
X];
356 fR[
Y][
Y] = fR[
Z][
Z] * fR[
X][
X] - fR[
X][
Z] * fR[
Z][
X];
357 fR[
Z][
Y] = fR[
X][
Z] * fR[
Y][
X] - fR[
Y][
Z] * fR[
X][
X];
360 fmod[
X] = sqrtf(fR[
X][
X] * fR[
X][
X] + fR[
Y][
X] * fR[
Y][
X] + fR[
Z][
X] * fR[
Z][
X]);
361 fmod[
Y] = sqrtf(fR[X][
Y] * fR[X][
Y] + fR[
Y][
Y] * fR[
Y][
Y] + fR[
Z][
Y] * fR[
Z][
Y]);
362 fmod[
Z] = sqrtf(fR[X][
Z] * fR[X][
Z] + fR[Y][
Z] * fR[Y][
Z] + fR[
Z][
Z] * fR[
Z][
Z]);
365 if (!((fmod[X] == 0.0F) || (fmod[Y] == 0.0F) || (fmod[Z] == 0.0F)))
368 for (j = X; j <=
Z; j++)
370 ftmp = 1.0F / fmod[j];
372 for (i = X; i <=
Z; i++)
387 fmodBc = sqrtf(fBc[X] * fBc[X] + fBc[Y] * fBc[Y] + fBc[Z] * fBc[Z]);
388 fGdotBc = fGp[
X] * fBc[
X] + fGp[
Y] * fBc[
Y] + fGp[
Z] * fBc[
Z];
389 if (!((fmod[Z] == 0.0F) || (fmodBc == 0.0F)))
391 *pfDelta =
fasin_deg(-fGdotBc / (fmod[Z] * fmodBc));
411 for (i =
X; i <=
Z; i++)
418 fR[
X][
X] = fR[
Y][
Y] * fR[
Z][
Z] - fR[
Z][
Y] * fR[
Y][
Z];
419 fR[
Y][
X] = fR[
Z][
Y] * fR[
X][
Z] - fR[
X][
Y] * fR[
Z][
Z];
420 fR[
Z][
X] = fR[
X][
Y] * fR[
Y][
Z] - fR[
Y][
Y] * fR[
X][
Z];
423 fR[
X][
Y] = fR[
Y][
Z] * fR[
Z][
X] - fR[
Z][
Z] * fR[
Y][
X];
424 fR[
Y][
Y] = fR[
Z][
Z] * fR[
X][
X] - fR[
X][
Z] * fR[
Z][
X];
425 fR[
Z][
Y] = fR[
X][
Z] * fR[
Y][
X] - fR[
Y][
Z] * fR[
X][
X];
428 fmod[
X] = sqrtf(fR[
X][
X] * fR[
X][
X] + fR[
Y][
X] * fR[
Y][
X] + fR[
Z][
X] * fR[
Z][
X]);
429 fmod[
Y] = sqrtf(fR[X][
Y] * fR[X][
Y] + fR[
Y][
Y] * fR[
Y][
Y] + fR[
Z][
Y] * fR[
Z][
Y]);
430 fmod[
Z] = sqrtf(fR[X][
Z] * fR[X][
Z] + fR[Y][
Z] * fR[Y][
Z] + fR[
Z][
Z] * fR[
Z][
Z]);
433 if (!((fmod[X] == 0.0F) || (fmod[Y] == 0.0F) || (fmod[Z] == 0.0F)))
436 for (j = X; j <=
Z; j++)
438 ftmp = 1.0F / fmod[j];
440 for (i = X; i <=
Z; i++)
455 fmodBc = sqrtf(fBc[X] * fBc[X] + fBc[Y] * fBc[Y] + fBc[Z] * fBc[Z]);
456 fGdotBc = fGp[
X] * fBc[
X] + fGp[
Y] * fBc[
Y] + fGp[
Z] * fBc[
Z];
457 if (!((fmod[Z] == 0.0F) || (fmodBc == 0.0F)))
459 *pfDelta =
fasin_deg(fGdotBc / (fmod[Z] * fmodBc));
467 float *pfRhoDeg,
float *pfChiDeg)
476 if (*pfPhiDeg == 180.0F)
482 if (*pfTheDeg == 90.0F)
485 *pfPsiDeg =
fatan2_deg(R[Z][
Y], R[Y][Y]) + *pfPhiDeg;
487 else if (*pfTheDeg == -90.0F)
490 *pfPsiDeg =
fatan2_deg(-R[Z][
Y], R[Y][Y]) - *pfPhiDeg;
499 if (*pfPsiDeg < 0.0F)
505 if (*pfPsiDeg >= 360.0F)
511 *pfRhoDeg = *pfPsiDeg;
521 float *pfRhoDeg,
float *pfChiDeg)
530 if (*pfTheDeg == 180.0F)
536 if (*pfPhiDeg == 90.0F)
541 else if (*pfPhiDeg == -90.0F)
553 if (*pfPsiDeg < 0.0F)
559 if (*pfPsiDeg >= 360.0F)
566 *pfRhoDeg = *pfPsiDeg;
576 float *pfRhoDeg,
float *pfChiDeg)
606 *pfTheDeg = 180.0F - *pfTheDeg;
610 if (*pfTheDeg >= 180.0F)
616 if (*pfTheDeg == 90.0F)
621 else if (*pfTheDeg == -90.0F)
632 if (fabs(*pfTheDeg) >= 90.0F)
639 if (*pfPsiDeg < 0.0F)
645 if (*pfPsiDeg >= 360.0F)
651 *pfRhoDeg = 360.0F - *pfPsiDeg;
654 if (*pfRhoDeg >= 360.0F)
677 fetadeg = fscaling * sqrtf(rvecdeg[
X] * rvecdeg[
X] + rvecdeg[
Y] * rvecdeg[
Y] + rvecdeg[
Z] * rvecdeg[
Z]);
679 fetarad2 = fetarad * fetarad;
683 if (fetarad2 <= 0.02F)
686 sinhalfeta = fetarad * (0.5F -
ONEOVER48 * fetarad2);
688 else if (fetarad2 <= 0.06F)
692 fetarad4 = fetarad2 * fetarad2;
698 sinhalfeta = (float)sinf(0.5F * fetarad);
705 ftmp = fscaling * sinhalfeta / fetadeg;
706 pq->
q1 = rvecdeg[
X] * ftmp;
707 pq->
q2 = rvecdeg[
Y] * ftmp;
708 pq->
q3 = rvecdeg[
Z] * ftmp;
713 pq->
q1 = pq->
q2 = pq->
q3 = 0.0F;
718 fvecsq = pq->
q1 * pq->
q1 + pq->
q2 * pq->
q2 + pq->
q3 * pq->
q3;
722 pq->
q0 = sqrtf(1.0F - fvecsq);
744 fq0sq = 0.25F * (1.0F + R[
X][
X] + R[
Y][
Y] + R[
Z][
Z]);
745 pq->
q0 = sqrtf(fabs(fq0sq));
751 recip4q0 = 0.25F / pq->
q0;
752 pq->
q1 = recip4q0 * (R[
Y][
Z] - R[
Z][
Y]);
753 pq->
q2 = recip4q0 * (R[
Z][
X] - R[
X][
Z]);
754 pq->
q3 = recip4q0 * (R[
X][
Y] - R[
Y][
X]);
761 pq->
q1 = sqrtf(fabs(0.5F * (1.0F + R[
X][
X]) - fq0sq));
762 pq->
q2 = sqrtf(fabs(0.5F * (1.0F + R[
Y][
Y]) - fq0sq));
763 pq->
q3 = sqrtf(fabs(0.5F * (1.0F + R[
Z][
Z]) - fq0sq));
766 if ((R[Y][Z] - R[Z][Y]) < 0.0F) pq->
q1 = -pq->
q1;
767 if ((R[Z][X] - R[X][Z]) < 0.0F) pq->
q2 = -pq->
q2;
768 if ((R[X][Y] - R[Y][X]) < 0.0F) pq->
q3 = -pq->
q3;
778 float f2q0q0, f2q0q1, f2q0q2, f2q0q3;
779 float f2q1q1, f2q1q2, f2q1q3;
780 float f2q2q2, f2q2q3;
785 f2q0q0 = f2q * pq->
q0;
786 f2q0q1 = f2q * pq->
q1;
787 f2q0q2 = f2q * pq->
q2;
788 f2q0q3 = f2q * pq->
q3;
790 f2q1q1 = f2q * pq->
q1;
791 f2q1q2 = f2q * pq->
q2;
792 f2q1q3 = f2q * pq->
q3;
794 f2q2q2 = f2q * pq->
q2;
795 f2q2q3 = f2q * pq->
q3;
796 f2q3q3 = 2.0F * pq->
q3 * pq->
q3;
799 R[
X][
X] = f2q0q0 + f2q1q1 - 1.0F;
800 R[
X][
Y] = f2q1q2 + f2q0q3;
801 R[
X][
Z] = f2q1q3 - f2q0q2;
802 R[
Y][
X] = f2q1q2 - f2q0q3;
803 R[
Y][
Y] = f2q0q0 + f2q2q2 - 1.0F;
804 R[
Y][
Z] = f2q2q3 + f2q0q1;
805 R[
Z][
X] = f2q1q3 + f2q0q2;
806 R[
Z][
Y] = f2q2q3 - f2q0q1;
807 R[
Z][
Z] = f2q0q0 + f2q3q3 - 1.0F;
823 ftrace = R[
X][
X] + R[
Y][
Y] + R[
Z][
Z];
828 else if (ftrace <= -1.0F)
834 fetadeg = acosf(0.5F * (ftrace - 1.0F)) *
FRADTODEG;
840 rvecdeg[
X] = R[
Y][
Z] - R[
Z][
Y];
841 rvecdeg[
Y] = R[
Z][
X] - R[
X][
Z];
842 rvecdeg[
Z] = R[
X][
Y] - R[
Y][
X];
843 fmodulus = sqrtf(rvecdeg[
X] * rvecdeg[
X] + rvecdeg[
Y] * rvecdeg[
Y] + rvecdeg[
Z] * rvecdeg[
Z]);
849 ftmp = fetadeg / fmodulus;
854 else if (ftrace >= 0.0F)
867 rvecdeg[
X] = 180.0F * sqrtf(fabs(0.5F * (R[
X][
X] + 1.0F)));
868 rvecdeg[
Y] = 180.0F * sqrtf(fabs(0.5F * (R[
Y][
Y] + 1.0F)));
869 rvecdeg[
Z] = 180.0F * sqrtf(fabs(0.5F * (R[Z][Z] + 1.0F)));
872 if ((R[
Y][Z] - R[Z][
Y]) < 0.0F) rvecdeg[
X] = -rvecdeg[
X];
873 if ((R[Z][
X] - R[
X][Z]) < 0.0F) rvecdeg[
Y] = -rvecdeg[
Y];
874 if ((R[
X][Y] - R[Y][
X]) < 0.0F) rvecdeg[
Z] = -rvecdeg[
Z];
890 if ((pq->
q0 >= 1.0F) || (pq->
q0 <= -1.0F))
899 fetarad = 2.0F * acosf(pq->
q0);
904 if (fetadeg >= 180.0F)
911 sinhalfeta = (float)sinf(0.5F * fetarad);
914 if (sinhalfeta == 0.0F)
917 rvecdeg[
X] = rvecdeg[
Y] = rvecdeg[
Z] = 0.0F;
922 ftmp = fetadeg / sinhalfeta;
923 rvecdeg[
X] = pq->
q1 * ftmp;
924 rvecdeg[
Y] = pq->
q2 * ftmp;
925 rvecdeg[
Z] = pq->
q3 * ftmp;
933 float fOmega[],
int32 loopcounter)
941 if (loopcounter == 0)
948 if (fdeltaq.
q0 < 0.0F)
950 fdeltaq.
q0 = -fdeltaq.
q0;
951 fdeltaq.
q1 = -fdeltaq.
q1;
952 fdeltaq.
q2 = -fdeltaq.
q2;
953 fdeltaq.
q3 = -fdeltaq.
q3;
958 ftmp = flpf + 0.75F * (1.0F - fdeltaq.
q0);
970 ftmp = fdeltaq.
q1 * fdeltaq.
q1 + fdeltaq.
q2 * fdeltaq.
q2 + fdeltaq.
q3 * fdeltaq.
q3;
974 fdeltaq.
q0 = sqrtf(1.0F - ftmp);
984 ftmp = 1.0F / fdeltat;
985 fOmega[
X] = rvecdeg[
X] * ftmp;
986 fOmega[
Y] = rvecdeg[
Y] * ftmp;
987 fOmega[
Z] = rvecdeg[
Z] * ftmp;
1003 if (loopcounter == 0)
1009 *pfLPS += flpf * (*pfS - *pfLPS);
1017 pqA->
q0 = pqB->
q0 * pqC->
q0 - pqB->
q1 * pqC->
q1 - pqB->
q2 * pqC->
q2 - pqB->
q3 * pqC->
q3;
1018 pqA->
q1 = pqB->
q0 * pqC->
q1 + pqB->
q1 * pqC->
q0 + pqB->
q2 * pqC->
q3 - pqB->
q3 * pqC->
q2;
1019 pqA->
q2 = pqB->
q0 * pqC->
q2 - pqB->
q1 * pqC->
q3 + pqB->
q2 * pqC->
q0 + pqB->
q3 * pqC->
q1;
1020 pqA->
q3 = pqB->
q0 * pqC->
q3 + pqB->
q1 * pqC->
q2 - pqB->
q2 * pqC->
q1 + pqB->
q3 * pqC->
q0;
1031 qProd.
q0 = pqA->
q0 * pqB->
q0 - pqA->
q1 * pqB->
q1 - pqA->
q2 * pqB->
q2 - pqA->
q3 * pqB->
q3;
1032 qProd.
q1 = pqA->
q0 * pqB->
q1 + pqA->
q1 * pqB->
q0 + pqA->
q2 * pqB->
q3 - pqA->
q3 * pqB->
q2;
1033 qProd.
q2 = pqA->
q0 * pqB->
q2 - pqA->
q1 * pqB->
q3 + pqA->
q2 * pqB->
q0 + pqA->
q3 * pqB->
q1;
1034 qProd.
q3 = pqA->
q0 * pqB->
q3 + pqA->
q1 * pqB->
q2 - pqA->
q2 * pqB->
q1 + pqA->
q3 * pqB->
q0;
1047 qProd.
q0 = pqA->
q0 * pqB->q0 + pqA->
q1 * pqB->q1 + pqA->
q2 * pqB->q2 + pqA->
q3 * pqB->q3;
1048 qProd.
q1 = pqA->
q0 * pqB->q1 - pqA->
q1 * pqB->q0 - pqA->
q2 * pqB->q3 + pqA->
q3 * pqB->q2;
1049 qProd.
q2 = pqA->
q0 * pqB->q2 + pqA->
q1 * pqB->q3 - pqA->
q2 * pqB->q0 - pqA->
q3 * pqB->q1;
1050 qProd.
q3 = pqA->
q0 * pqB->q3 - pqA->
q1 * pqB->q2 + pqA->
q2 * pqB->q1 - pqA->
q3 * pqB->q0;
1061 fNorm = sqrtf(pqA->
q0 * pqA->
q0 + pqA->
q1 * pqA->
q1 + pqA->
q2 * pqA->
q2 + pqA->
q3 * pqA->
q3);
1065 fNorm = 1.0F / fNorm;
1075 pqA->
q1 = pqA->
q2 = pqA->
q3 = 0.0F;
1094 pqA->
q1 = pqA->
q2 = pqA->
q3 = 0.0F;
void f3DOFMagnetometerMatrixNED(float fR[][3], float fBc[])
void f3DOFTiltNED(float fR[][3], float fGp[])
void fAndroidAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg)
void fNEDAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg)
void fqAeq1(struct fquaternion *pqA)
void f3DOFMagnetometerMatrixWin8(float fR[][3], float fBc[])
void fRotationVectorDegFromQuaternion(struct fquaternion *pq, float rvecdeg[])
void fRotationVectorDegFromRotationMatrix(float R[][3], float rvecdeg[])
void feCompassNED(float fR[][3], float *pfDelta, float fBc[], float fGp[])
void qAeqBxC(struct fquaternion *pqA, const struct fquaternion *pqB, const struct fquaternion *pqC)
void feCompassAndroid(float fR[][3], float *pfDelta, float fBc[], float fGp[])
void fqAeqNormqA(struct fquaternion *pqA)
void f3DOFTiltAndroid(float fR[][3], float fGp[])
void fQuaternionFromRotationVectorDeg(struct fquaternion *pq, const float rvecdeg[], float fscaling)
void qAeqAxB(struct fquaternion *pqA, const struct fquaternion *pqB)
long int32
This defines int32 as long.
short int16
This defines int16 as short.
void f3x3matrixAeqI(float A[][3])
struct fquaternion qconjgAxB(const struct fquaternion *pqA, const struct fquaternion *pqB)
void fLPFOrientationQuaternion(struct fquaternion *pq, struct fquaternion *pLPq, float flpf, float fdeltat, float fOmega[], int32 loopcounter)
void f3x3matrixAeqScalar(float A[][3], float Scalar)
void fRotationMatrixFromQuaternion(float R[][3], const struct fquaternion *pq)
void fQuaternionFromRotationMatrix(float R[][3], struct fquaternion *pq)
void fLPFScalar(float *pfS, float *pfLPS, float flpf, int32 loopcounter)
void f3DOFTiltWin8(float fR[][3], float fGp[])
float fatan2_deg(float y, float x)
void f3DOFMagnetometerMatrixAndroid(float fR[][3], float fBc[])
void feCompassWin8(float fR[][3], float *pfDelta, float fBc[], float fGp[])
void fWin8AnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg)