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 /*! \file matrix.c
10 \brief Matrix manipulation functions
11
12 Contains functions for basic manipulation of 3x3 matrices
13 */
14
15 #include "stdio.h"
16 #include "math.h"
17 #include "stdlib.h"
18 #include "time.h"
19
20 #include "sensor_fusion.h"
21 #include "matrix.h"
22
23 // compile time constants that are private to this file
24 #define CORRUPTMATRIX 0.001F // column vector modulus limit for rotation matrix
25
26 // function sets the 3x3 matrix A to the identity matrix
f3x3matrixAeqI(float A[][3])27 void f3x3matrixAeqI(float A[][3])
28 {
29 float *pAij; // pointer to A[i][j]
30 int8 i,
31 j; // loop counters
32 for (i = 0; i < 3; i++)
33 {
34 // set pAij to &A[i][j=0]
35 pAij = A[i];
36 for (j = 0; j < 3; j++)
37 {
38 *(pAij++) = 0.0F;
39 }
40
41 A[i][i] = 1.0F;
42 }
43
44 return;
45 }
46
47 // function sets 3x3 matrix A to 3x3 matrix B
f3x3matrixAeqB(float A[][3],float B[][3])48 void f3x3matrixAeqB(float A[][3], float B[][3])
49 {
50 float *pAij; // pointer to A[i][j]
51 float *pBij; // pointer to B[i][j]
52 int8 i,
53 j; // loop counters
54 for (i = 0; i < 3; i++)
55 {
56 // set pAij to &A[i][j=0] and pBij to &B[i][j=0]
57 pAij = A[i];
58 pBij = B[i];
59 for (j = 0; j < 3; j++)
60 {
61 *(pAij++) = *(pBij++);
62 }
63 }
64
65 return;
66 }
67
68 // function sets the matrix A to the identity matrix
fmatrixAeqI(float * A[],int16 rc)69 void fmatrixAeqI(float *A[], int16 rc)
70 {
71 // rc = rows and columns in A
72 float *pAij; // pointer to A[i][j]
73 int8 i,
74 j; // loop counters
75 for (i = 0; i < rc; i++)
76 {
77 // set pAij to &A[i][j=0]
78 pAij = A[i];
79 for (j = 0; j < rc; j++)
80 {
81 *(pAij++) = 0.0F;
82 }
83
84 A[i][i] = 1.0F;
85 }
86
87 return;
88 }
89
90 // function sets every entry in the 3x3 matrix A to a constant scalar
f3x3matrixAeqScalar(float A[][3],float Scalar)91 void f3x3matrixAeqScalar(float A[][3], float Scalar)
92 {
93 float *pAij; // pointer to A[i][j]
94 int8 i,
95 j; // counters
96 for (i = 0; i < 3; i++)
97 {
98 // set pAij to &A[i][j=0]
99 pAij = A[i];
100 for (j = 0; j < 3; j++)
101 {
102 *(pAij++) = Scalar;
103 }
104 }
105
106 return;
107 }
108
109 // function multiplies all elements of 3x3 matrix A by the specified scalar
f3x3matrixAeqAxScalar(float A[][3],float Scalar)110 void f3x3matrixAeqAxScalar(float A[][3], float Scalar)
111 {
112 float *pAij; // pointer to A[i][j]
113 int8 i,
114 j; // loop counters
115 for (i = 0; i < 3; i++)
116 {
117 // set pAij to &A[i][j=0]
118 pAij = A[i];
119 for (j = 0; j < 3; j++)
120 {
121 *(pAij++) *= Scalar;
122 }
123 }
124
125 return;
126 }
127
128 // function negates all elements of 3x3 matrix A
f3x3matrixAeqMinusA(float A[][3])129 void f3x3matrixAeqMinusA(float A[][3])
130 {
131 float *pAij; // pointer to A[i][j]
132 int8 i,
133 j; // loop counters
134 for (i = 0; i < 3; i++)
135 {
136 // set pAij to &A[i][j=0]
137 pAij = A[i];
138 for (j = 0; j < 3; j++)
139 {
140 *pAij = -*pAij;
141 pAij++;
142 }
143 }
144
145 return;
146 }
147
148 // function directly calculates the symmetric inverse of a symmetric 3x3 matrix
149 // only the on and above diagonal terms in B are used and need to be specified
f3x3matrixAeqInvSymB(float A[][3],float B[][3])150 void f3x3matrixAeqInvSymB(float A[][3], float B[][3])
151 {
152 float fB11B22mB12B12; // B[1][1] * B[2][2] - B[1][2] * B[1][2]
153 float fB12B02mB01B22; // B[1][2] * B[0][2] - B[0][1] * B[2][2]
154 float fB01B12mB11B02; // B[0][1] * B[1][2] - B[1][1] * B[0][2]
155 float ftmp; // determinant and then reciprocal
156
157 // calculate useful products
158 fB11B22mB12B12 = B[1][1] * B[2][2] - B[1][2] * B[1][2];
159 fB12B02mB01B22 = B[1][2] * B[0][2] - B[0][1] * B[2][2];
160 fB01B12mB11B02 = B[0][1] * B[1][2] - B[1][1] * B[0][2];
161
162 // set ftmp to the determinant of the input matrix B
163 ftmp = B[0][0] *
164 fB11B22mB12B12 +
165 B[0][1] *
166 fB12B02mB01B22 +
167 B[0][2] *
168 fB01B12mB11B02;
169
170 // set A to the inverse of B for any determinant except zero
171 if (ftmp != 0.0F)
172 {
173 ftmp = 1.0F / ftmp;
174 A[0][0] = fB11B22mB12B12 * ftmp;
175 A[1][0] = A[0][1] = fB12B02mB01B22 * ftmp;
176 A[2][0] = A[0][2] = fB01B12mB11B02 * ftmp;
177 A[1][1] = (B[0][0] * B[2][2] - B[0][2] * B[0][2]) * ftmp;
178 A[2][1] = A[1][2] = (B[0][2] * B[0][1] - B[0][0] * B[1][2]) * ftmp;
179 A[2][2] = (B[0][0] * B[1][1] - B[0][1] * B[0][1]) * ftmp;
180 }
181 else
182 {
183 // provide the identity matrix if the determinant is zero
184 f3x3matrixAeqI(A);
185 }
186
187 return;
188 }
189
190 // function calculates the determinant of a 3x3 matrix
f3x3matrixDetA(float A[][3])191 float f3x3matrixDetA(float A[][3])
192 {
193 return
194 (
195 A[CHX][CHX] *
196 (
197 A[CHY][CHY] *
198 A[CHZ][CHZ] -
199 A[CHY][CHZ] *
200 A[CHZ][CHY]
201 ) +
202 A[CHX][CHY] *
203 (A[CHY][CHZ] * A[CHZ][CHX] - A[CHY][CHX] * A[CHZ][CHZ]) +
204 A[CHX][CHZ] *
205 (A[CHY][CHX] * A[CHZ][CHY] - A[CHY][CHY] * A[CHZ][CHX])
206 );
207 }
208
209 // function computes all eigenvalues and eigenvectors of a real symmetric matrix A[0..n-1][0..n-1]
210 // stored in the top left of a 10x10 array A[10][10]
211 // A[][] is changed on output.
212 // eigval[0..n-1] returns the eigenvalues of A[][].
213 // eigvec[0..n-1][0..n-1] returns the normalized eigenvectors of A[][]
214 // the eigenvectors are not sorted by value
215 // n can vary up to and including 10 but the matrices A and eigvec must have 10 columns.
fEigenCompute10(float A[][10],float eigval[],float eigvec[][10],int8 n)216 void fEigenCompute10(float A[][10], float eigval[], float eigvec[][10], int8 n)
217 {
218 // maximum number of iterations to achieve convergence: in practice 6 is typical
219 #define NITERATIONS 15
220 // various trig functions of the jacobi rotation angle phi
221 float cot2phi,
222 tanhalfphi,
223 tanphi,
224 sinphi,
225 cosphi;
226
227 // scratch variable to prevent over-writing during rotations
228 float ftmp;
229
230 // residue from remaining non-zero above diagonal terms
231 float residue;
232
233 // matrix row and column indices
234 int8 i,
235 j;
236
237 // general loop counter
238 int8 k;
239
240 // timeout ctr for number of passes of the algorithm
241 int8 ctr;
242
243 // initialize eigenvectors matrix and eigenvalues array
244 for (i = 0; i < n; i++)
245 {
246 // loop over all columns
247 for (j = 0; j < n; j++)
248 {
249 // set on diagonal and off-diagonal elements to zero
250 eigvec[i][j] = 0.0F;
251 }
252
253 // correct the diagonal elements to 1.0
254 eigvec[i][i] = 1.0F;
255
256 // initialize the array of eigenvalues to the diagonal elements of A
257 eigval[i] = A[i][i];
258 }
259
260 // initialize the counter and loop until converged or NITERATIONS reached
261 ctr = 0;
262 do
263 {
264 // compute the absolute value of the above diagonal elements as exit criterion
265 residue = 0.0F;
266
267 // loop over rows excluding last row
268 for (i = 0; i < n - 1; i++)
269 {
270 // loop over above diagonal columns
271 for (j = i + 1; j < n; j++)
272 {
273 // accumulate the residual off diagonal terms which are being driven to zero
274 residue += fabsf(A[i][j]);
275 }
276 }
277
278 // check if we still have work to do
279 if (residue > 0.0F)
280 {
281 // loop over all rows with the exception of the last row (since only rotating above diagonal elements)
282 for (i = 0; i < n - 1; i++)
283 {
284 // loop over columns j (where j is always greater than i since above diagonal)
285 for (j = i + 1; j < n; j++)
286 {
287 // only continue with this element if the element is non-zero
288 if (fabsf(A[i][j]) > 0.0F)
289 {
290 // calculate cot(2*phi) where phi is the Jacobi rotation angle
291 cot2phi = 0.5F * (eigval[j] - eigval[i]) / (A[i][j]);
292
293 // calculate tan(phi) correcting sign to ensure the smaller solution is used
294 tanphi = 1.0F / (fabsf(cot2phi) + sqrtf(1.0F + cot2phi * cot2phi));
295 if (cot2phi < 0.0F)
296 {
297 tanphi = -tanphi;
298 }
299
300 // calculate the sine and cosine of the Jacobi rotation angle phi
301 cosphi = 1.0F / sqrtf(1.0F + tanphi * tanphi);
302 sinphi = tanphi * cosphi;
303
304 // calculate tan(phi/2)
305 tanhalfphi = sinphi / (1.0F + cosphi);
306
307 // set tmp = tan(phi) times current matrix element used in update of leading diagonal elements
308 ftmp = tanphi * A[i][j];
309
310 // apply the jacobi rotation to diagonal elements [i][i] and [j][j] stored in the eigenvalue array
311 // eigval[i] = eigval[i] - tan(phi) * A[i][j]
312 eigval[i] -= ftmp;
313
314 // eigval[j] = eigval[j] + tan(phi) * A[i][j]
315 eigval[j] += ftmp;
316
317 // by definition, applying the jacobi rotation on element i, j results in 0.0
318 A[i][j] = 0.0F;
319
320 // apply the jacobi rotation to all elements of the eigenvector matrix
321 for (k = 0; k < n; k++)
322 {
323 // store eigvec[k][i]
324 ftmp = eigvec[k][i];
325
326 // eigvec[k][i] = eigvec[k][i] - sin(phi) * (eigvec[k][j] + tan(phi/2) * eigvec[k][i])
327 eigvec[k][i] = ftmp - sinphi * (eigvec[k][j] + tanhalfphi * ftmp);
328
329 // eigvec[k][j] = eigvec[k][j] + sin(phi) * (eigvec[k][i] - tan(phi/2) * eigvec[k][j])
330 eigvec[k][j] = eigvec[k][j] + sinphi * (ftmp - tanhalfphi * eigvec[k][j]);
331 }
332
333 // apply the jacobi rotation only to those elements of matrix m that can change
334 for (k = 0; k <= i - 1; k++)
335 {
336 // store A[k][i]
337 ftmp = A[k][i];
338
339 // A[k][i] = A[k][i] - sin(phi) * (A[k][j] + tan(phi/2) * A[k][i])
340 A[k][i] = ftmp - sinphi * (A[k][j] + tanhalfphi * ftmp);
341
342 // A[k][j] = A[k][j] + sin(phi) * (A[k][i] - tan(phi/2) * A[k][j])
343 A[k][j] = A[k][j] + sinphi * (ftmp - tanhalfphi * A[k][j]);
344 }
345
346 for (k = i + 1; k <= j - 1; k++)
347 {
348 // store A[i][k]
349 ftmp = A[i][k];
350
351 // A[i][k] = A[i][k] - sin(phi) * (A[k][j] + tan(phi/2) * A[i][k])
352 A[i][k] = ftmp - sinphi * (A[k][j] + tanhalfphi * ftmp);
353
354 // A[k][j] = A[k][j] + sin(phi) * (A[i][k] - tan(phi/2) * A[k][j])
355 A[k][j] = A[k][j] + sinphi * (ftmp - tanhalfphi * A[k][j]);
356 }
357
358 for (k = j + 1; k < n; k++)
359 {
360 // store A[i][k]
361 ftmp = A[i][k];
362
363 // A[i][k] = A[i][k] - sin(phi) * (A[j][k] + tan(phi/2) * A[i][k])
364 A[i][k] = ftmp - sinphi * (A[j][k] + tanhalfphi * ftmp);
365
366 // A[j][k] = A[j][k] + sin(phi) * (A[i][k] - tan(phi/2) * A[j][k])
367 A[j][k] = A[j][k] + sinphi * (ftmp - tanhalfphi * A[j][k]);
368 }
369 } // end of test for matrix element A[i][j] non-zero
370 } // end of loop over columns j
371 } // end of loop over rows i
372 } // end of test for non-zero value of residue
373 } while ((residue > 0.0F) && (ctr++ < NITERATIONS));
374
375 // end of main loop
376 return;
377 }
378
379 // function computes all eigenvalues and eigenvectors of a real symmetric matrix A[0..n-1][0..n-1]
380 // stored in the top left of a 4x4 array A[4][4]
381 // A[][] is changed on output.
382 // eigval[0..n-1] returns the eigenvalues of A[][].
383 // eigvec[0..n-1][0..n-1] returns the normalized eigenvectors of A[][]
384 // the eigenvectors are not sorted by value
385 // n can vary up to and including 4 but the matrices A and eigvec must have 4 columns.
386 // this function is identical to eigencompute10 except for the workaround for 4x4 matrices since C cannot
387
388 // handle functions accepting matrices with variable numbers of columns.
fEigenCompute4(float A[][4],float eigval[],float eigvec[][4],int8 n)389 void fEigenCompute4(float A[][4], float eigval[], float eigvec[][4], int8 n)
390 {
391 // maximum number of iterations to achieve convergence: in practice 6 is typical
392 #define NITERATIONS 15
393 // various trig functions of the jacobi rotation angle phi
394 float cot2phi,
395 tanhalfphi,
396 tanphi,
397 sinphi,
398 cosphi;
399
400 // scratch variable to prevent over-writing during rotations
401 float ftmp;
402
403 // residue from remaining non-zero above diagonal terms
404 float residue;
405
406 // matrix row and column indices
407 int8 ir,
408 ic;
409
410 // general loop counter
411 int8 j;
412
413 // timeout ctr for number of passes of the algorithm
414 int8 ctr;
415
416 // initialize eigenvectors matrix and eigenvalues array
417 for (ir = 0; ir < n; ir++)
418 {
419 // loop over all columns
420 for (ic = 0; ic < n; ic++)
421 {
422 // set on diagonal and off-diagonal elements to zero
423 eigvec[ir][ic] = 0.0F;
424 }
425
426 // correct the diagonal elements to 1.0
427 eigvec[ir][ir] = 1.0F;
428
429 // initialize the array of eigenvalues to the diagonal elements of m
430 eigval[ir] = A[ir][ir];
431 }
432
433 // initialize the counter and loop until converged or NITERATIONS reached
434 ctr = 0;
435 do
436 {
437 // compute the absolute value of the above diagonal elements as exit criterion
438 residue = 0.0F;
439
440 // loop over rows excluding last row
441 for (ir = 0; ir < n - 1; ir++)
442 {
443 // loop over above diagonal columns
444 for (ic = ir + 1; ic < n; ic++)
445 {
446 // accumulate the residual off diagonal terms which are being driven to zero
447 residue += fabsf(A[ir][ic]);
448 }
449 }
450
451 // check if we still have work to do
452 if (residue > 0.0F)
453 {
454 // loop over all rows with the exception of the last row (since only rotating above diagonal elements)
455 for (ir = 0; ir < n - 1; ir++)
456 {
457 // loop over columns ic (where ic is always greater than ir since above diagonal)
458 for (ic = ir + 1; ic < n; ic++)
459 {
460 // only continue with this element if the element is non-zero
461 if (fabsf(A[ir][ic]) > 0.0F)
462 {
463 // calculate cot(2*phi) where phi is the Jacobi rotation angle
464 cot2phi = 0.5F *
465 (eigval[ic] - eigval[ir]) /
466 (A[ir][ic]);
467
468 // calculate tan(phi) correcting sign to ensure the smaller solution is used
469 tanphi = 1.0F / (fabsf(cot2phi) + sqrtf(1.0F + cot2phi * cot2phi));
470 if (cot2phi < 0.0F)
471 {
472 tanphi = -tanphi;
473 }
474
475 // calculate the sine and cosine of the Jacobi rotation angle phi
476 cosphi = 1.0F / sqrtf(1.0F + tanphi * tanphi);
477 sinphi = tanphi * cosphi;
478
479 // calculate tan(phi/2)
480 tanhalfphi = sinphi / (1.0F + cosphi);
481
482 // set tmp = tan(phi) times current matrix element used in update of leading diagonal elements
483 ftmp = tanphi * A[ir][ic];
484
485 // apply the jacobi rotation to diagonal elements [ir][ir] and [ic][ic] stored in the eigenvalue array
486 // eigval[ir] = eigval[ir] - tan(phi) * A[ir][ic]
487 eigval[ir] -= ftmp;
488
489 // eigval[ic] = eigval[ic] + tan(phi) * A[ir][ic]
490 eigval[ic] += ftmp;
491
492 // by definition, applying the jacobi rotation on element ir, ic results in 0.0
493 A[ir][ic] = 0.0F;
494
495 // apply the jacobi rotation to all elements of the eigenvector matrix
496 for (j = 0; j < n; j++)
497 {
498 // store eigvec[j][ir]
499 ftmp = eigvec[j][ir];
500
501 // eigvec[j][ir] = eigvec[j][ir] - sin(phi) * (eigvec[j][ic] + tan(phi/2) * eigvec[j][ir])
502 eigvec[j][ir] = ftmp - sinphi * (eigvec[j][ic] + tanhalfphi * ftmp);
503
504 // eigvec[j][ic] = eigvec[j][ic] + sin(phi) * (eigvec[j][ir] - tan(phi/2) * eigvec[j][ic])
505 eigvec[j][ic] = eigvec[j][ic] + sinphi * (ftmp - tanhalfphi * eigvec[j][ic]);
506 }
507
508 // apply the jacobi rotation only to those elements of matrix m that can change
509 for (j = 0; j <= ir - 1; j++)
510 {
511 // store A[j][ir]
512 ftmp = A[j][ir];
513
514 // A[j][ir] = A[j][ir] - sin(phi) * (A[j][ic] + tan(phi/2) * A[j][ir])
515 A[j][ir] = ftmp - sinphi * (A[j][ic] + tanhalfphi * ftmp);
516
517 // A[j][ic] = A[j][ic] + sin(phi) * (A[j][ir] - tan(phi/2) * A[j][ic])
518 A[j][ic] = A[j][ic] + sinphi * (ftmp - tanhalfphi * A[j][ic]);
519 }
520
521 for (j = ir + 1; j <= ic - 1; j++)
522 {
523 // store A[ir][j]
524 ftmp = A[ir][j];
525
526 // A[ir][j] = A[ir][j] - sin(phi) * (A[j][ic] + tan(phi/2) * A[ir][j])
527 A[ir][j] = ftmp - sinphi * (A[j][ic] + tanhalfphi * ftmp);
528
529 // A[j][ic] = A[j][ic] + sin(phi) * (A[ir][j] - tan(phi/2) * A[j][ic])
530 A[j][ic] = A[j][ic] + sinphi * (ftmp - tanhalfphi * A[j][ic]);
531 }
532
533 for (j = ic + 1; j < n; j++)
534 {
535 // store A[ir][j]
536 ftmp = A[ir][j];
537
538 // A[ir][j] = A[ir][j] - sin(phi) * (A[ic][j] + tan(phi/2) * A[ir][j])
539 A[ir][j] = ftmp - sinphi * (A[ic][j] + tanhalfphi * ftmp);
540
541 // A[ic][j] = A[ic][j] + sin(phi) * (A[ir][j] - tan(phi/2) * A[ic][j])
542 A[ic][j] = A[ic][j] + sinphi * (ftmp - tanhalfphi * A[ic][j]);
543 }
544 } // end of test for matrix element already zero
545 } // end of loop over columns
546 } // end of loop over rows
547 } // end of test for non-zero residue
548 } while ((residue > 0.0F) && (ctr++ < NITERATIONS));
549
550 // end of main loop
551 return;
552 }
553
fComputeEigSlice(float fmatA[10][10],float fmatB[10][10],float fvecA[10],int8 i,int8 j,int8 iMatrixSize)554 void fComputeEigSlice(float fmatA[10][10], float fmatB[10][10], float fvecA[10],
555 int8 i, int8 j, int8 iMatrixSize)
556 {
557 float cot2phi; // cotangent of half jacobi rotation angle
558 float tanhalfphi; // tangent of half jacobi rotation angle
559 float tanphi; // tangent of jacobi rotation angle
560 float sinphi; // sine of jacobi rotation angle
561 float cosphi; // cosine of jacobi rotation angle
562 float ftmp; // scratch
563 int8 k; // loop counter
564
565 // calculate cot(2*phi) where phi is the Jacobi rotation angle
566 cot2phi = 0.5F * (fvecA[j] - fvecA[i]) / (fmatA[i][j]);
567
568 // calculate tan(phi) correcting sign to ensure the smaller solution is used
569 tanphi = 1.0F / (fabsf(cot2phi) + sqrtf(1.0F + cot2phi * cot2phi));
570 if (cot2phi < 0.0F) tanphi = -tanphi;
571
572 // calculate the sine and cosine of the Jacobi rotation angle phi
573 cosphi = 1.0F / sqrtf(1.0F + tanphi * tanphi);
574 sinphi = tanphi * cosphi;
575
576 // calculate tan(phi/2)
577 tanhalfphi = sinphi / (1.0F + cosphi);
578
579 // set tmp = tan(phi) times current matrix element used in update of leading diagonal elements
580 ftmp = tanphi * fmatA[i][j];
581
582 // apply the jacobi rotation to diagonal elements [i][i] and [j][j] stored in the eigenvalue array
583 // fvecA[i] = fvecA[i] - tan(phi) * fmatA[i][j]
584 fvecA[i] -= ftmp;
585
586 // fvecA[j] = fvecA[j] + tan(phi) * fmatA[i][j]
587 fvecA[j] += ftmp;
588
589 // by definition, applying the jacobi rotation on element i, j results in 0.0
590 fmatA[i][j] = 0.0F;
591
592 // apply the jacobi rotation to all elements of the eigenvector matrix
593 for (k = 0; k < iMatrixSize; k++)
594 {
595 // store fmatB[k][i]
596 ftmp = fmatB[k][i];
597
598 // fmatB[k][i] = fmatB[k][i] - sin(phi) * (fmatB[k][j] + tan(phi/2) * fmatB[k][i])
599 fmatB[k][i] = ftmp - sinphi * (fmatB[k][j] + tanhalfphi * ftmp);
600
601 // fmatB[k][j] = fmatB[k][j] + sin(phi) * (fmatB[k][i] - tan(phi/2) * fmatB[k][j])
602 fmatB[k][j] = fmatB[k][j] + sinphi * (ftmp - tanhalfphi * fmatB[k][j]);
603 }
604
605 // apply the jacobi rotation only to those elements of matrix that can change
606 for (k = 0; k <= i - 1; k++)
607 {
608 // store fmatA[k][i]
609 ftmp = fmatA[k][i];
610
611 // fmatA[k][i] = fmatA[k][i] - sin(phi) * (fmatA[k][j] + tan(phi/2) * fmatA[k][i])
612 fmatA[k][i] = ftmp - sinphi * (fmatA[k][j] + tanhalfphi * ftmp);
613
614 // fmatA[k][j] = fmatA[k][j] + sin(phi) * (fmatA[k][i] - tan(phi/2) * fmatA[k][j])
615 fmatA[k][j] = fmatA[k][j] + sinphi * (ftmp - tanhalfphi * fmatA[k][j]);
616 }
617
618 for (k = i + 1; k <= j - 1; k++)
619 {
620 // store fmatA[i][k]
621 ftmp = fmatA[i][k];
622
623 // fmatA[i][k] = fmatA[i][k] - sin(phi) * (fmatA[k][j] + tan(phi/2) * fmatA[i][k])
624 fmatA[i][k] = ftmp - sinphi * (fmatA[k][j] + tanhalfphi * ftmp);
625
626 // fmatA[k][j] = fmatA[k][j] + sin(phi) * (fmatA[i][k] - tan(phi/2) * fmatA[k][j])
627 fmatA[k][j] = fmatA[k][j] + sinphi * (ftmp - tanhalfphi * fmatA[k][j]);
628 }
629
630 for (k = j + 1; k < iMatrixSize; k++)
631 {
632 // store fmatA[i][k]
633 ftmp = fmatA[i][k];
634
635 // fmatA[i][k] = fmatA[i][k] - sin(phi) * (fmatA[j][k] + tan(phi/2) * fmatA[i][k])
636 fmatA[i][k] = ftmp - sinphi * (fmatA[j][k] + tanhalfphi * ftmp);
637
638 // fmatA[j][k] = fmatA[j][k] + sin(phi) * (fmatA[i][k] - tan(phi/2) * fmatA[j][k])
639 fmatA[j][k] = fmatA[j][k] + sinphi * (ftmp - tanhalfphi * fmatA[j][k]);
640 }
641
642 return;
643 }
644
645 // function uses Gauss-Jordan elimination to compute the inverse of matrix A in situ
646
647 // on exit, A is replaced with its inverse
fmatrixAeqInvA(float * A[],int8 iColInd[],int8 iRowInd[],int8 iPivot[],int8 isize,int8 * pierror)648 void fmatrixAeqInvA(float *A[], int8 iColInd[], int8 iRowInd[], int8 iPivot[],
649 int8 isize, int8 *pierror)
650 {
651 float largest; // largest element used for pivoting
652 float scaling; // scaling factor in pivoting
653 float recippiv; // reciprocal of pivot element
654 float ftmp; // temporary variable used in swaps
655 int8 i,
656 j,
657 k,
658 l,
659 m; // index counters
660 int8 iPivotRow,
661 iPivotCol; // row and column of pivot element
662
663 // to avoid compiler warnings
664 iPivotRow = iPivotCol = 0;
665
666 // default to successful inversion
667 *pierror = false;
668
669 // initialize the pivot array to 0
670 for (j = 0; j < isize; j++)
671 {
672 iPivot[j] = 0;
673 }
674
675 // main loop i over the dimensions of the square matrix A
676 for (i = 0; i < isize; i++)
677 {
678 // zero the largest element found for pivoting
679 largest = 0.0F;
680
681 // loop over candidate rows j
682 for (j = 0; j < isize; j++)
683 {
684 // check if row j has been previously pivoted
685 if (iPivot[j] != 1)
686 {
687 // loop over candidate columns k
688 for (k = 0; k < isize; k++)
689 {
690 // check if column k has previously been pivoted
691 if (iPivot[k] == 0)
692 {
693 // check if the pivot element is the largest found so far
694 if (fabsf(A[j][k]) >= largest)
695 {
696 // and store this location as the current best candidate for pivoting
697 iPivotRow = j;
698 iPivotCol = k;
699 largest = (float) fabsf(A[iPivotRow][iPivotCol]);
700 }
701 }
702 else if (iPivot[k] > 1)
703 {
704 // zero determinant situation: exit with identity matrix and set error flag
705 fmatrixAeqI(A, isize);
706 *pierror = true;
707 return;
708 }
709 }
710 }
711 }
712
713 // increment the entry in iPivot to denote it has been selected for pivoting
714 iPivot[iPivotCol]++;
715
716 // check the pivot rows iPivotRow and iPivotCol are not the same before swapping
717 if (iPivotRow != iPivotCol)
718 {
719 // loop over columns l
720 for (l = 0; l < isize; l++)
721 {
722 // and swap all elements of rows iPivotRow and iPivotCol
723 ftmp = A[iPivotRow][l];
724 A[iPivotRow][l] = A[iPivotCol][l];
725 A[iPivotCol][l] = ftmp;
726 }
727 }
728
729 // record that on the i-th iteration rows iPivotRow and iPivotCol were swapped
730 iRowInd[i] = iPivotRow;
731 iColInd[i] = iPivotCol;
732
733 // check for zero on-diagonal element (singular matrix) and return with identity matrix if detected
734 if (A[iPivotCol][iPivotCol] == 0.0F)
735 {
736 // zero determinant situation: exit with identity matrix and set error flag
737 fmatrixAeqI(A, isize);
738 *pierror = true;
739 return;
740 }
741
742 // calculate the reciprocal of the pivot element knowing it's non-zero
743 recippiv = 1.0F / A[iPivotCol][iPivotCol];
744
745 // by definition, the diagonal element normalizes to 1
746 A[iPivotCol][iPivotCol] = 1.0F;
747
748 // multiply all of row iPivotCol by the reciprocal of the pivot element including the diagonal element
749 // the diagonal element A[iPivotCol][iPivotCol] now has value equal to the reciprocal of its previous value
750 for (l = 0; l < isize; l++)
751 {
752 if (A[iPivotCol][l] != 0.0F) A[iPivotCol][l] *= recippiv;
753 }
754
755 // loop over all rows m of A
756 for (m = 0; m < isize; m++)
757 {
758 if (m != iPivotCol)
759 {
760 // scaling factor for this row m is in column iPivotCol
761 scaling = A[m][iPivotCol];
762
763 // zero this element
764 A[m][iPivotCol] = 0.0F;
765
766 // loop over all columns l of A and perform elimination
767 for (l = 0; l < isize; l++)
768 {
769 if ((A[iPivotCol][l] != 0.0F) && (scaling != 0.0F))
770 A[m][l] -= A[iPivotCol][l] * scaling;
771 }
772 }
773 }
774 } // end of loop i over the matrix dimensions
775
776 // finally, loop in inverse order to apply the missing column swaps
777 for (l = isize - 1; l >= 0; l--)
778 {
779 // set i and j to the two columns to be swapped
780 i = iRowInd[l];
781 j = iColInd[l];
782
783 // check that the two columns i and j to be swapped are not the same
784 if (i != j)
785 {
786 // loop over all rows k to swap columns i and j of A
787 for (k = 0; k < isize; k++)
788 {
789 ftmp = A[k][i];
790 A[k][i] = A[k][j];
791 A[k][j] = ftmp;
792 }
793 }
794 }
795
796 return;
797 }
798
799 // function rotates 3x1 vector u onto 3x1 vector using 3x3 rotation matrix fR.
800
801 // the rotation is applied in the inverse direction if itranpose is true
fveqRu(float fv[],float fR[][3],float fu[],int8 itranspose)802 void fveqRu(float fv[], float fR[][3], float fu[], int8 itranspose)
803 {
804 if (!itranspose)
805 {
806 // normal, non-transposed rotation
807 fv[CHX] = fR[CHX][CHX] *
808 fu[CHX] +
809 fR[CHX][CHY] *
810 fu[CHY] +
811 fR[CHX][CHZ] *
812 fu[CHZ];
813 fv[CHY] = fR[CHY][CHX] *
814 fu[CHX] +
815 fR[CHY][CHY] *
816 fu[CHY] +
817 fR[CHY][CHZ] *
818 fu[CHZ];
819 fv[CHZ] = fR[CHZ][CHX] *
820 fu[CHX] +
821 fR[CHZ][CHY] *
822 fu[CHY] +
823 fR[CHZ][CHZ] *
824 fu[CHZ];
825 }
826 else
827 {
828 // transposed (inverse rotation)
829 fv[CHX] = fR[CHX][CHX] *
830 fu[CHX] +
831 fR[CHY][CHX] *
832 fu[CHY] +
833 fR[CHZ][CHX] *
834 fu[CHZ];
835 fv[CHY] = fR[CHX][CHY] *
836 fu[CHX] +
837 fR[CHY][CHY] *
838 fu[CHY] +
839 fR[CHZ][CHY] *
840 fu[CHZ];
841 fv[CHZ] = fR[CHX][CHZ] *
842 fu[CHX] +
843 fR[CHY][CHZ] *
844 fu[CHY] +
845 fR[CHZ][CHZ] *
846 fu[CHZ];
847 }
848
849 return;
850 }
851
852 // function multiplies the 3x1 vector V by a 3x3 matrix A
fVeq3x3AxV(float V[3],float A[][3])853 void fVeq3x3AxV(float V[3], float A[][3])
854 {
855 float ftmp[3]; // scratch vector
856
857 // set ftmp = V
858 ftmp[CHX] = V[CHX];
859 ftmp[CHY] = V[CHY];
860 ftmp[CHZ] = V[CHZ];
861
862 // set V = A * ftmp = A * V
863 V[CHX] = A[CHX][CHX] *
864 ftmp[CHX] +
865 A[CHX][CHY] *
866 ftmp[CHY] +
867 A[CHX][CHZ] *
868 ftmp[CHZ];
869 V[CHY] = A[CHY][CHX] *
870 ftmp[CHX] +
871 A[CHY][CHY] *
872 ftmp[CHY] +
873 A[CHY][CHZ] *
874 ftmp[CHZ];
875 V[CHZ] = A[CHZ][CHX] *
876 ftmp[CHX] +
877 A[CHZ][CHY] *
878 ftmp[CHY] +
879 A[CHZ][CHZ] *
880 ftmp[CHZ];
881
882 return;
883 }
884
