framework-arduinoteensy-cxx20/libraries/NXPMotionSense/matrix.c

// Copyright (c) 2014, Freescale Semiconductor, Inc.
// All rights reserved.
// vim: set ts=4:
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of Freescale Semiconductor, Inc. nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL FREESCALE SEMICONDUCTOR, INC. BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// This file contains matrix manipulation functions.
//

#include <inttypes.h>
#include <math.h>

// compile time constants that are private to this file
#define CORRUPTMATRIX 0.001F			// column vector modulus limit for rotation matrix

// vector components
#define X 0
#define Y 1
#define Z 2

// function sets the 3x3 matrix A to the identity matrix
void f3x3matrixAeqI(float A[][3])
{
	float *pAij;	// pointer to A[i][j]
	int8_t i, j;	// loop counters

	for (i = 0; i < 3; i++) {
		// set pAij to &A[i][j=0]
		pAij = A[i];
		for (j = 0; j < 3; j++) {
			*(pAij++) = 0.0F;
		}
		A[i][i] = 1.0F;
	}
}

// function sets the matrix A to the identity matrix
void fmatrixAeqI(float *A[], int16_t rc)
{
	// rc = rows and columns in A

	float *pAij;	// pointer to A[i][j]
	int8_t i, j;	// loop counters

	for (i = 0; i < rc; i++) {
		// set pAij to &A[i][j=0]
		pAij = A[i];
		for (j = 0; j < rc; j++) {
			*(pAij++) = 0.0F;
		}
		A[i][i] = 1.0F;
	}
}

// function sets every entry in the 3x3 matrix A to a constant scalar
void f3x3matrixAeqScalar(float A[][3], float Scalar)
{
	float *pAij;	// pointer to A[i][j]
	int8_t i, j;	// counters

	for (i = 0; i < 3; i++) {
		// set pAij to &A[i][j=0]
		pAij = A[i];
		for (j = 0; j < 3; j++) {
			*(pAij++) = Scalar;
		}
	}
}

// function multiplies all elements of 3x3 matrix A by the specified scalar
void f3x3matrixAeqAxScalar(float A[][3], float Scalar)
{
	float *pAij;	// pointer to A[i][j]
	int8_t i, j;	// loop counters

	for (i = 0; i < 3; i++) {
		// set pAij to &A[i][j=0]
		pAij = A[i];
		for (j = 0; j < 3; j++) {
			*(pAij++) *= Scalar;
		}
	}
}

// function negates all elements of 3x3 matrix A
void f3x3matrixAeqMinusA(float A[][3])
{
	float *pAij;	// pointer to A[i][j]
	int8_t i, j;	// loop counters

	for (i = 0; i < 3; i++) {
		// set pAij to &A[i][j=0]
		pAij = A[i];
		for (j = 0; j < 3; j++) {
			*pAij = -*pAij;
			pAij++;
		}
	}
}

// function directly calculates the symmetric inverse of a symmetric 3x3 matrix
// only the on and above diagonal terms in B are used and need to be specified
void f3x3matrixAeqInvSymB(float A[][3], float B[][3])
{
	float fB11B22mB12B12;	// B[1][1] * B[2][2] - B[1][2] * B[1][2]
	float fB12B02mB01B22;	// B[1][2] * B[0][2] - B[0][1] * B[2][2]
	float fB01B12mB11B02;	// B[0][1] * B[1][2] - B[1][1] * B[0][2]
	float ftmp;				// determinant and then reciprocal

	// calculate useful products
	fB11B22mB12B12 = B[1][1] * B[2][2] - B[1][2] * B[1][2];
	fB12B02mB01B22 = B[1][2] * B[0][2] - B[0][1] * B[2][2];
	fB01B12mB11B02 = B[0][1] * B[1][2] - B[1][1] * B[0][2];

	// set ftmp to the determinant of the input matrix B
	ftmp = B[0][0] * fB11B22mB12B12 + B[0][1] * fB12B02mB01B22 + B[0][2] * fB01B12mB11B02;

	// set A to the inverse of B for any determinant except zero
	if (ftmp != 0.0F) {
		ftmp = 1.0F / ftmp;
		A[0][0] = fB11B22mB12B12 * ftmp;
		A[1][0] = A[0][1] = fB12B02mB01B22 * ftmp;
		A[2][0] = A[0][2] = fB01B12mB11B02 * ftmp;
		A[1][1] = (B[0][0] * B[2][2] - B[0][2] * B[0][2]) * ftmp;
		A[2][1] = A[1][2] = (B[0][2] * B[0][1] - B[0][0] * B[1][2]) * ftmp;
		A[2][2] = (B[0][0] * B[1][1] - B[0][1] * B[0][1]) * ftmp;
	} else {
		// provide the identity matrix if the determinant is zero
		f3x3matrixAeqI(A);
	}
}

// function calculates the determinant of a 3x3 matrix
float f3x3matrixDetA(float A[][3])
{
	return (A[X][X] * (A[Y][Y] * A[Z][Z] - A[Y][Z] * A[Z][Y]) +
			A[X][Y] * (A[Y][Z] * A[Z][X] - A[Y][X] * A[Z][Z]) +
			A[X][Z] * (A[Y][X] * A[Z][Y] - A[Y][Y] * A[Z][X]));
}

// function computes all eigenvalues and eigenvectors of a real symmetric matrix A[0..n-1][0..n-1]
// stored in the top left of a 10x10 array A[10][10]
// A[][] is changed on output.
// eigval[0..n-1] returns the eigenvalues of A[][].
// eigvec[0..n-1][0..n-1] returns the normalized eigenvectors of A[][]
// the eigenvectors are not sorted by value
void eigencompute(float A[][10], float eigval[], float eigvec[][10], int8_t n)
{
	// maximum number of iterations to achieve convergence: in practice 6 is typical
#define NITERATIONS 15

	// various trig functions of the jacobi rotation angle phi
	float cot2phi, tanhalfphi, tanphi, sinphi, cosphi;
	// scratch variable to prevent over-writing during rotations
	float ftmp;
	// residue from remaining non-zero above diagonal terms
	float residue;
	// matrix row and column indices
	int8_t ir, ic;
	// general loop counter
	int8_t j;
	// timeout ctr for number of passes of the algorithm
	int8_t ctr;

	// initialize eigenvectors matrix and eigenvalues array
	for (ir = 0; ir < n; ir++) {
		// loop over all columns
		for (ic = 0; ic < n; ic++) {
			// set on diagonal and off-diagonal elements to zero
			eigvec[ir][ic] = 0.0F;
		}

		// correct the diagonal elements to 1.0
		eigvec[ir][ir] = 1.0F;

		// initialize the array of eigenvalues to the diagonal elements of m
		eigval[ir] = A[ir][ir];
	}

	// initialize the counter and loop until converged or NITERATIONS reached
	ctr = 0;
	do {
		// compute the absolute value of the above diagonal elements as exit criterion
		residue = 0.0F;
		// loop over rows excluding last row
		for (ir = 0; ir < n - 1; ir++) {
			// loop over above diagonal columns
			for (ic = ir + 1; ic < n; ic++) {
				// accumulate the residual off diagonal terms which are being driven to zero
				residue += fabsf(A[ir][ic]);
			}
		}

		// check if we still have work to do
		if (residue > 0.0F) {
			// loop over all rows with the exception of the last row (since only rotating above diagonal elements)
			for (ir = 0; ir < n - 1; ir++) {
				// loop over columns ic (where ic is always greater than ir since above diagonal)
				for (ic = ir + 1; ic < n; ic++) {
					// only continue with this element if the element is non-zero
					if (fabsf(A[ir][ic]) > 0.0F) {
						// calculate cot(2*phi) where phi is the Jacobi rotation angle
						cot2phi = 0.5F * (eigval[ic] - eigval[ir]) / (A[ir][ic]);

						// calculate tan(phi) correcting sign to ensure the smaller solution is used
						tanphi = 1.0F / (fabsf(cot2phi) + sqrtf(1.0F + cot2phi * cot2phi));
						if (cot2phi < 0.0F) {
							tanphi = -tanphi;
						}

						// calculate the sine and cosine of the Jacobi rotation angle phi
						cosphi = 1.0F / sqrtf(1.0F + tanphi * tanphi);
						sinphi = tanphi * cosphi;

						// calculate tan(phi/2)
						tanhalfphi = sinphi / (1.0F + cosphi);

						// set tmp = tan(phi) times current matrix element used in update of leading diagonal elements
						ftmp = tanphi * A[ir][ic];

						// apply the jacobi rotation to diagonal elements [ir][ir] and [ic][ic] stored in the eigenvalue array
						// eigval[ir] = eigval[ir] - tan(phi) *  A[ir][ic]
						eigval[ir] -= ftmp;
						// eigval[ic] = eigval[ic] + tan(phi) * A[ir][ic]
						eigval[ic] += ftmp;

						// by definition, applying the jacobi rotation on element ir, ic results in 0.0
						A[ir][ic] = 0.0F;

						// apply the jacobi rotation to all elements of the eigenvector matrix
						for (j = 0; j < n; j++) {
							// store eigvec[j][ir]
							ftmp = eigvec[j][ir];
							// eigvec[j][ir] = eigvec[j][ir] - sin(phi) * (eigvec[j][ic] + tan(phi/2) * eigvec[j][ir])
							eigvec[j][ir] = ftmp - sinphi * (eigvec[j][ic] + tanhalfphi * ftmp);
							// eigvec[j][ic] = eigvec[j][ic] + sin(phi) * (eigvec[j][ir] - tan(phi/2) * eigvec[j][ic])
							eigvec[j][ic] = eigvec[j][ic] + sinphi * (ftmp - tanhalfphi * eigvec[j][ic]);
						}

						// apply the jacobi rotation only to those elements of matrix m that can change
						for (j = 0; j <= ir - 1; j++) {
							// store A[j][ir]
							ftmp = A[j][ir];
							// A[j][ir] = A[j][ir] - sin(phi) * (A[j][ic] + tan(phi/2) * A[j][ir])
							A[j][ir] = ftmp - sinphi * (A[j][ic] + tanhalfphi * ftmp);
							// A[j][ic] = A[j][ic] + sin(phi) * (A[j][ir] - tan(phi/2) * A[j][ic])
							A[j][ic] = A[j][ic] + sinphi * (ftmp - tanhalfphi * A[j][ic]);
						}
						for (j = ir + 1; j <= ic - 1; j++) {
							// store A[ir][j]
							ftmp = A[ir][j];
							// A[ir][j] = A[ir][j] - sin(phi) * (A[j][ic] + tan(phi/2) * A[ir][j])
							A[ir][j] = ftmp - sinphi * (A[j][ic] + tanhalfphi * ftmp);
							// A[j][ic] = A[j][ic] + sin(phi) * (A[ir][j] - tan(phi/2) * A[j][ic])
							A[j][ic] = A[j][ic] + sinphi * (ftmp - tanhalfphi * A[j][ic]);
						}
						for (j = ic + 1; j < n; j++) {
							// store A[ir][j]
							ftmp = A[ir][j];
							// A[ir][j] = A[ir][j] - sin(phi) * (A[ic][j] + tan(phi/2) * A[ir][j])
							A[ir][j] = ftmp - sinphi * (A[ic][j] + tanhalfphi * ftmp);
							// A[ic][j] = A[ic][j] + sin(phi) * (A[ir][j] - tan(phi/2) * A[ic][j])
							A[ic][j] = A[ic][j] + sinphi * (ftmp - tanhalfphi * A[ic][j]);
						}
					}   // end of test for matrix element already zero
				}   // end of loop over columns
			}   // end of loop over rows
		}  // end of test for non-zero residue
	} while ((residue > 0.0F) && (ctr++ < NITERATIONS)); // end of main loop
}

// function uses Gauss-Jordan elimination to compute the inverse of matrix A in situ
// on exit, A is replaced with its inverse
void fmatrixAeqInvA(float *A[], int8_t iColInd[], int8_t iRowInd[], int8_t iPivot[], int8_t isize)
{
	float largest;					// largest element used for pivoting
	float scaling;					// scaling factor in pivoting
	float recippiv;					// reciprocal of pivot element
	float ftmp;						// temporary variable used in swaps
	int8_t i, j, k, l, m;			// index counters
	int8_t iPivotRow, iPivotCol;	// row and column of pivot element

	// to avoid compiler warnings
	iPivotRow = iPivotCol = 0;

	// initialize the pivot array to 0
	for (j = 0; j < isize; j++) {
		iPivot[j] = 0;
	}

	// main loop i over the dimensions of the square matrix A
	for (i = 0; i < isize; i++) {
		// zero the largest element found for pivoting
		largest = 0.0F;
		// loop over candidate rows j
		for (j = 0; j < isize; j++) {
			// check if row j has been previously pivoted
			if (iPivot[j] != 1) {
				// loop over candidate columns k
				for (k = 0; k < isize; k++) {
					// check if column k has previously been pivoted
					if (iPivot[k] == 0) {
						// check if the pivot element is the largest found so far
						if (fabsf(A[j][k]) >= largest) {
							// and store this location as the current best candidate for pivoting
							iPivotRow = j;
							iPivotCol = k;
							largest = (float) fabsf(A[iPivotRow][iPivotCol]);
						}
					} else if (iPivot[k] > 1) {
						// zero determinant situation: exit with identity matrix
						fmatrixAeqI(A, isize);
						return;
					}
				}
			}
		}
		// increment the entry in iPivot to denote it has been selected for pivoting
		iPivot[iPivotCol]++;

		// check the pivot rows iPivotRow and iPivotCol are not the same before swapping
		if (iPivotRow != iPivotCol) {
			// loop over columns l
			for (l = 0; l < isize; l++) {
				// and swap all elements of rows iPivotRow and iPivotCol
				ftmp = A[iPivotRow][l];
				A[iPivotRow][l] = A[iPivotCol][l];
				A[iPivotCol][l] = ftmp;
			}
		}

		// record that on the i-th iteration rows iPivotRow and iPivotCol were swapped
		iRowInd[i] = iPivotRow;
		iColInd[i] = iPivotCol;

		// check for zero on-diagonal element (singular matrix) and return with identity matrix if detected
		if (A[iPivotCol][iPivotCol] == 0.0F) {
			// zero determinant situation: exit with identity matrix
			fmatrixAeqI(A, isize);
			return;
		}

		// calculate the reciprocal of the pivot element knowing it's non-zero
		recippiv = 1.0F / A[iPivotCol][iPivotCol];
		// by definition, the diagonal element normalizes to 1
		A[iPivotCol][iPivotCol] = 1.0F;
		// multiply all of row iPivotCol by the reciprocal of the pivot element including the diagonal element
		// the diagonal element A[iPivotCol][iPivotCol] now has value equal to the reciprocal of its previous value
		for (l = 0; l < isize; l++) {
			A[iPivotCol][l] *= recippiv;
		}
		// loop over all rows m of A
		for (m = 0; m < isize; m++) {
			if (m != iPivotCol) {
				// scaling factor for this row m is in column iPivotCol
				scaling = A[m][iPivotCol];
				// zero this element
				A[m][iPivotCol] = 0.0F;
				// loop over all columns l of A and perform elimination
				for (l = 0; l < isize; l++) {
					A[m][l] -= A[iPivotCol][l] * scaling;
				}
			}
		}
	} // end of loop i over the matrix dimensions

	// finally, loop in inverse order to apply the missing column swaps
	for (l = isize - 1; l >= 0; l--) {
		// set i and j to the two columns to be swapped
		i = iRowInd[l];
		j = iColInd[l];

		// check that the two columns i and j to be swapped are not the same
		if (i != j) {
			// loop over all rows k to swap columns i and j of A
			for (k = 0; k < isize; k++) {
				ftmp = A[k][i];
				A[k][i] = A[k][j];
				A[k][j] = ftmp;
			}
		}
	}
}

