mpower.c File Reference

#include "mpower.h"
#include "rt_nonfinite.h"
#include <math.h>
#include <string.h>

Include dependency graph for mpower.c:

Go to the source code of this file.

Functions
void	mpower (const double a[9], double c[9])
	Calculates the inverse of a 3x3 matrix.

Function Documentation

mpower()

void mpower	(	const double	a[9],
		double	c[9]
	)

Calculates the inverse of a 3x3 matrix.

Parameters

[in]	a	A 9-element array representing the input 3x3 matrix (row-major).
[out]	c	A 9-element array where the resulting inverse matrix will be stored.

Returns

None

Warning

This function does not check if the matrix is invertible (i.e., if its determinant is zero). Providing a singular matrix may result in undefined behavior or NaNs in the output.

Definition at line 24 of file mpower.c.

{
  double x[9];
  double absx11;
  double absx21;
  double absx31;
  int p1;
  int p2;
  int p3;
  memcpy(&x[0], &a[0], 9U * sizeof(double));
  p1 = 0;
  p2 = 3;
  p3 = 6;
  absx11 = fabs(a[0]);
  absx21 = fabs(a[1]);
  absx31 = fabs(a[2]);
  if ((absx21 > absx11) && (absx21 > absx31)) {
    p1 = 3;
    p2 = 0;
    x[0] = a[1];
    x[1] = a[0];
    x[3] = a[4];
    x[4] = a[3];
    x[6] = a[7];
    x[7] = a[6];
  } else if (absx31 > absx11) {
    p1 = 6;
    p3 = 0;
    x[0] = a[2];
    x[2] = a[0];
    x[3] = a[5];
    x[5] = a[3];
    x[6] = a[8];
    x[8] = a[6];
  }
  x[1] /= x[0];
  x[2] /= x[0];
  x[4] -= x[1] * x[3];
  x[5] -= x[2] * x[3];
  x[7] -= x[1] * x[6];
  x[8] -= x[2] * x[6];
  if (fabs(x[5]) > fabs(x[4])) {
    int itmp;
    itmp = p2;
    p2 = p3;
    p3 = itmp;
    absx11 = x[1];
    x[1] = x[2];
    x[2] = absx11;
    absx11 = x[4];
    x[4] = x[5];
    x[5] = absx11;
    absx11 = x[7];
    x[7] = x[8];
    x[8] = absx11;
  }
  x[5] /= x[4];
  x[8] -= x[5] * x[7];
  absx11 = (x[1] * x[5] - x[2]) / x[8];
  absx21 = -(x[1] + x[7] * absx11) / x[4];
  c[p1] = ((1.0 - x[3] * absx21) - x[6] * absx11) / x[0];
  c[p1 + 1] = absx21;
  c[p1 + 2] = absx11;
  absx11 = -x[5] / x[8];
  absx21 = (1.0 - x[7] * absx11) / x[4];
  c[p2] = -(x[3] * absx21 + x[6] * absx11) / x[0];
  c[p2 + 1] = absx21;
  c[p2 + 2] = absx11;
  absx11 = 1.0 / x[8];
  absx21 = -x[7] * absx11 / x[4];
  c[p3] = -(x[3] * absx21 + x[6] * absx11) / x[0];
  c[p3 + 1] = absx21;
  c[p3 + 2] = absx11;
}

Referenced by mekf().

Here is the caller graph for this function: