1 /*
2 * Copyright (c) 2015, Freescale Semiconductor, Inc.
3 * Copyright 2016-2017 NXP
4 * All rights reserved.
5 *
6 * SPDX-License-Identifier: BSD-3-Clause
7 */
8
9 // This file contains functions designed to operate on, or compute, orientations.
10 // These may be in rotation matrix form, quaternion form, or Euler angles.
11 // It also includes functions designed to operate with specify reference frames
12 // (Android, Windows 8, NED).
13 //
14
15 /*! \file orientation.c
16 \brief Functions to convert between various orientation representations
17
18 Functions to convert between various orientation representations. Also
19 includes functions for manipulating quaternions.
20 */
21
22 #include "stdio.h"
23 #include "math.h"
24 #include "stdlib.h"
25 #include "time.h"
26 #include "string.h"
27
28 #include "sensor_fusion.h"
29 #include "orientation.h"
30 #include "fusion.h"
31 #include "matrix.h"
32 #include "approximations.h"
33
34 // compile time constants that are private to this file
35 #define SMALLQ0 1E-4F // limit of quaternion scalar component requiring special algorithm
36 #define CORRUPTQUAT 0.001F // threshold for deciding rotation quaternion is corrupt
37 #define SMALLMODULUS 0.01F // limit where rounding errors may appear
38
39 #if F_USING_ACCEL // Need tilt conversion routines
40 // Aerospace NED accelerometer 3DOF tilt function computing rotation matrix fR
41 #if (THISCOORDSYSTEM == NED) || (THISCOORDSYSTEM == ANDROID)
f3DOFTiltNED(float fR[][3],float fGc[])42 void f3DOFTiltNED(float fR[][3], float fGc[])
43 {
44 // the NED self-consistency twist occurs at 90 deg pitch
45
46 // local variables
47 int16 i; // counter
48 float fmodGxyz; // modulus of the x, y, z accelerometer readings
49 float fmodGyz; // modulus of the y, z accelerometer readings
50 float frecipmodGxyz; // reciprocal of modulus
51 float ftmp; // scratch variable
52
53 // compute the accelerometer squared magnitudes
54 fmodGyz = fGc[CHY] * fGc[CHY] + fGc[CHZ] * fGc[CHZ];
55 fmodGxyz = fmodGyz + fGc[CHX] * fGc[CHX];
56
57 // check for freefall special case where no solution is possible
58 if (fmodGxyz == 0.0F)
59 {
60 f3x3matrixAeqI(fR);
61 return;
62 }
63
64 // check for vertical up or down gimbal lock case
65 if (fmodGyz == 0.0F)
66 {
67 f3x3matrixAeqScalar(fR, 0.0F);
68 fR[CHY][CHY] = 1.0F;
69 if (fGc[CHX] >= 0.0F)
70 {
71 fR[CHX][CHZ] = 1.0F;
72 fR[CHZ][CHX] = -1.0F;
73 }
74 else
75 {
76 fR[CHX][CHZ] = -1.0F;
77 fR[CHZ][CHX] = 1.0F;
78 }
79 return;
80 }
81
82 // compute moduli for the general case
83 fmodGyz = sqrtf(fmodGyz);
84 fmodGxyz = sqrtf(fmodGxyz);
85 frecipmodGxyz = 1.0F / fmodGxyz;
86 ftmp = fmodGxyz / fmodGyz;
87
88 // normalize the accelerometer reading into the z column
89 for (i = CHX; i <= CHZ; i++)
90 {
91 fR[i][CHZ] = fGc[i] * frecipmodGxyz;
92 }
93
94 // construct x column of orientation matrix
95 fR[CHX][CHX] = fmodGyz * frecipmodGxyz;
96 fR[CHY][CHX] = -fR[CHX][CHZ] * fR[CHY][CHZ] * ftmp;
97 fR[CHZ][CHX] = -fR[CHX][CHZ] * fR[CHZ][CHZ] * ftmp;
98
99 // construct y column of orientation matrix
100 fR[CHX][CHY] = 0.0F;
101 fR[CHY][CHY] = fR[CHZ][CHZ] * ftmp;
102 fR[CHZ][CHY] = -fR[CHY][CHZ] * ftmp;
103
104 return;
105 }
106 #endif // #if THISCOORDSYSTEM == NED
107
108 // Android accelerometer 3DOF tilt function computing rotation matrix fR
109 #if THISCOORDSYSTEM == ANDROID
f3DOFTiltAndroid(float fR[][3],float fGc[])110 void f3DOFTiltAndroid(float fR[][3], float fGc[])
111 {
112 // the Android tilt matrix is mathematically identical to the NED tilt matrix
113 // the Android self-consistency twist occurs at 90 deg roll
114 f3DOFTiltNED(fR, fGc);
115 return;
116 }
117 #endif // #if THISCOORDSYSTEM == ANDROID
118
119 // Windows 8 accelerometer 3DOF tilt function computing rotation matrix fR
120 #if (THISCOORDSYSTEM == WIN8) || (THISCOORDSYSTEM == ANDROID)
f3DOFTiltWin8(float fR[][3],float fGc[])121 void f3DOFTiltWin8(float fR[][3], float fGc[])
122 {
123 // the Win8 self-consistency twist occurs at 90 deg roll
124
125 // local variables
126 float fmodGxyz; // modulus of the x, y, z accelerometer readings
127 float fmodGxz; // modulus of the x, z accelerometer readings
128 float frecipmodGxyz; // reciprocal of modulus
129 float ftmp; // scratch variable
130 int8 i; // counter
131
132 // compute the accelerometer squared magnitudes
133 fmodGxz = fGc[CHX] * fGc[CHX] + fGc[CHZ] * fGc[CHZ];
134 fmodGxyz = fmodGxz + fGc[CHY] * fGc[CHY];
135
136 // check for freefall special case where no solution is possible
137 if (fmodGxyz == 0.0F)
138 {
139 f3x3matrixAeqI(fR);
140 return;
141 }
142
143 // check for vertical up or down gimbal lock case
144 if (fmodGxz == 0.0F)
145 {
146 f3x3matrixAeqScalar(fR, 0.0F);
147 fR[CHX][CHX] = 1.0F;
148 if (fGc[CHY] >= 0.0F)
149 {
150 fR[CHY][CHZ] = -1.0F;
151 fR[CHZ][CHY] = 1.0F;
152 }
153 else
154 {
155 fR[CHY][CHZ] = 1.0F;
156 fR[CHZ][CHY] = -1.0F;
157 }
158 return;
159 }
160
161 // compute moduli for the general case
162 fmodGxz = sqrtf(fmodGxz);
163 fmodGxyz = sqrtf(fmodGxyz);
164 frecipmodGxyz = 1.0F / fmodGxyz;
165 ftmp = fmodGxyz / fmodGxz;
166 if (fGc[CHZ] < 0.0F)
167 {
168 ftmp = -ftmp;
169 }
170
171 // normalize the negated accelerometer reading into the z column
172 for (i = CHX; i <= CHZ; i++)
173 {
174 fR[i][CHZ] = -fGc[i] * frecipmodGxyz;
175 }
176
177 // construct x column of orientation matrix
178 fR[CHX][CHX] = -fR[CHZ][CHZ] * ftmp;
179 fR[CHY][CHX] = 0.0F;
180 fR[CHZ][CHX] = fR[CHX][CHZ] * ftmp;;
181
182 // // construct y column of orientation matrix
183 fR[CHX][CHY] = fR[CHX][CHZ] * fR[CHY][CHZ] * ftmp;
184 fR[CHY][CHY] = -fmodGxz * frecipmodGxyz;
185 if (fGc[CHZ] < 0.0F)
186 {
187 fR[CHY][CHY] = -fR[CHY][CHY];
188 }
189 fR[CHZ][CHY] = fR[CHY][CHZ] * fR[CHZ][CHZ] * ftmp;
190
191 return;
192 }
193 #endif // #if (THISCOORDSYSTEM == WIN8)
194 #endif // #if F_USING_ACCEL // Need tilt conversion routines
195
196 #if F_USING_MAG // Need eCompass conversion routines
197 // Aerospace NED magnetometer 3DOF flat eCompass function computing rotation matrix fR
198 #if THISCOORDSYSTEM == NED
f3DOFMagnetometerMatrixNED(float fR[][3],float fBc[])199 void f3DOFMagnetometerMatrixNED(float fR[][3], float fBc[])
200 {
201 // local variables
202 float fmodBxy; // modulus of the x, y magnetometer readings
203
204 // compute the magnitude of the horizontal (x and y) magnetometer reading
205 fmodBxy = sqrtf(fBc[CHX] * fBc[CHX] + fBc[CHY] * fBc[CHY]);
206
207 // check for zero field special case where no solution is possible
208 if (fmodBxy == 0.0F)
209 {
210 f3x3matrixAeqI(fR);
211 return;
212 }
213
214 // define the fixed entries in the z row and column
215 fR[CHZ][CHX] = fR[CHZ][CHY] = fR[CHX][CHZ] = fR[CHY][CHZ] = 0.0F;
216 fR[CHZ][CHZ] = 1.0F;
217
218 // define the remaining entries
219 fR[CHX][CHX] = fR[CHY][CHY] = fBc[CHX] / fmodBxy;
220 fR[CHY][CHX] = fBc[CHY] / fmodBxy;
221 fR[CHX][CHY] = -fR[CHY][CHX];
222
223 return;
224 }
225 #endif // #if THISCOORDSYSTEM == NED
226
227 // Android magnetometer 3DOF flat eCompass function computing rotation matrix fR
228 #if (THISCOORDSYSTEM == ANDROID) || (THISCOORDSYSTEM == WIN8)
f3DOFMagnetometerMatrixAndroid(float fR[][3],float fBc[])229 void f3DOFMagnetometerMatrixAndroid(float fR[][3], float fBc[])
230 {
231 // local variables
232 float fmodBxy; // modulus of the x, y magnetometer readings
233
234 // compute the magnitude of the horizontal (x and y) magnetometer reading
235 fmodBxy = sqrtf(fBc[CHX] * fBc[CHX] + fBc[CHY] * fBc[CHY]);
236
237 // check for zero field special case where no solution is possible
238 if (fmodBxy == 0.0F)
239 {
240 f3x3matrixAeqI(fR);
241 return;
242 }
243
244 // define the fixed entries in the z row and column
245 fR[CHZ][CHX] = fR[CHZ][CHY] = fR[CHX][CHZ] = fR[CHY][CHZ] = 0.0F;
246 fR[CHZ][CHZ] = 1.0F;
247
248 // define the remaining entries
249 fR[CHX][CHX] = fR[CHY][CHY] = fBc[CHY] / fmodBxy;
250 fR[CHX][CHY] = fBc[CHX] / fmodBxy;
251 fR[CHY][CHX] = -fR[CHX][CHY];
252
253 return;
254 }
255 #endif // #if THISCOORDSYSTEM == ANDROID
256
257 // Windows 8 magnetometer 3DOF flat eCompass function computing rotation matrix fR
258 #if (THISCOORDSYSTEM == WIN8)
f3DOFMagnetometerMatrixWin8(float fR[][3],float fBc[])259 void f3DOFMagnetometerMatrixWin8(float fR[][3], float fBc[])
260 {
261 // call the Android function since it is identical to the Windows 8 matrix
262 f3DOFMagnetometerMatrixAndroid(fR, fBc);
263
264 return;
265 }
266 #endif // #if (THISCOORDSYSTEM == WIN8)
267
268 // NED: basic 6DOF e-Compass function computing rotation matrix fR and magnetic inclination angle fDelta
269 #if THISCOORDSYSTEM == NED
feCompassNED(float fR[][3],float * pfDelta,float * pfsinDelta,float * pfcosDelta,float fBc[],float fGc[],float * pfmodBc,float * pfmodGc)270 void feCompassNED(float fR[][3], float *pfDelta, float *pfsinDelta, float *pfcosDelta, float fBc[], float fGc[], float *pfmodBc, float *pfmodGc)
271 {
272 // local variables
273 float fmod[3]; // column moduli
274 float fGcdotBc; // dot product of vectors G.Bc
275 float ftmp; // scratch variable
276 int8 i, j; // loop counters
277
278 // set the inclination angle to zero in case it is not computed later
279 *pfDelta = *pfsinDelta = 0.0F;
280 *pfcosDelta = 1.0F;
281
282 // place the un-normalized gravity and geomagnetic vectors into the rotation matrix z and x axes
283 for (i = CHX; i <= CHZ; i++)
284 {
285 fR[i][CHZ] = fGc[i];
286 fR[i][CHX] = fBc[i];
287 }
288
289 // set y vector to vector product of z and x vectors
290 fR[CHX][CHY] = fR[CHY][CHZ] * fR[CHZ][CHX] - fR[CHZ][CHZ] * fR[CHY][CHX];
291 fR[CHY][CHY] = fR[CHZ][CHZ] * fR[CHX][CHX] - fR[CHX][CHZ] * fR[CHZ][CHX];
292 fR[CHZ][CHY] = fR[CHX][CHZ] * fR[CHY][CHX] - fR[CHY][CHZ] * fR[CHX][CHX];
293
294 // set x vector to vector product of y and z vectors
295 fR[CHX][CHX] = fR[CHY][CHY] * fR[CHZ][CHZ] - fR[CHZ][CHY] * fR[CHY][CHZ];
296 fR[CHY][CHX] = fR[CHZ][CHY] * fR[CHX][CHZ] - fR[CHX][CHY] * fR[CHZ][CHZ];
297 fR[CHZ][CHX] = fR[CHX][CHY] * fR[CHY][CHZ] - fR[CHY][CHY] * fR[CHX][CHZ];
298
299 // calculate the rotation matrix column moduli
300 fmod[CHX] = sqrtf(fR[CHX][CHX] * fR[CHX][CHX] + fR[CHY][CHX] * fR[CHY][CHX] + fR[CHZ][CHX] * fR[CHZ][CHX]);
301 fmod[CHY] = sqrtf(fR[CHX][CHY] * fR[CHX][CHY] + fR[CHY][CHY] * fR[CHY][CHY] + fR[CHZ][CHY] * fR[CHZ][CHY]);
302 fmod[CHZ] = sqrtf(fR[CHX][CHZ] * fR[CHX][CHZ] + fR[CHY][CHZ] * fR[CHY][CHZ] + fR[CHZ][CHZ] * fR[CHZ][CHZ]);
303
304 // normalize the rotation matrix columns
305 if (!((fmod[CHX] == 0.0F) || (fmod[CHY] == 0.0F) || (fmod[CHZ] == 0.0F)))
306 {
307 // loop over columns j
308 for (j = CHX; j <= CHZ; j++)
309 {
310 ftmp = 1.0F / fmod[j];
311 // loop over rows i
312 for (i = CHX; i <= CHZ; i++)
313 {
314 // normalize by the column modulus
315 fR[i][j] *= ftmp;
316 }
317 }
318 }
319 else
320 {
321 // no solution is possible so set rotation to identity matrix
322 f3x3matrixAeqI(fR);
323 return;
324 }
325
326 // compute the geomagnetic inclination angle (deg)
327 *pfmodGc = fmod[CHZ];
328 *pfmodBc = sqrtf(fBc[CHX] * fBc[CHX] + fBc[CHY] * fBc[CHY] + fBc[CHZ] * fBc[CHZ]);
329 fGcdotBc = fGc[CHX] * fBc[CHX] + fGc[CHY] * fBc[CHY] + fGc[CHZ] * fBc[CHZ];
330 if (!((*pfmodGc == 0.0F) || (*pfmodBc == 0.0F)))
331 {
332 *pfsinDelta = fGcdotBc / (*pfmodGc * *pfmodBc);
333 *pfcosDelta = sqrtf(1.0F - *pfsinDelta * *pfsinDelta);
334 *pfDelta = fasin_deg(*pfsinDelta);
335 }
336
337 return;
338 }
339 #endif // #if THISCOORDSYSTEM == NED
340
341 // Android: basic 6DOF e-Compass function computing rotation matrix fR and magnetic inclination angle fDelta
342 #if THISCOORDSYSTEM == ANDROID
feCompassAndroid(float fR[][3],float * pfDelta,float * pfsinDelta,float * pfcosDelta,float fBc[],float fGc[],float * pfmodBc,float * pfmodGc)343 void feCompassAndroid(float fR[][3], float *pfDelta, float *pfsinDelta, float *pfcosDelta, float fBc[], float fGc[],
344 float *pfmodBc, float *pfmodGc)
345 {
346 // local variables
347 float fmod[3]; // column moduli
348 float fGcdotBc; // dot product of vectors G.Bc
349 float ftmp; // scratch variable
350 int8 i, j; // loop counters
351
352 // set the inclination angle to zero in case it is not computed later
353 *pfDelta = *pfsinDelta = 0.0F;
354 *pfcosDelta = 1.0F;
355
356 // place the un-normalized gravity and geomagnetic vectors into the rotation matrix z and y axes
357 for (i = CHX; i <= CHZ; i++)
358 {
359 fR[i][CHZ] = fGc[i];
360 fR[i][CHY] = fBc[i];
361 }
362
363 // set x vector to vector product of y and z vectors
364 fR[CHX][CHX] = fR[CHY][CHY] * fR[CHZ][CHZ] - fR[CHZ][CHY] * fR[CHY][CHZ];
365 fR[CHY][CHX] = fR[CHZ][CHY] * fR[CHX][CHZ] - fR[CHX][CHY] * fR[CHZ][CHZ];
366 fR[CHZ][CHX] = fR[CHX][CHY] * fR[CHY][CHZ] - fR[CHY][CHY] * fR[CHX][CHZ];
367
368 // set y vector to vector product of z and x vectors
369 fR[CHX][CHY] = fR[CHY][CHZ] * fR[CHZ][CHX] - fR[CHZ][CHZ] * fR[CHY][CHX];
370 fR[CHY][CHY] = fR[CHZ][CHZ] * fR[CHX][CHX] - fR[CHX][CHZ] * fR[CHZ][CHX];
371 fR[CHZ][CHY] = fR[CHX][CHZ] * fR[CHY][CHX] - fR[CHY][CHZ] * fR[CHX][CHX];
372
373 // calculate the rotation matrix column moduli
374 fmod[CHX] = sqrtf(fR[CHX][CHX] * fR[CHX][CHX] + fR[CHY][CHX] * fR[CHY][CHX] + fR[CHZ][CHX] * fR[CHZ][CHX]);
375 fmod[CHY] = sqrtf(fR[CHX][CHY] * fR[CHX][CHY] + fR[CHY][CHY] * fR[CHY][CHY] + fR[CHZ][CHY] * fR[CHZ][CHY]);
376 fmod[CHZ] = sqrtf(fR[CHX][CHZ] * fR[CHX][CHZ] + fR[CHY][CHZ] * fR[CHY][CHZ] + fR[CHZ][CHZ] * fR[CHZ][CHZ]);
377
378 // normalize the rotation matrix columns
379 if (!((fmod[CHX] == 0.0F) || (fmod[CHY] == 0.0F) || (fmod[CHZ] == 0.0F)))
380 {
381 // loop over columns j
382 for (j = CHX; j <= CHZ; j++)
383 {
384 ftmp = 1.0F / fmod[j];
385 // loop over rows i
386 for (i = CHX; i <= CHZ; i++)
387 {
388 // normalize by the column modulus
389 fR[i][j] *= ftmp;
390 }
391 }
392 }
393 else
394 {
395 // no solution is possible so set rotation to identity matrix
396 f3x3matrixAeqI(fR);
397 return;
398 }
399
400 // compute the geomagnetic inclination angle (deg)
401 *pfmodGc = fmod[CHZ];
402 *pfmodBc = sqrtf(fBc[CHX] * fBc[CHX] + fBc[CHY] * fBc[CHY] + fBc[CHZ] * fBc[CHZ]);
403 fGcdotBc = fGc[CHX] * fBc[CHX] + fGc[CHY] * fBc[CHY] + fGc[CHZ] * fBc[CHZ];
404 if (!((*pfmodGc == 0.0F) || (*pfmodBc == 0.0F)))
405 {
406 *pfsinDelta = -fGcdotBc / (*pfmodGc * *pfmodBc);
407 *pfcosDelta = sqrtf(1.0F - *pfsinDelta * *pfsinDelta);
408 *pfDelta = fasin_deg(*pfsinDelta);
409 }
410
411 return;
412 }
413 #endif // #if THISCOORDSYSTEM == ANDROID
414
415 // Win8: basic 6DOF e-Compass function computing rotation matrix fR and magnetic inclination angle fDelta
416 #if (THISCOORDSYSTEM == WIN8)
feCompassWin8(float fR[][3],float * pfDelta,float * pfsinDelta,float * pfcosDelta,float fBc[],float fGc[],float * pfmodBc,float * pfmodGc)417 void feCompassWin8(float fR[][3], float *pfDelta, float *pfsinDelta, float *pfcosDelta, float fBc[], float fGc[],
418 float *pfmodBc, float *pfmodGc)
419 {
420 // local variables
421 float fmod[3]; // column moduli
422 float fGcdotBc; // dot product of vectors G.Bc
423 float ftmp; // scratch variable
424 int8 i, j; // loop counters
425
426 // set the inclination angle to zero in case it is not computed later
427 *pfDelta = *pfsinDelta = 0.0F;
428 *pfcosDelta = 1.0F;
429
430 // place the negated un-normalized gravity and un-normalized geomagnetic vectors into the rotation matrix z and y axes
431 for (i = CHX; i <= CHZ; i++)
432 {
433 fR[i][CHZ] = -fGc[i];
434 fR[i][CHY] = fBc[i];
435 }
436
437 // set x vector to vector product of y and z vectors
438 fR[CHX][CHX] = fR[CHY][CHY] * fR[CHZ][CHZ] - fR[CHZ][CHY] * fR[CHY][CHZ];
439 fR[CHY][CHX] = fR[CHZ][CHY] * fR[CHX][CHZ] - fR[CHX][CHY] * fR[CHZ][CHZ];
440 fR[CHZ][CHX] = fR[CHX][CHY] * fR[CHY][CHZ] - fR[CHY][CHY] * fR[CHX][CHZ];
441
442 // set y vector to vector product of z and x vectors
443 fR[CHX][CHY] = fR[CHY][CHZ] * fR[CHZ][CHX] - fR[CHZ][CHZ] * fR[CHY][CHX];
444 fR[CHY][CHY] = fR[CHZ][CHZ] * fR[CHX][CHX] - fR[CHX][CHZ] * fR[CHZ][CHX];
445 fR[CHZ][CHY] = fR[CHX][CHZ] * fR[CHY][CHX] - fR[CHY][CHZ] * fR[CHX][CHX];
446
447 // calculate the rotation matrix column moduli
448 fmod[CHX] = sqrtf(fR[CHX][CHX] * fR[CHX][CHX] + fR[CHY][CHX] * fR[CHY][CHX] + fR[CHZ][CHX] * fR[CHZ][CHX]);
449 fmod[CHY] = sqrtf(fR[CHX][CHY] * fR[CHX][CHY] + fR[CHY][CHY] * fR[CHY][CHY] + fR[CHZ][CHY] * fR[CHZ][CHY]);
450 fmod[CHZ] = sqrtf(fR[CHX][CHZ] * fR[CHX][CHZ] + fR[CHY][CHZ] * fR[CHY][CHZ] + fR[CHZ][CHZ] * fR[CHZ][CHZ]);
451
452 // normalize the rotation matrix columns
453 if (!((fmod[CHX] == 0.0F) || (fmod[CHY] == 0.0F) || (fmod[CHZ] == 0.0F)))
454 {
455 // loop over columns j
456 for (j = CHX; j <= CHZ; j++)
457 {
458 ftmp = 1.0F / fmod[j];
459 // loop over rows i
460 for (i = CHX; i <= CHZ; i++)
461 {
462 // normalize by the column modulus
463 fR[i][j] *= ftmp;
464 }
465 }
466 }
467 else
468 {
469 // no solution is possible so set rotation to identity matrix
470 f3x3matrixAeqI(fR);
471 return;
472 }
473
474 // compute the geomagnetic inclination angle (deg)
475 *pfmodGc = fmod[CHZ];
476 *pfmodBc = sqrtf(fBc[CHX] * fBc[CHX] + fBc[CHY] * fBc[CHY] + fBc[CHZ] * fBc[CHZ]);
477 fGcdotBc = fGc[CHX] * fBc[CHX] + fGc[CHY] * fBc[CHY] + fGc[CHZ] * fBc[CHZ];
478 if (!((*pfmodGc == 0.0F) || (*pfmodBc == 0.0F)))
479 {
480 *pfsinDelta = fGcdotBc / (*pfmodGc * *pfmodBc);
481 *pfcosDelta = sqrtf(1.0F - *pfsinDelta * *pfsinDelta);
482 *pfDelta = fasin_deg(*pfsinDelta);
483 }
484
485 return;
486 }
487 #endif // #if (THISCOORDSYSTEM == WIN8)
488 #endif // #if F_USING_MAG // Need eCompass conversion routines
489
490 // extract the NED angles in degrees from the NED rotation matrix
491 #if THISCOORDSYSTEM == NED
fNEDAnglesDegFromRotationMatrix(float R[][3],float * pfPhiDeg,float * pfTheDeg,float * pfPsiDeg,float * pfRhoDeg,float * pfChiDeg)492 void fNEDAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg,
493 float *pfRhoDeg, float *pfChiDeg)
494 {
495 // calculate the pitch angle -90.0 <= Theta <= 90.0 deg
496 *pfTheDeg = fasin_deg(-R[CHX][CHZ]);
497
498 // calculate the roll angle range -180.0 <= Phi < 180.0 deg
499 *pfPhiDeg = fatan2_deg(R[CHY][CHZ], R[CHZ][CHZ]);
500
501 // map +180 roll onto the functionally equivalent -180 deg roll
502 if (*pfPhiDeg == 180.0F)
503 {
504 *pfPhiDeg = -180.0F;
505 }
506
507 // calculate the yaw (compass) angle 0.0 <= Psi < 360.0 deg
508 if (*pfTheDeg == 90.0F)
509 {
510 // vertical upwards gimbal lock case
511 *pfPsiDeg = fatan2_deg(R[CHZ][CHY], R[CHY][CHY]) + *pfPhiDeg;
512 }
513 else if (*pfTheDeg == -90.0F)
514 {
515 // vertical downwards gimbal lock case
516 *pfPsiDeg = fatan2_deg(-R[CHZ][CHY], R[CHY][CHY]) - *pfPhiDeg;
517 }
518 else
519 {
520 // general case
521 *pfPsiDeg = fatan2_deg(R[CHX][CHY], R[CHX][CHX]);
522 }
523
524 // map yaw angle Psi onto range 0.0 <= Psi < 360.0 deg
525 if (*pfPsiDeg < 0.0F)
526 {
527 *pfPsiDeg += 360.0F;
528 }
529
530 // check for rounding errors mapping small negative angle to 360 deg
531 if (*pfPsiDeg >= 360.0F)
532 {
533 *pfPsiDeg = 0.0F;
534 }
535
536 // for NED, the compass heading Rho equals the yaw angle Psi
537 *pfRhoDeg = *pfPsiDeg;
538
539 // calculate the tilt angle from vertical Chi (0 <= Chi <= 180 deg)
540 *pfChiDeg = facos_deg(R[CHZ][CHZ]);
541
542 return;
543 }
544 #endif // #if THISCOORDSYSTEM == NED
545
546 // extract the Android angles in degrees from the Android rotation matrix
547 #if THISCOORDSYSTEM == ANDROID
fAndroidAnglesDegFromRotationMatrix(float R[][3],float * pfPhiDeg,float * pfTheDeg,float * pfPsiDeg,float * pfRhoDeg,float * pfChiDeg)548 void fAndroidAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg,
549 float *pfRhoDeg, float *pfChiDeg)
550 {
551 // calculate the roll angle -90.0 <= Phi <= 90.0 deg
552 *pfPhiDeg = fasin_deg(R[CHX][CHZ]);
553
554 // calculate the pitch angle -180.0 <= The < 180.0 deg
555 *pfTheDeg = fatan2_deg(-R[CHY][CHZ], R[CHZ][CHZ]);
556
557 // map +180 pitch onto the functionally equivalent -180 deg pitch
558 if (*pfTheDeg == 180.0F)
559 {
560 *pfTheDeg = -180.0F;
561 }
562
563 // calculate the yaw (compass) angle 0.0 <= Psi < 360.0 deg
564 if (*pfPhiDeg == 90.0F)
565 {
566 // vertical downwards gimbal lock case
567 *pfPsiDeg = fatan2_deg(R[CHY][CHX], R[CHY][CHY]) - *pfTheDeg;
568 }
569 else if (*pfPhiDeg == -90.0F)
570 {
571 // vertical upwards gimbal lock case
572 *pfPsiDeg = fatan2_deg(R[CHY][CHX], R[CHY][CHY]) + *pfTheDeg;
573 }
574 else
575 {
576 // general case
577 *pfPsiDeg = fatan2_deg(-R[CHX][CHY], R[CHX][CHX]);
578 }
579
580 // map yaw angle Psi onto range 0.0 <= Psi < 360.0 deg
581 if (*pfPsiDeg < 0.0F)
582 {
583 *pfPsiDeg += 360.0F;
584 }
585
586 // check for rounding errors mapping small negative angle to 360 deg
587 if (*pfPsiDeg >= 360.0F)
588 {
589 *pfPsiDeg = 0.0F;
590 }
591
592 // the compass heading angle Rho equals the yaw angle Psi
593 // this definition is compliant with Motorola Xoom tablet behavior
594 *pfRhoDeg = *pfPsiDeg;
595
596 // calculate the tilt angle from vertical Chi (0 <= Chi <= 180 deg)
597 *pfChiDeg = facos_deg(R[CHZ][CHZ]);
598
599 return;
600 }
601 #endif // #if THISCOORDSYSTEM == ANDROID
602
603 // extract the Windows 8 angles in degrees from the Windows 8 rotation matrix
604 #if (THISCOORDSYSTEM == WIN8)
fWin8AnglesDegFromRotationMatrix(float R[][3],float * pfPhiDeg,float * pfTheDeg,float * pfPsiDeg,float * pfRhoDeg,float * pfChiDeg)605 void fWin8AnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg,
606 float *pfRhoDeg, float *pfChiDeg)
607 {
608 // calculate the roll angle -90.0 <= Phi <= 90.0 deg
609 if (R[CHZ][CHZ] == 0.0F)
610 {
611 if (R[CHX][CHZ] >= 0.0F)
612 {
613 // tan(phi) is -infinity
614 *pfPhiDeg = -90.0F;
615 }
616 else
617 {
618 // tan(phi) is +infinity
619 *pfPhiDeg = 90.0F;
620 }
621 }
622 else
623 {
624 // general case
625 *pfPhiDeg = fatan_deg(-R[CHX][CHZ] / R[CHZ][CHZ]);
626 }
627
628 // first calculate the pitch angle The in the range -90.0 <= The <= 90.0 deg
629 *pfTheDeg = fasin_deg(R[CHY][CHZ]);
630
631 // use R[CHZ][CHZ]=cos(Phi)*cos(The) to correct the quadrant of The remembering
632 // cos(Phi) is non-negative so that cos(The) has the same sign as R[CHZ][CHZ].
633 if (R[CHZ][CHZ] < 0.0F)
634 {
635 // wrap The around +90 deg and -90 deg giving result 90 to 270 deg
636 *pfTheDeg = 180.0F - *pfTheDeg;
637 }
638
639 // map the pitch angle The to the range -180.0 <= The < 180.0 deg
640 if (*pfTheDeg >= 180.0F)
641 {
642 *pfTheDeg -= 360.0F;
643 }
644
645 // calculate the yaw angle Psi
646 if (*pfTheDeg == 90.0F)
647 {
648 // vertical upwards gimbal lock case: -270 <= Psi < 90 deg
649 *pfPsiDeg = fatan2_deg(R[CHX][CHY], R[CHX][CHX]) - *pfPhiDeg;
650 }
651 else if (*pfTheDeg == -90.0F)
652 {
653 // vertical downwards gimbal lock case: -270 <= Psi < 90 deg
654 *pfPsiDeg = fatan2_deg(R[CHX][CHY], R[CHX][CHX]) + *pfPhiDeg;
655 }
656 else
657 {
658 // general case: -180 <= Psi < 180 deg
659 *pfPsiDeg = fatan2_deg(-R[CHY][CHX], R[CHY][CHY]);
660
661 // correct the quadrant for Psi using the value of The (deg) to give -180 <= Psi < 380 deg
662 if (fabsf(*pfTheDeg) >= 90.0F)
663 {
664 *pfPsiDeg += 180.0F;
665 }
666 }
667
668 // map yaw angle Psi onto range 0.0 <= Psi < 360.0 deg
669 if (*pfPsiDeg < 0.0F)
670 {
671 *pfPsiDeg += 360.0F;
672 }
673
674 // check for any rounding error mapping small negative angle to 360 deg
675 if (*pfPsiDeg >= 360.0F)
676 {
677 *pfPsiDeg = 0.0F;
678 }
679
680 // compute the compass angle Rho = 360 - Psi
681 *pfRhoDeg = 360.0F - *pfPsiDeg;
682
683 // check for rounding errors mapping small negative angle to 360 deg and zero degree case
684 if (*pfRhoDeg >= 360.0F)
685 {
686 *pfRhoDeg = 0.0F;
687 }
688
689 // calculate the tilt angle from vertical Chi (0 <= Chi <= 180 deg)
690 *pfChiDeg = facos_deg(R[CHZ][CHZ]);
691
692 return;
693 }
694 #endif // #if (THISCOORDSYSTEM == WIN8)
695
696 // computes normalized rotation quaternion from a rotation vector (deg)
fQuaternionFromRotationVectorDeg(Quaternion * pq,const float rvecdeg[],float fscaling)697 void fQuaternionFromRotationVectorDeg(Quaternion *pq, const float rvecdeg[], float fscaling)
698 {
699 float fetadeg; // rotation angle (deg)
700 float fetarad; // rotation angle (rad)
701 float fetarad2; // eta (rad)^2
702 float fetarad4; // eta (rad)^4
703 float sinhalfeta; // sin(eta/2)
704 float fvecsq; // q1^2+q2^2+q3^2
705 float ftmp; // scratch variable
706
707 // compute the scaled rotation angle eta (deg) which can be both positve or negative
708 fetadeg = fscaling * sqrtf(rvecdeg[CHX] * rvecdeg[CHX] + rvecdeg[CHY] * rvecdeg[CHY] + rvecdeg[CHZ] * rvecdeg[CHZ]);
709 fetarad = fetadeg * FPIOVER180;
710 fetarad2 = fetarad * fetarad;
711
712 // calculate the sine and cosine using small angle approximations or exact
713 // angles under sqrt(0.02)=0.141 rad is 8.1 deg and 1620 deg/s (=936deg/s in 3 axes) at 200Hz and 405 deg/s at 50Hz
714 if (fetarad2 <= 0.02F)
715 {
716 // use MacLaurin series up to and including third order
717 sinhalfeta = fetarad * (0.5F - ONEOVER48 * fetarad2);
718 }
719 else if (fetarad2 <= 0.06F)
720 {
721 // use MacLaurin series up to and including fifth order
722 // angles under sqrt(0.06)=0.245 rad is 14.0 deg and 2807 deg/s (=1623deg/s in 3 axes) at 200Hz and 703 deg/s at 50Hz
723 fetarad4 = fetarad2 * fetarad2;
724 sinhalfeta = fetarad * (0.5F - ONEOVER48 * fetarad2 + ONEOVER3840 * fetarad4);
725 }
726 else
727 {
728 // use exact calculation
729 sinhalfeta = (float)sinf(0.5F * fetarad);
730 }
731
732 // compute the vector quaternion components q1, q2, q3
733 if (fetadeg != 0.0F)
734 {
735 // general case with non-zero rotation angle
736 ftmp = fscaling * sinhalfeta / fetadeg;
737 pq->q1 = rvecdeg[CHX] * ftmp; // q1 = nx * sin(eta/2)
738 pq->q2 = rvecdeg[CHY] * ftmp; // q2 = ny * sin(eta/2)
739 pq->q3 = rvecdeg[CHZ] * ftmp; // q3 = nz * sin(eta/2)
740 }
741 else
742 {
743 // zero rotation angle giving zero vector component
744 pq->q1 = pq->q2 = pq->q3 = 0.0F;
745 }
746
747 // compute the scalar quaternion component q0 by explicit normalization
748 // taking care to avoid rounding errors giving negative operand to sqrt
749 fvecsq = pq->q1 * pq->q1 + pq->q2 * pq->q2 + pq->q3 * pq->q3;
750 if (fvecsq <= 1.0F)
751 {
752 // normal case
753 pq->q0 = sqrtf(1.0F - fvecsq);
754 }
755 else
756 {
757 // rounding errors are present
758 pq->q0 = 0.0F;
759 }
760
761 return;
762 }
763
764 // compute the orientation quaternion from a 3x3 rotation matrix
fQuaternionFromRotationMatrix(float R[][3],Quaternion * pq)765 void fQuaternionFromRotationMatrix(float R[][3], Quaternion *pq)
766 {
767 float fq0sq; // q0^2
768 float recip4q0; // 1/4q0
769
770 // get q0^2 and q0
771 fq0sq = 0.25F * (1.0F + R[CHX][CHX] + R[CHY][CHY] + R[CHZ][CHZ]);
772 pq->q0 = sqrtf(fabsf(fq0sq));
773
774 // normal case when q0 is not small meaning rotation angle not near 180 deg
775 if (pq->q0 > SMALLQ0)
776 {
777 // calculate q1 to q3
778 recip4q0 = 0.25F / pq->q0;
779 pq->q1 = recip4q0 * (R[CHY][CHZ] - R[CHZ][CHY]);
780 pq->q2 = recip4q0 * (R[CHZ][CHX] - R[CHX][CHZ]);
781 pq->q3 = recip4q0 * (R[CHX][CHY] - R[CHY][CHX]);
782 } // end of general case
783 else
784 {
785 // special case of near 180 deg corresponds to nearly symmetric matrix
786 // which is not numerically well conditioned for division by small q0
787 // instead get absolute values of q1 to q3 from leading diagonal
788 pq->q1 = sqrtf(fabsf(0.5F + 0.5F * R[CHX][CHX] - fq0sq));
789 pq->q2 = sqrtf(fabsf(0.5F + 0.5F * R[CHY][CHY] - fq0sq));
790 pq->q3 = sqrtf(fabsf(0.5F + 0.5F * R[CHZ][CHZ] - fq0sq));
791
792 // correct the signs of q1 to q3 by examining the signs of differenced off-diagonal terms
793 if ((R[CHY][CHZ] - R[CHZ][CHY]) < 0.0F) pq->q1 = -pq->q1;
794 if ((R[CHZ][CHX] - R[CHX][CHZ]) < 0.0F) pq->q2 = -pq->q2;
795 if ((R[CHX][CHY] - R[CHY][CHX]) < 0.0F) pq->q3 = -pq->q3;
796 } // end of special case
797
798 // ensure that the resulting quaternion is normalized even if the input rotation matrix was not
799 fqAeqNormqA(pq);
800
801
802 return;
803 }
804
805 // compute the rotation matrix from an orientation quaternion
fRotationMatrixFromQuaternion(float R[][3],const Quaternion * pq)806 void fRotationMatrixFromQuaternion(float R[][3], const Quaternion *pq)
807 {
808 float f2q;
809 float f2q0q0, f2q0q1, f2q0q2, f2q0q3;
810 float f2q1q1, f2q1q2, f2q1q3;
811 float f2q2q2, f2q2q3;
812 float f2q3q3;
813
814 // set f2q to 2*q0 and calculate products
815 f2q = 2.0F * pq->q0;
816 f2q0q0 = f2q * pq->q0;
817 f2q0q1 = f2q * pq->q1;
818 f2q0q2 = f2q * pq->q2;
819 f2q0q3 = f2q * pq->q3;
820 // set f2q to 2*q1 and calculate products
821 f2q = 2.0F * pq->q1;
822 f2q1q1 = f2q * pq->q1;
823 f2q1q2 = f2q * pq->q2;
824 f2q1q3 = f2q * pq->q3;
825 // set f2q to 2*q2 and calculate products
826 f2q = 2.0F * pq->q2;
827 f2q2q2 = f2q * pq->q2;
828 f2q2q3 = f2q * pq->q3;
829 f2q3q3 = 2.0F * pq->q3 * pq->q3;
830
831 // calculate the rotation matrix assuming the quaternion is normalized
832 R[CHX][CHX] = f2q0q0 + f2q1q1 - 1.0F;
833 R[CHX][CHY] = f2q1q2 + f2q0q3;
834 R[CHX][CHZ] = f2q1q3 - f2q0q2;
835 R[CHY][CHX] = f2q1q2 - f2q0q3;
836 R[CHY][CHY] = f2q0q0 + f2q2q2 - 1.0F;
837 R[CHY][CHZ] = f2q2q3 + f2q0q1;
838 R[CHZ][CHX] = f2q1q3 + f2q0q2;
839 R[CHZ][CHY] = f2q2q3 - f2q0q1;
840 R[CHZ][CHZ] = f2q0q0 + f2q3q3 - 1.0F;
841
842 return;
843 }
844
845 // computes rotation vector (deg) from rotation quaternion
fRotationVectorDegFromQuaternion(Quaternion * pq,float rvecdeg[])846 void fRotationVectorDegFromQuaternion(Quaternion *pq, float rvecdeg[])
847 {
848 float fetarad; // rotation angle (rad)
849 float fetadeg; // rotation angle (deg)
850 float sinhalfeta; // sin(eta/2)
851 float ftmp; // scratch variable
852
853 // calculate the rotation angle in the range 0 <= eta < 360 deg
854 if ((pq->q0 >= 1.0F) || (pq->q0 <= -1.0F))
855 {
856 // rotation angle is 0 deg or 2*180 deg = 360 deg = 0 deg
857 fetarad = 0.0F;
858 fetadeg = 0.0F;
859 }
860 else
861 {
862 // general case returning 0 < eta < 360 deg
863 fetarad = 2.0F * acosf(pq->q0);
864 fetadeg = fetarad * F180OVERPI;
865 }
866
867 // map the rotation angle onto the range -180 deg <= eta < 180 deg
868 if (fetadeg >= 180.0F)
869 {
870 fetadeg -= 360.0F;
871 fetarad = fetadeg * FPIOVER180;
872 }
873
874 // calculate sin(eta/2) which will be in the range -1 to +1
875 sinhalfeta = (float)sinf(0.5F * fetarad);
876
877 // calculate the rotation vector (deg)
878 if (sinhalfeta == 0.0F)
879 {
880 // the rotation angle eta is zero and the axis is irrelevant
881 rvecdeg[CHX] = rvecdeg[CHY] = rvecdeg[CHZ] = 0.0F;
882 }
883 else
884 {
885 // general case with non-zero rotation angle
886 ftmp = fetadeg / sinhalfeta;
887 rvecdeg[CHX] = pq->q1 * ftmp;
888 rvecdeg[CHY] = pq->q2 * ftmp;
889 rvecdeg[CHZ] = pq->q3 * ftmp;
890 }
891
892 return;
893 }
894
895 // function low pass filters an orientation quaternion and computes virtual gyro rotation rate
fLPFOrientationQuaternion(Quaternion * pq,Quaternion * pLPq,float flpf,float fdeltat,float fOmega[])896 void fLPFOrientationQuaternion(Quaternion *pq, Quaternion *pLPq, float flpf, float fdeltat, float fOmega[])
897 {
898 // local variables
899 Quaternion fdeltaq; // delta rotation quaternion
900 float rvecdeg[3]; // rotation vector (deg)
901 float ftmp; // scratch variable
902
903 // set fdeltaq to the delta rotation quaternion conjg(fLPq[n-1) . fqn
904 fdeltaq = qconjgAxB(pLPq, pq);
905 if (fdeltaq.q0 < 0.0F)
906 {
907 fdeltaq.q0 = -fdeltaq.q0;
908 fdeltaq.q1 = -fdeltaq.q1;
909 fdeltaq.q2 = -fdeltaq.q2;
910 fdeltaq.q3 = -fdeltaq.q3;
911 }
912
913 // set ftmp to a scaled value which equals flpf in the limit of small rotations (q0=1)
914 // but which rises to 1 (all pass) as the delta rotation angle increases (q0 tends to zero)
915 ftmp = flpf + (1.0F - flpf) * sqrtf(fabs(1.0F - fdeltaq.q0 * fdeltaq.q0));
916
917 // scale the vector component of the delta rotation quaternion by the corrected lpf value
918 // and re-compute the scalar component q0 to ensure normalization
919 fdeltaq.q1 *= ftmp;
920 fdeltaq.q2 *= ftmp;
921 fdeltaq.q3 *= ftmp;
922 fdeltaq.q0 = sqrtf(fabsf(1.0F - fdeltaq.q1 * fdeltaq.q1 - fdeltaq.q2 * fdeltaq.q2 - fdeltaq.q3 * fdeltaq.q3));
923
924 // calculate the delta rotation vector from fdeltaq and the virtual gyro angular velocity (deg/s)
925 fRotationVectorDegFromQuaternion(&fdeltaq, rvecdeg);
926 ftmp = 1.0F / fdeltat;
927 fOmega[CHX] = rvecdeg[CHX] * ftmp;
928 fOmega[CHY] = rvecdeg[CHY] * ftmp;
929 fOmega[CHZ] = rvecdeg[CHZ] * ftmp;
930
931 // set LPq[n] = LPq[n-1] . deltaq[n]
932 qAeqAxB(pLPq, &fdeltaq);
933
934 // renormalize the low pass filtered quaternion to prevent error accumulation.
935 // the renormalization function also ensures that q0 is non-negative
936 fqAeqNormqA(pLPq);
937
938 return;
939 }
940
941 // function compute the quaternion product qA * qB
qAeqBxC(Quaternion * pqA,const Quaternion * pqB,const Quaternion * pqC)942 void qAeqBxC(Quaternion *pqA, const Quaternion *pqB, const Quaternion *pqC)
943 {
944 pqA->q0 = pqB->q0 * pqC->q0 - pqB->q1 * pqC->q1 - pqB->q2 * pqC->q2 - pqB->q3 * pqC->q3;
945 pqA->q1 = pqB->q0 * pqC->q1 + pqB->q1 * pqC->q0 + pqB->q2 * pqC->q3 - pqB->q3 * pqC->q2;
946 pqA->q2 = pqB->q0 * pqC->q2 - pqB->q1 * pqC->q3 + pqB->q2 * pqC->q0 + pqB->q3 * pqC->q1;
947 pqA->q3 = pqB->q0 * pqC->q3 + pqB->q1 * pqC->q2 - pqB->q2 * pqC->q1 + pqB->q3 * pqC->q0;
948
949 return;
950 }
951
952 // function compute the quaternion product qA = qA * qB
qAeqAxB(Quaternion * pqA,const Quaternion * pqB)953 void qAeqAxB(Quaternion *pqA, const Quaternion *pqB)
954 {
955 Quaternion qProd;
956
957 // perform the quaternion product
958 qProd.q0 = pqA->q0 * pqB->q0 - pqA->q1 * pqB->q1 - pqA->q2 * pqB->q2 - pqA->q3 * pqB->q3;
959 qProd.q1 = pqA->q0 * pqB->q1 + pqA->q1 * pqB->q0 + pqA->q2 * pqB->q3 - pqA->q3 * pqB->q2;
960 qProd.q2 = pqA->q0 * pqB->q2 - pqA->q1 * pqB->q3 + pqA->q2 * pqB->q0 + pqA->q3 * pqB->q1;
961 qProd.q3 = pqA->q0 * pqB->q3 + pqA->q1 * pqB->q2 - pqA->q2 * pqB->q1 + pqA->q3 * pqB->q0;
962
963 // copy the result back into qA
964 *pqA = qProd;
965
966 return;
967 }
968
969 // function compute the quaternion product conjg(qA) * qB
qconjgAxB(const Quaternion * pqA,const Quaternion * pqB)970 Quaternion qconjgAxB(const Quaternion *pqA, const Quaternion *pqB)
971 {
972 Quaternion qProd;
973
974 qProd.q0 = pqA->q0 * pqB->q0 + pqA->q1 * pqB->q1 + pqA->q2 * pqB->q2 + pqA->q3 * pqB->q3;
975 qProd.q1 = pqA->q0 * pqB->q1 - pqA->q1 * pqB->q0 - pqA->q2 * pqB->q3 + pqA->q3 * pqB->q2;
976 qProd.q2 = pqA->q0 * pqB->q2 + pqA->q1 * pqB->q3 - pqA->q2 * pqB->q0 - pqA->q3 * pqB->q1;
977 qProd.q3 = pqA->q0 * pqB->q3 - pqA->q1 * pqB->q2 + pqA->q2 * pqB->q1 - pqA->q3 * pqB->q0;
978
979 return qProd;
980 }
981
982 // function normalizes a rotation quaternion and ensures q0 is non-negative
fqAeqNormqA(Quaternion * pqA)983 void fqAeqNormqA(Quaternion *pqA)
984 {
985 float fNorm; // quaternion Norm
986
987 // calculate the quaternion Norm
988 fNorm = sqrtf(pqA->q0 * pqA->q0 + pqA->q1 * pqA->q1 + pqA->q2 * pqA->q2 + pqA->q3 * pqA->q3);
989 if (fNorm > CORRUPTQUAT)
990 {
991 // general case
992 fNorm = 1.0F / fNorm;
993 pqA->q0 *= fNorm;
994 pqA->q1 *= fNorm;
995 pqA->q2 *= fNorm;
996 pqA->q3 *= fNorm;
997 }
998 else
999 {
1000 // return with identity quaternion since the quaternion is corrupted
1001 pqA->q0 = 1.0F;
1002 pqA->q1 = pqA->q2 = pqA->q3 = 0.0F;
1003 }
1004
1005 // correct a negative scalar component if the function was called with negative q0
1006 if (pqA->q0 < 0.0F)
1007 {
1008 pqA->q0 = -pqA->q0;
1009 pqA->q1 = -pqA->q1;
1010 pqA->q2 = -pqA->q2;
1011 pqA->q3 = -pqA->q3;
1012 }
1013
1014 return;
1015 }
1016
1017 // set a quaternion to the unit quaternion
fqAeq1(Quaternion * pqA)1018 void fqAeq1(Quaternion *pqA)
1019 {
1020 pqA->q0 = 1.0F;
1021 pqA->q1 = pqA->q2 = pqA->q3 = 0.0F;
1022
1023 return;
1024 }
1025
1026 // function computes the rotation quaternion that rotates unit vector u onto unit vector v as v=q*.u.q
1027 // using q = 1/sqrt(2) * {sqrt(1 + u.v) - u x v / sqrt(1 + u.v)}
fveqconjgquq(Quaternion * pfq,float fu[],float fv[])1028 void fveqconjgquq(Quaternion *pfq, float fu[], float fv[])
1029 {
1030 float fuxv[3]; // vector product u x v
1031 float fsqrt1plusudotv; // sqrt(1 + u.v)
1032 float ftmp; // scratch
1033
1034 // compute sqrt(1 + u.v) and scalar quaternion component q0 (valid for all angles including 180 deg)
1035 fsqrt1plusudotv = sqrtf(fabsf(1.0F + fu[CHX] * fv[CHX] + fu[CHY] * fv[CHY] + fu[CHZ] * fv[CHZ]));
1036 pfq->q0 = ONEOVERSQRT2 * fsqrt1plusudotv;
1037
1038 // calculate the vector product uxv
1039 fuxv[CHX] = fu[CHY] * fv[CHZ] - fu[CHZ] * fv[CHY];
1040 fuxv[CHY] = fu[CHZ] * fv[CHX] - fu[CHX] * fv[CHZ];
1041 fuxv[CHZ] = fu[CHX] * fv[CHY] - fu[CHY] * fv[CHX];
1042
1043 // compute the vector component of the quaternion
1044 if (fsqrt1plusudotv != 0.0F)
1045 {
1046 // general case where u and v are not anti-parallel where u.v=-1
1047 ftmp = ONEOVERSQRT2 / fsqrt1plusudotv;
1048 pfq->q1 = -fuxv[CHX] * ftmp;
1049 pfq->q2 = -fuxv[CHY] * ftmp;
1050 pfq->q3 = -fuxv[CHZ] * ftmp;
1051 }
1052 else
1053 {
1054 // degenerate case where u and v are anti-aligned and the 180 deg rotation quaternion is not uniquely defined.
1055 // first calculate the un-normalized vector component (the scalar component q0 is already set to zero)
1056 pfq->q1 = fu[CHY] - fu[CHZ];
1057 pfq->q2 = fu[CHZ] - fu[CHX];
1058 pfq->q3 = fu[CHX] - fu[CHY];
1059 // and normalize the quaternion for this case checking for fu[CHX]=fu[CHY]=fuCHZ] where q1=q2=q3=0.
1060 ftmp = sqrtf(fabsf(pfq->q1 * pfq->q1 + pfq->q2 * pfq->q2 + pfq->q3 * pfq->q3));
1061 if (ftmp != 0.0F)
1062 {
1063 // normal case where all three components of fu (and fv=-fu) are not equal
1064 ftmp = 1.0F / ftmp;
1065 pfq->q1 *= ftmp;
1066 pfq->q2 *= ftmp;
1067 pfq->q3 *= ftmp;
1068 }
1069 else
1070 {
1071 // final case where the three entries are equal but since the vectors are known to be normalized
1072 // the vector u must be 1/root(3)*{1, 1, 1} or -1/root(3)*{1, 1, 1} so simply set the
1073 // rotation vector to 1/root(2)*{1, -1, 0} to cover both cases
1074 pfq->q1 = ONEOVERSQRT2;
1075 pfq->q2 = -ONEOVERSQRT2;
1076 pfq->q3 = 0.0F;
1077 }
1078 }
1079
1080 return;
1081 }
1082
