ISF  2.2 rev 5
Intelligent Sensing Framework for Kinetis with Processor Expert
matrix.c
Go to the documentation of this file.
1 // Copyright (c) 2015, Freescale Semiconductor, Inc.
2 // All rights reserved.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 // * Redistributions of source code must retain the above copyright
7 // notice, this list of conditions and the following disclaimer.
8 // * Redistributions in binary form must reproduce the above copyright
9 // notice, this list of conditions and the following disclaimer in the
10 // documentation and/or other materials provided with the distribution.
11 // * Neither the name of Freescale Semiconductor, Inc. nor the
12 // names of its contributors may be used to endorse or promote products
13 // derived from this software without specific prior written permission.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
16 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
17 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18 // DISCLAIMED. IN NO EVENT SHALL FREESCALE SEMICONDUCTOR, INC. BE LIABLE FOR ANY
19 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
20 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
22 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
24 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25 //
26 // This file contains matrix manipulation functions.
27 //
28 
29 #include "math.h"
30 #include "matrix.h"
31 
32 // compile time constants that are private to this file
33 #define CORRUPTMATRIX 0.001F // column vector modulus limit for rotation matrix
34 
35 // function sets the 3x3 matrix A to the identity matrix
36 void f3x3matrixAeqI(float A[][3])
37 {
38  float *pAij; // pointer to A[i][j]
39  int8 i, j; // loop counters
40 
41  for (i = 0; i < 3; i++)
42  {
43  // set pAij to &A[i][j=0]
44  pAij = A[i];
45  for (j = 0; j < 3; j++)
46  {
47  *(pAij++) = 0.0F;
48  }
49  A[i][i] = 1.0F;
50  }
51  return;
52 }
53 
54 // function sets 3x3 matrix A to 3x3 matrix B
55 void f3x3matrixAeqB(float A[][3], float B[][3])
56 {
57  float *pAij; // pointer to A[i][j]
58  float *pBij; // pointer to B[i][j]
59  int8 i, j; // loop counters
60 
61  for (i = 0; i < 3; i++)
62  {
63  // set pAij to &A[i][j=0] and pBij to &B[i][j=0]
64  pAij = A[i];
65  pBij = B[i];
66  for (j = 0; j < 3; j++)
67  {
68  *(pAij++) = *(pBij++);
69  }
70  }
71  return;
72 }
73 
74 // function sets the matrix A to the identity matrix
75 void fmatrixAeqI(float *A[], int16 rc)
76 {
77  // rc = rows and columns in A
78 
79  float *pAij; // pointer to A[i][j]
80  int8 i, j; // loop counters
81 
82  for (i = 0; i < rc; i++)
83  {
84  // set pAij to &A[i][j=0]
85  pAij = A[i];
86  for (j = 0; j < rc; j++)
87  {
88  *(pAij++) = 0.0F;
89  }
90  A[i][i] = 1.0F;
91  }
92  return;
93 }
94 
95 // function sets every entry in the 3x3 matrix A to a constant scalar
96 void f3x3matrixAeqScalar(float A[][3], float Scalar)
97 {
98  float *pAij; // pointer to A[i][j]
99  int8 i, j; // counters
100 
101  for (i = 0; i < 3; i++)
102  {
103  // set pAij to &A[i][j=0]
104  pAij = A[i];
105  for (j = 0; j < 3; j++)
106  {
107  *(pAij++) = Scalar;
108  }
109  }
110  return;
111 }
112 
113 // function multiplies all elements of 3x3 matrix A by the specified scalar
114 void f3x3matrixAeqAxScalar(float A[][3], float Scalar)
115 {
116  float *pAij; // pointer to A[i][j]
117  int8 i, j; // loop counters
118 
119  for (i = 0; i < 3; i++)
120  {
121  // set pAij to &A[i][j=0]
122  pAij = A[i];
123  for (j = 0; j < 3; j++)
124  {
125  *(pAij++) *= Scalar;
126  }
127  }
128 
129  return;
130 }
131 
132 // function negates all elements of 3x3 matrix A
133 void f3x3matrixAeqMinusA(float A[][3])
134 {
135  float *pAij; // pointer to A[i][j]
136  int8 i, j; // loop counters
137 
138  for (i = 0; i < 3; i++)
139  {
140  // set pAij to &A[i][j=0]
141  pAij = A[i];
142  for (j = 0; j < 3; j++)
143  {
144  *pAij = -*pAij;
145  pAij++;
146  }
147  }
148 
149  return;
150 }
151 
152 // function directly calculates the symmetric inverse of a symmetric 3x3 matrix
153 // only the on and above diagonal terms in B are used and need to be specified
154 void f3x3matrixAeqInvSymB(float A[][3], float B[][3])
155 {
156  float fB11B22mB12B12; // B[1][1] * B[2][2] - B[1][2] * B[1][2]
157  float fB12B02mB01B22; // B[1][2] * B[0][2] - B[0][1] * B[2][2]
158  float fB01B12mB11B02; // B[0][1] * B[1][2] - B[1][1] * B[0][2]
159  float ftmp; // determinant and then reciprocal
160 
161  // calculate useful products
162  fB11B22mB12B12 = B[1][1] * B[2][2] - B[1][2] * B[1][2];
163  fB12B02mB01B22 = B[1][2] * B[0][2] - B[0][1] * B[2][2];
164  fB01B12mB11B02 = B[0][1] * B[1][2] - B[1][1] * B[0][2];
165 
166  // set ftmp to the determinant of the input matrix B
167  ftmp = B[0][0] * fB11B22mB12B12 + B[0][1] * fB12B02mB01B22 + B[0][2] * fB01B12mB11B02;
168 
169  // set A to the inverse of B for any determinant except zero
170  if (ftmp != 0.0F)
171  {
172  ftmp = 1.0F / ftmp;
173  A[0][0] = fB11B22mB12B12 * ftmp;
174  A[1][0] = A[0][1] = fB12B02mB01B22 * ftmp;
175  A[2][0] = A[0][2] = fB01B12mB11B02 * ftmp;
176  A[1][1] = (B[0][0] * B[2][2] - B[0][2] * B[0][2]) * ftmp;
177  A[2][1] = A[1][2] = (B[0][2] * B[0][1] - B[0][0] * B[1][2]) * ftmp;
178  A[2][2] = (B[0][0] * B[1][1] - B[0][1] * B[0][1]) * ftmp;
179  }
180  else
181  {
182  // provide the identity matrix if the determinant is zero
183  f3x3matrixAeqI(A);
184  }
185  return;
186 }
187 
188 // function calculates the determinant of a 3x3 matrix
189 float f3x3matrixDetA(float A[][3])
190 {
191  return (A[X][X] * (A[Y][Y] * A[Z][Z] - A[Y][Z] * A[Z][Y]) +
192  A[X][Y] * (A[Y][Z] * A[Z][X] - A[Y][X] * A[Z][Z]) +
193  A[X][Z] * (A[Y][X] * A[Z][Y] - A[Y][Y] * A[Z][X]));
194 }
195 
196 // function computes all eigenvalues and eigenvectors of a real symmetric matrix A[0..n-1][0..n-1]
197 // stored in the top left of a 10x10 array A[10][10]
198 // A[][] is changed on output.
199 // eigval[0..n-1] returns the eigenvalues of A[][].
200 // eigvec[0..n-1][0..n-1] returns the normalized eigenvectors of A[][]
201 // the eigenvectors are not sorted by value
202 // n can vary up to and including 10 but the matrices A and eigvec must have 10 columns.
203 void eigencompute10(float A[][10], float eigval[], float eigvec[][10], int8 n)
204 {
205  // maximum number of iterations to achieve convergence: in practice 6 is typical
206 #define NITERATIONS 15
207 
208  // various trig functions of the jacobi rotation angle phi
209  float cot2phi, tanhalfphi, tanphi, sinphi, cosphi;
210  // scratch variable to prevent over-writing during rotations
211  float ftmp;
212  // residue from remaining non-zero above diagonal terms
213  float residue;
214  // matrix row and column indices
215  int8 ir, ic;
216  // general loop counter
217  int8 j;
218  // timeout ctr for number of passes of the algorithm
219  int8 ctr;
220 
221  // initialize eigenvectors matrix and eigenvalues array
222  for (ir = 0; ir < n; ir++)
223  {
224  // loop over all columns
225  for (ic = 0; ic < n; ic++)
226  {
227  // set on diagonal and off-diagonal elements to zero
228  eigvec[ir][ic] = 0.0F;
229  }
230 
231  // correct the diagonal elements to 1.0
232  eigvec[ir][ir] = 1.0F;
233 
234  // initialize the array of eigenvalues to the diagonal elements of m
235  eigval[ir] = A[ir][ir];
236  }
237 
238  // initialize the counter and loop until converged or NITERATIONS reached
239  ctr = 0;
240  do
241  {
242  // compute the absolute value of the above diagonal elements as exit criterion
243  residue = 0.0F;
244  // loop over rows excluding last row
245  for (ir = 0; ir < n - 1; ir++)
246  {
247  // loop over above diagonal columns
248  for (ic = ir + 1; ic < n; ic++)
249  {
250  // accumulate the residual off diagonal terms which are being driven to zero
251  residue += fabsf(A[ir][ic]);
252  }
253  }
254 
255  // check if we still have work to do
256  if (residue > 0.0F)
257  {
258  // loop over all rows with the exception of the last row (since only rotating above diagonal elements)
259  for (ir = 0; ir < n - 1; ir++)
260  {
261  // loop over columns ic (where ic is always greater than ir since above diagonal)
262  for (ic = ir + 1; ic < n; ic++)
263  {
264  // only continue with this element if the element is non-zero
265  if (fabsf(A[ir][ic]) > 0.0F)
266  {
267  // calculate cot(2*phi) where phi is the Jacobi rotation angle
268  cot2phi = 0.5F * (eigval[ic] - eigval[ir]) / (A[ir][ic]);
269 
270  // calculate tan(phi) correcting sign to ensure the smaller solution is used
271  tanphi = 1.0F / (fabsf(cot2phi) + sqrtf(1.0F + cot2phi * cot2phi));
272  if (cot2phi < 0.0F)
273  {
274  tanphi = -tanphi;
275  }
276 
277  // calculate the sine and cosine of the Jacobi rotation angle phi
278  cosphi = 1.0F / sqrtf(1.0F + tanphi * tanphi);
279  sinphi = tanphi * cosphi;
280 
281  // calculate tan(phi/2)
282  tanhalfphi = sinphi / (1.0F + cosphi);
283 
284  // set tmp = tan(phi) times current matrix element used in update of leading diagonal elements
285  ftmp = tanphi * A[ir][ic];
286 
287  // apply the jacobi rotation to diagonal elements [ir][ir] and [ic][ic] stored in the eigenvalue array
288  // eigval[ir] = eigval[ir] - tan(phi) * A[ir][ic]
289  eigval[ir] -= ftmp;
290  // eigval[ic] = eigval[ic] + tan(phi) * A[ir][ic]
291  eigval[ic] += ftmp;
292 
293  // by definition, applying the jacobi rotation on element ir, ic results in 0.0
294  A[ir][ic] = 0.0F;
295 
296  // apply the jacobi rotation to all elements of the eigenvector matrix
297  for (j = 0; j < n; j++)
298  {
299  // store eigvec[j][ir]
300  ftmp = eigvec[j][ir];
301  // eigvec[j][ir] = eigvec[j][ir] - sin(phi) * (eigvec[j][ic] + tan(phi/2) * eigvec[j][ir])
302  eigvec[j][ir] = ftmp - sinphi * (eigvec[j][ic] + tanhalfphi * ftmp);
303  // eigvec[j][ic] = eigvec[j][ic] + sin(phi) * (eigvec[j][ir] - tan(phi/2) * eigvec[j][ic])
304  eigvec[j][ic] = eigvec[j][ic] + sinphi * (ftmp - tanhalfphi * eigvec[j][ic]);
305  }
306 
307  // apply the jacobi rotation only to those elements of matrix m that can change
308  for (j = 0; j <= ir - 1; j++)
309  {
310  // store A[j][ir]
311  ftmp = A[j][ir];
312  // A[j][ir] = A[j][ir] - sin(phi) * (A[j][ic] + tan(phi/2) * A[j][ir])
313  A[j][ir] = ftmp - sinphi * (A[j][ic] + tanhalfphi * ftmp);
314  // A[j][ic] = A[j][ic] + sin(phi) * (A[j][ir] - tan(phi/2) * A[j][ic])
315  A[j][ic] = A[j][ic] + sinphi * (ftmp - tanhalfphi * A[j][ic]);
316  }
317  for (j = ir + 1; j <= ic - 1; j++)
318  {
319  // store A[ir][j]
320  ftmp = A[ir][j];
321  // A[ir][j] = A[ir][j] - sin(phi) * (A[j][ic] + tan(phi/2) * A[ir][j])
322  A[ir][j] = ftmp - sinphi * (A[j][ic] + tanhalfphi * ftmp);
323  // A[j][ic] = A[j][ic] + sin(phi) * (A[ir][j] - tan(phi/2) * A[j][ic])
324  A[j][ic] = A[j][ic] + sinphi * (ftmp - tanhalfphi * A[j][ic]);
325  }
326  for (j = ic + 1; j < n; j++)
327  {
328  // store A[ir][j]
329  ftmp = A[ir][j];
330  // A[ir][j] = A[ir][j] - sin(phi) * (A[ic][j] + tan(phi/2) * A[ir][j])
331  A[ir][j] = ftmp - sinphi * (A[ic][j] + tanhalfphi * ftmp);
332  // A[ic][j] = A[ic][j] + sin(phi) * (A[ir][j] - tan(phi/2) * A[ic][j])
333  A[ic][j] = A[ic][j] + sinphi * (ftmp - tanhalfphi * A[ic][j]);
334  }
335  } // end of test for matrix element already zero
336  } // end of loop over columns
337  } // end of loop over rows
338  } // end of test for non-zero residue
339  } while ((residue > 0.0F) && (ctr++ < NITERATIONS)); // end of main loop
340 
341  return;
342 }
343 
344 // function computes all eigenvalues and eigenvectors of a real symmetric matrix A[0..n-1][0..n-1]
345 // stored in the top left of a 4x4 array A[4][4]
346 // A[][] is changed on output.
347 // eigval[0..n-1] returns the eigenvalues of A[][].
348 // eigvec[0..n-1][0..n-1] returns the normalized eigenvectors of A[][]
349 // the eigenvectors are not sorted by value
350 // n can vary up to and including 4 but the matrices A and eigvec must have 4 columns.
351 // this function is identical to eigencompute10 except for the workaround for 4x4 matrices since C cannot
352 // handle functions accepting matrices with variable numbers of columns.
353 void eigencompute4(float A[][4], float eigval[], float eigvec[][4], int8 n)
354 {
355  // maximum number of iterations to achieve convergence: in practice 6 is typical
356 #define NITERATIONS 15
357 
358  // various trig functions of the jacobi rotation angle phi
359  float cot2phi, tanhalfphi, tanphi, sinphi, cosphi;
360  // scratch variable to prevent over-writing during rotations
361  float ftmp;
362  // residue from remaining non-zero above diagonal terms
363  float residue;
364  // matrix row and column indices
365  int8 ir, ic;
366  // general loop counter
367  int8 j;
368  // timeout ctr for number of passes of the algorithm
369  int8 ctr;
370 
371  // initialize eigenvectors matrix and eigenvalues array
372  for (ir = 0; ir < n; ir++)
373  {
374  // loop over all columns
375  for (ic = 0; ic < n; ic++)
376  {
377  // set on diagonal and off-diagonal elements to zero
378  eigvec[ir][ic] = 0.0F;
379  }
380 
381  // correct the diagonal elements to 1.0
382  eigvec[ir][ir] = 1.0F;
383 
384  // initialize the array of eigenvalues to the diagonal elements of m
385  eigval[ir] = A[ir][ir];
386  }
387 
388  // initialize the counter and loop until converged or NITERATIONS reached
389  ctr = 0;
390  do
391  {
392  // compute the absolute value of the above diagonal elements as exit criterion
393  residue = 0.0F;
394  // loop over rows excluding last row
395  for (ir = 0; ir < n - 1; ir++)
396  {
397  // loop over above diagonal columns
398  for (ic = ir + 1; ic < n; ic++)
399  {
400  // accumulate the residual off diagonal terms which are being driven to zero
401  residue += fabsf(A[ir][ic]);
402  }
403  }
404 
405  // check if we still have work to do
406  if (residue > 0.0F)
407  {
408  // loop over all rows with the exception of the last row (since only rotating above diagonal elements)
409  for (ir = 0; ir < n - 1; ir++)
410  {
411  // loop over columns ic (where ic is always greater than ir since above diagonal)
412  for (ic = ir + 1; ic < n; ic++)
413  {
414  // only continue with this element if the element is non-zero
415  if (fabsf(A[ir][ic]) > 0.0F)
416  {
417  // calculate cot(2*phi) where phi is the Jacobi rotation angle
418  cot2phi = 0.5F * (eigval[ic] - eigval[ir]) / (A[ir][ic]);
419 
420  // calculate tan(phi) correcting sign to ensure the smaller solution is used
421  tanphi = 1.0F / (fabsf(cot2phi) + sqrtf(1.0F + cot2phi * cot2phi));
422  if (cot2phi < 0.0F)
423  {
424  tanphi = -tanphi;
425  }
426 
427  // calculate the sine and cosine of the Jacobi rotation angle phi
428  cosphi = 1.0F / sqrtf(1.0F + tanphi * tanphi);
429  sinphi = tanphi * cosphi;
430 
431  // calculate tan(phi/2)
432  tanhalfphi = sinphi / (1.0F + cosphi);
433 
434  // set tmp = tan(phi) times current matrix element used in update of leading diagonal elements
435  ftmp = tanphi * A[ir][ic];
436 
437  // apply the jacobi rotation to diagonal elements [ir][ir] and [ic][ic] stored in the eigenvalue array
438  // eigval[ir] = eigval[ir] - tan(phi) * A[ir][ic]
439  eigval[ir] -= ftmp;
440  // eigval[ic] = eigval[ic] + tan(phi) * A[ir][ic]
441  eigval[ic] += ftmp;
442 
443  // by definition, applying the jacobi rotation on element ir, ic results in 0.0
444  A[ir][ic] = 0.0F;
445 
446  // apply the jacobi rotation to all elements of the eigenvector matrix
447  for (j = 0; j < n; j++)
448  {
449  // store eigvec[j][ir]
450  ftmp = eigvec[j][ir];
451  // eigvec[j][ir] = eigvec[j][ir] - sin(phi) * (eigvec[j][ic] + tan(phi/2) * eigvec[j][ir])
452  eigvec[j][ir] = ftmp - sinphi * (eigvec[j][ic] + tanhalfphi * ftmp);
453  // eigvec[j][ic] = eigvec[j][ic] + sin(phi) * (eigvec[j][ir] - tan(phi/2) * eigvec[j][ic])
454  eigvec[j][ic] = eigvec[j][ic] + sinphi * (ftmp - tanhalfphi * eigvec[j][ic]);
455  }
456 
457  // apply the jacobi rotation only to those elements of matrix m that can change
458  for (j = 0; j <= ir - 1; j++)
459  {
460  // store A[j][ir]
461  ftmp = A[j][ir];
462  // A[j][ir] = A[j][ir] - sin(phi) * (A[j][ic] + tan(phi/2) * A[j][ir])
463  A[j][ir] = ftmp - sinphi * (A[j][ic] + tanhalfphi * ftmp);
464  // A[j][ic] = A[j][ic] + sin(phi) * (A[j][ir] - tan(phi/2) * A[j][ic])
465  A[j][ic] = A[j][ic] + sinphi * (ftmp - tanhalfphi * A[j][ic]);
466  }
467  for (j = ir + 1; j <= ic - 1; j++)
468  {
469  // store A[ir][j]
470  ftmp = A[ir][j];
471  // A[ir][j] = A[ir][j] - sin(phi) * (A[j][ic] + tan(phi/2) * A[ir][j])
472  A[ir][j] = ftmp - sinphi * (A[j][ic] + tanhalfphi * ftmp);
473  // A[j][ic] = A[j][ic] + sin(phi) * (A[ir][j] - tan(phi/2) * A[j][ic])
474  A[j][ic] = A[j][ic] + sinphi * (ftmp - tanhalfphi * A[j][ic]);
475  }
476  for (j = ic + 1; j < n; j++)
477  {
478  // store A[ir][j]
479  ftmp = A[ir][j];
480  // A[ir][j] = A[ir][j] - sin(phi) * (A[ic][j] + tan(phi/2) * A[ir][j])
481  A[ir][j] = ftmp - sinphi * (A[ic][j] + tanhalfphi * ftmp);
482  // A[ic][j] = A[ic][j] + sin(phi) * (A[ir][j] - tan(phi/2) * A[ic][j])
483  A[ic][j] = A[ic][j] + sinphi * (ftmp - tanhalfphi * A[ic][j]);
484  }
485  } // end of test for matrix element already zero
486  } // end of loop over columns
487  } // end of loop over rows
488  } // end of test for non-zero residue
489  } while ((residue > 0.0F) && (ctr++ < NITERATIONS)); // end of main loop
490 
491  return;
492 }
493 
494 // function uses Gauss-Jordan elimination to compute the inverse of matrix A in situ
495 // on exit, A is replaced with its inverse
496 void fmatrixAeqInvA(float *A[], int8 iColInd[], int8 iRowInd[], int8 iPivot[], int8 isize, int8 *pierror)
497 {
498  float largest; // largest element used for pivoting
499  float scaling; // scaling factor in pivoting
500  float recippiv; // reciprocal of pivot element
501  float ftmp; // temporary variable used in swaps
502  int8 i, j, k, l, m; // index counters
503  int8 iPivotRow, iPivotCol; // row and column of pivot element
504 
505  // to avoid compiler warnings
506  iPivotRow = iPivotCol = 0;
507 
508  // default to successful inversion
509  *pierror = false;
510 
511  // initialize the pivot array to 0
512  for (j = 0; j < isize; j++)
513  {
514  iPivot[j] = 0;
515  }
516 
517  // main loop i over the dimensions of the square matrix A
518  for (i = 0; i < isize; i++)
519  {
520  // zero the largest element found for pivoting
521  largest = 0.0F;
522  // loop over candidate rows j
523  for (j = 0; j < isize; j++)
524  {
525  // check if row j has been previously pivoted
526  if (iPivot[j] != 1)
527  {
528  // loop over candidate columns k
529  for (k = 0; k < isize; k++)
530  {
531  // check if column k has previously been pivoted
532  if (iPivot[k] == 0)
533  {
534  // check if the pivot element is the largest found so far
535  if (fabsf(A[j][k]) >= largest)
536  {
537  // and store this location as the current best candidate for pivoting
538  iPivotRow = j;
539  iPivotCol = k;
540  largest = (float) fabsf(A[iPivotRow][iPivotCol]);
541  }
542  }
543  else if (iPivot[k] > 1)
544  {
545  // zero determinant situation: exit with identity matrix and set error flag
546  fmatrixAeqI(A, isize);
547  *pierror = true;
548  return;
549  }
550  }
551  }
552  }
553  // increment the entry in iPivot to denote it has been selected for pivoting
554  iPivot[iPivotCol]++;
555 
556  // check the pivot rows iPivotRow and iPivotCol are not the same before swapping
557  if (iPivotRow != iPivotCol)
558  {
559  // loop over columns l
560  for (l = 0; l < isize; l++)
561  {
562  // and swap all elements of rows iPivotRow and iPivotCol
563  ftmp = A[iPivotRow][l];
564  A[iPivotRow][l] = A[iPivotCol][l];
565  A[iPivotCol][l] = ftmp;
566  }
567  }
568 
569  // record that on the i-th iteration rows iPivotRow and iPivotCol were swapped
570  iRowInd[i] = iPivotRow;
571  iColInd[i] = iPivotCol;
572 
573  // check for zero on-diagonal element (singular matrix) and return with identity matrix if detected
574  if (A[iPivotCol][iPivotCol] == 0.0F)
575  {
576  // zero determinant situation: exit with identity matrix and set error flag
577  fmatrixAeqI(A, isize);
578  *pierror = true;
579  return;
580  }
581 
582  // calculate the reciprocal of the pivot element knowing it's non-zero
583  recippiv = 1.0F / A[iPivotCol][iPivotCol];
584  // by definition, the diagonal element normalizes to 1
585  A[iPivotCol][iPivotCol] = 1.0F;
586  // multiply all of row iPivotCol by the reciprocal of the pivot element including the diagonal element
587  // the diagonal element A[iPivotCol][iPivotCol] now has value equal to the reciprocal of its previous value
588  for (l = 0; l < isize; l++)
589  {
590  if (A[iPivotCol][l] != 0.0F)
591  A[iPivotCol][l] *= recippiv;
592  }
593  // loop over all rows m of A
594  for (m = 0; m < isize; m++)
595  {
596  if (m != iPivotCol)
597  {
598  // scaling factor for this row m is in column iPivotCol
599  scaling = A[m][iPivotCol];
600  // zero this element
601  A[m][iPivotCol] = 0.0F;
602  // loop over all columns l of A and perform elimination
603  for (l = 0; l < isize; l++)
604  {
605  if ((A[iPivotCol][l] != 0.0F) && (scaling != 0.0F))
606  A[m][l] -= A[iPivotCol][l] * scaling;
607  }
608  }
609  }
610  } // end of loop i over the matrix dimensions
611 
612  // finally, loop in inverse order to apply the missing column swaps
613  for (l = isize - 1; l >= 0; l--)
614  {
615  // set i and j to the two columns to be swapped
616  i = iRowInd[l];
617  j = iColInd[l];
618 
619  // check that the two columns i and j to be swapped are not the same
620  if (i != j)
621  {
622  // loop over all rows k to swap columns i and j of A
623  for (k = 0; k < isize; k++)
624  {
625  ftmp = A[k][i];
626  A[k][i] = A[k][j];
627  A[k][j] = ftmp;
628  }
629  }
630  }
631 
632  return;
633 }
634 
635 // function re-orthonormalizes a 3x3 rotation matrix
636 void fmatrixAeqRenormRotA(float A[][3])
637 {
638  float ftmp; // scratch variable
639 
640  // normalize the X column of the low pass filtered orientation matrix
641  ftmp = sqrtf(A[X][X] * A[X][X] + A[Y][X] * A[Y][X] + A[Z][X] * A[Z][X]);
642  if (ftmp > CORRUPTMATRIX)
643  {
644  // normalize the x column vector
645  ftmp = 1.0F / ftmp;
646  A[X][X] *= ftmp;
647  A[Y][X] *= ftmp;
648  A[Z][X] *= ftmp;
649  }
650  else
651  {
652  // set x column vector to {1, 0, 0}
653  A[X][X] = 1.0F;
654  A[Y][X] = A[Z][X] = 0.0F;
655  }
656 
657  // force the y column vector to be orthogonal to x using y = y-(x.y)x
658  ftmp = A[X][X] * A[X][Y] + A[Y][X] * A[Y][Y] + A[Z][X] * A[Z][Y];
659  A[X][Y] -= ftmp * A[X][X];
660  A[Y][Y] -= ftmp * A[Y][X];
661  A[Z][Y] -= ftmp * A[Z][X];
662 
663  // normalize the y column vector
664  ftmp = sqrtf(A[X][Y] * A[X][Y] + A[Y][Y] * A[Y][Y] + A[Z][Y] * A[Z][Y]);
665  if (ftmp > CORRUPTMATRIX)
666  {
667  // normalize the y column vector
668  ftmp = 1.0F / ftmp;
669  A[X][Y] *= ftmp;
670  A[Y][Y] *= ftmp;
671  A[Z][Y] *= ftmp;
672  }
673  else
674  {
675  // set y column vector to {0, 1, 0}
676  A[Y][Y] = 1.0F;
677  A[X][Y] = A[Z][Y] = 0.0F;
678  }
679 
680  // finally set the z column vector to x vector cross y vector (automatically normalized)
681  A[X][Z] = A[Y][X] * A[Z][Y] - A[Z][X] * A[Y][Y];
682  A[Y][Z] = A[Z][X] * A[X][Y] - A[X][X] * A[Z][Y];
683  A[Z][Z] = A[X][X] * A[Y][Y] - A[Y][X] * A[X][Y];
684 
685  return;
686 }
void fmatrixAeqInvA(float *A[], int8 iColInd[], int8 iRowInd[], int8 iPivot[], int8 isize, int8 *pierror)
Definition: matrix.c:496
void f3x3matrixAeqB(float A[][3], float B[][3])
Definition: matrix.c:55
void f3x3matrixAeqMinusA(float A[][3])
Definition: matrix.c:133
void f3x3matrixAeqAxScalar(float A[][3], float Scalar)
Definition: matrix.c:114
Definition: matrix.h:38
float f3x3matrixDetA(float A[][3])
Definition: matrix.c:189
#define CORRUPTMATRIX
Definition: matrix.c:33
void fmatrixAeqI(float *A[], int16 rc)
Definition: matrix.c:75
#define NITERATIONS
signed short int int16
Definition: isf_types.h:73
void f3x3matrixAeqInvSymB(float A[][3], float B[][3])
Definition: matrix.c:154
Definition: matrix.h:40
Definition: matrix.h:39
void f3x3matrixAeqI(float A[][3])
Definition: matrix.c:36
void eigencompute4(float A[][4], float eigval[], float eigvec[][4], int8 n)
Definition: matrix.c:353
void f3x3matrixAeqScalar(float A[][3], float Scalar)
Definition: matrix.c:96
void fmatrixAeqRenormRotA(float A[][3])
Definition: matrix.c:636
void eigencompute10(float A[][10], float eigval[], float eigvec[][10], int8 n)
Definition: matrix.c:203
signed char int8
Definition: isf_types.h:72