// function re-orthonormalizes a 3x3 rotation matrix
void fmatrixAeqRenormRotA(float A[][3])
{
	float ftmp;					// scratch variable

	// normalize the X column of the low pass filtered orientation matrix
	ftmp = sqrtf(A[X][X] * A[X][X] + A[Y][X] * A[Y][X] + A[Z][X] * A[Z][X]);
	if (ftmp > CORRUPTMATRIX) {
		// normalize the x column vector
		ftmp = 1.0F / ftmp;
		A[X][X] *= ftmp;
		A[Y][X] *= ftmp;
		A[Z][X] *= ftmp;
	} else {
		// set x column vector to {1, 0, 0}
		A[X][X] = 1.0F;
		A[Y][X] = A[Z][X] = 0.0F;
	}

	// force the y column vector to be orthogonal to x using y = y-(x.y)x
	ftmp = A[X][X] * A[X][Y] + A[Y][X] * A[Y][Y] + A[Z][X] * A[Z][Y];
	A[X][Y] -= ftmp * A[X][X];
	A[Y][Y] -= ftmp * A[Y][X];
	A[Z][Y] -= ftmp * A[Z][X];

	// normalize the y column vector
	ftmp = sqrtf(A[X][Y] * A[X][Y] + A[Y][Y] * A[Y][Y] + A[Z][Y] * A[Z][Y]);
	if (ftmp > CORRUPTMATRIX) {
		// normalize the y column vector
		ftmp = 1.0F / ftmp;
		A[X][Y] *= ftmp;
		A[Y][Y] *= ftmp;
		A[Z][Y] *= ftmp;
	} else {
		// set y column vector to {0, 1, 0}
		A[Y][Y] = 1.0F;
		A[X][Y] = A[Z][Y] = 0.0F;
	}

	// finally set the z column vector to x vector cross y vector (automatically normalized)
	A[X][Z] = A[Y][X] * A[Z][Y] - A[Z][X] * A[Y][Y];
	A[Y][Z] = A[Z][X] * A[X][Y] - A[X][X] * A[Z][Y];
	A[Z][Z] = A[X][X] * A[Y][Y] - A[Y][X] * A[X][Y];
}