zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Math: add inverse matrix to math library

mission-4.1.18
Siddharth Bharat Purohit 10 years ago committed by Andrew Tridgell
parent
eec5c2a5eb
commit
b654b1c21b
2 changed files with 433 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      libraries/AP_Math/AP_Math.h
  2. 429
      libraries/AP_Math/matrix_alg.cpp

4
libraries/AP_Math/AP_Math.h
Unescape View File

@ -35,6 +35,7 @@ @@ -35,6 +35,7 @@
# define M_PI_2 1.570796326794897f
#endif
//Single precision conversions
#define TINY_FLOAT 1.0e-20f
#define DEG_TO_RAD 0.017453292519943295769236907684886f
#define RAD_TO_DEG 57.295779513082320876798154814105f
@ -172,6 +173,9 @@ float constrain_float(float amt, float low, float high); @@ -172,6 +173,9 @@ float constrain_float(float amt, float low, float high);
int16_t constrain_int16(int16_t amt, int16_t low, int16_t high);
int32_t constrain_int32(int32_t amt, int32_t low, int32_t high);
//matrix algebra
bool inverse(float x[], float y[], uint16_t dim);
// degrees -> radians
float radians(float deg);

429
libraries/AP_Math/matrix_alg.cpp
Unescape View File

@ -0,0 +1,429 @@ @@ -0,0 +1,429 @@
#include<AP_Math.h>
#include <AP_HAL.h>
extern const AP_HAL::HAL& hal;
/*
* generic matrix inverse code
*
* @param x, input nxn matrix
* @param n, dimension of square matrix
* @returns determinant of square matrix
* Known Issues/ Possible Enhancements:
* -more efficient method should be available, following is code generated from matlab
*/
float detnxn(const float C[],const uint8_t n)
{
float f;
float *A = new float[n*n];
int8_t *ipiv = new int8_t[n];
int32_t i0;
int32_t j;
int32_t c;
int32_t iy;
int32_t ix;
float smax;
int32_t jy;
float s;
int32_t b_j;
int32_t ijA;
bool isodd;
memcpy(&A[0], &C[0], n*n * sizeof(float));
for (i0 = 0; i0 < n; i0++) {
ipiv[i0] = (int8_t)(1 + i0);
}
for (j = 0; j < n-1; j++) {
c = j * (n+1);
iy = 0;
ix = c;
smax = fabs(A[c]);
for (jy = 2; jy <= n - 1 - j; jy++) {
ix++;
s = fabs(A[ix]);
if (s > smax) {
iy = jy - 1;
smax = s;
}
}
if (A[c + iy] != 0.0) {
if (iy != 0) {
ipiv[j] = (int8_t)((j + iy) + 1);
ix = j;
iy += j;
for (jy = 0; jy < n; jy++) {
smax = A[ix];
A[ix] = A[iy];
A[iy] = smax;
ix += n;
iy += n;
}
}
i0 = (c - j) + n;
for (iy = c + 1; iy + 1 <= i0; iy++) {
A[iy] /= A[c];
}
}
iy = c;
jy = c + n;
for (b_j = 1; b_j <= n - 1 - j; b_j++) {
smax = A[jy];
if (A[jy] != 0.0) {
ix = c + 1;
i0 = (iy - j) + (2*n);
for (ijA = n + 1 + iy; ijA + 1 <= i0; ijA++) {
A[ijA] += A[ix] * -smax;
ix++;
}
}
jy += n;
iy += n;
}
}
f = A[0];
isodd = false;
for (jy = 0; jy < (n-1); jy++) {
f *= A[(jy + n * (1 + jy)) + 1];
if (ipiv[jy] > 1 + jy) {
isodd = !isodd;
}
}
if (isodd) {
f = -f;
}
delete[] A;
delete[] ipiv;
return f;
}
/*
* generic matrix inverse code
*
* @param x, input nxn matrix
* @param y, Output inverted nxn matrix
* @param n, dimension of square matrix
* @returns false = matrix is Singular, true = matrix inversion successful
* Known Issues/ Possible Enhancements:
* -more efficient method should be available, following is code generated from matlab
*/
bool inversenxn(const float x[], float y[], const uint8_t n)
{
if(fabsf(detnxn(x,n)) < TINY_FLOAT) {
return false;
}
float *A = new float[n*n];
int32_t i0;
int32_t *ipiv = new int32_t[n];
int32_t j;
int32_t c;
int32_t pipk;
int32_t ix;
float smax;
int32_t k;
float s;
int32_t jy;
int32_t ijA;
int32_t *p = new int32_t[n];
for (i0 = 0; i0 < n*n; i0++) {
A[i0] = x[i0];
y[i0] = 0.0f;
}
for (i0 = 0; i0 < n; i0++) {
ipiv[i0] = (int32_t)(1 + i0);
}
for (j = 0; j < (n-1); j++) {
c = j * (n+1);
pipk = 0;
ix = c;
smax = fabsf(A[c]);
for (k = 2; k <= (n-1) - j; k++) {
ix++;
s = fabsf(A[ix]);
if (s > smax) {
pipk = k - 1;
smax = s;
}
}
if (A[c + pipk] != 0.0f) {
if (pipk != 0) {
ipiv[j] = (int32_t)((j + pipk) + 1);
ix = j;
pipk += j;
for (k = 0; k < n; k++) {
smax = A[ix];
A[ix] = A[pipk];
A[pipk] = smax;
ix += n;
pipk += n;
}
}
i0 = (c - j) + n;
for (jy = c + 1; jy + 1 <= i0; jy++) {
A[jy] /= A[c];
}
}
pipk = c;
jy = c + n;
for (k = 1; k <= (n-1) - j; k++) {
smax = A[jy];
if (A[jy] != 0.0f) {
ix = c + 1;
i0 = (pipk - j) + (2*n);
for (ijA = (n+1) + pipk; ijA + 1 <= i0; ijA++) {
A[ijA] += A[ix] * -smax;
ix++;
}
}
jy += n;
pipk += n;
}
}
for (i0 = 0; i0 < n; i0++) {
p[i0] = (int32_t)(1 + i0);
}
for (k = 0; k < (n-1); k++) {
if (ipiv[k] > 1 + k) {
pipk = p[ipiv[k] - 1];
p[ipiv[k] - 1] = p[k];
p[k] = (int32_t)pipk;
}
}
for (k = 0; k < n; k++) {
y[k + n * (p[k] - 1)] = 1.0;
for (j = k; j + 1 < (n+1); j++) {
if (y[j + n * (p[k] - 1)] != 0.0f) {
for (jy = j + 1; jy + 1 < (n+1); jy++) {
y[jy + n * (p[k] - 1)] -= y[j + n * (p[k] - 1)] * A[jy + n * j];
}
}
}
}
for (j = 0; j < n; j++) {
c = n * j;
for (k = (n-1); k > -1; k += -1) {
pipk = n * k;
if (y[k + c] != 0.0f) {
y[k + c] /= A[k + pipk];
for (jy = 0; jy + 1 <= k; jy++) {
y[jy + c] -= y[k + c] * A[jy + pipk];
}
}
}
}
delete[] A;
delete[] ipiv;
delete[] p;
return true;
}
/*
* matrix inverse code only for 3x3 square matrix
*
* @param m, input 4x4 matrix
* @param invOut, Output inverted 4x4 matrix
* @returns false = matrix is Singular, true = matrix inversion successful
*/
bool inverse3x3(float m[], float invOut[])
{
float inv[9];
// computes the inverse of a matrix m
float det = m[0] * (m[4] * m[8] - m[7] * m[5]) -
m[1] * (m[3] * m[8] - m[5] * m[6]) +
m[2] * (m[3] * m[7] - m[4] * m[6]);
if(fabsf(det) < TINY_FLOAT){
return false;
}
float invdet = 1 / det;
inv[0] = (m[4] * m[8] - m[7] * m[5]) * invdet;
inv[1] = (m[2] * m[7] - m[1] * m[8]) * invdet;
inv[2] = (m[1] * m[5] - m[2] * m[4]) * invdet;
inv[3] = (m[5] * m[6] - m[5] * m[8]) * invdet;
inv[4] = (m[0] * m[8] - m[2] * m[6]) * invdet;
inv[5] = (m[3] * m[2] - m[0] * m[5]) * invdet;
inv[6] = (m[3] * m[7] - m[6] * m[4]) * invdet;
inv[7] = (m[6] * m[1] - m[0] * m[7]) * invdet;
inv[8] = (m[0] * m[4] - m[3] * m[1]) * invdet;
for(uint8_t i = 0; i < 9; i++){
invOut[i] = inv[i];
}
return true;
}
/*
* matrix inverse code only for 4x4 square matrix copied from
* gluInvertMatrix implementation in
* opengl for 4x4 matrices.
*
* @param m, input 4x4 matrix
* @param invOut, Output inverted 4x4 matrix
* @returns false = matrix is Singular, true = matrix inversion successful
*/
bool inverse4x4(float m[],float invOut[])
{
float inv[16], det;
uint8_t i;
inv[0] = m[5] * m[10] * m[15] -
m[5] * m[11] * m[14] -
m[9] * m[6] * m[15] +
m[9] * m[7] * m[14] +
m[13] * m[6] * m[11] -
m[13] * m[7] * m[10];
inv[4] = -m[4] * m[10] * m[15] +
m[4] * m[11] * m[14] +
m[8] * m[6] * m[15] -
m[8] * m[7] * m[14] -
m[12] * m[6] * m[11] +
m[12] * m[7] * m[10];
inv[8] = m[4] * m[9] * m[15] -
m[4] * m[11] * m[13] -
m[8] * m[5] * m[15] +
m[8] * m[7] * m[13] +
m[12] * m[5] * m[11] -
m[12] * m[7] * m[9];
inv[12] = -m[4] * m[9] * m[14] +
m[4] * m[10] * m[13] +
m[8] * m[5] * m[14] -
m[8] * m[6] * m[13] -
m[12] * m[5] * m[10] +
m[12] * m[6] * m[9];
inv[1] = -m[1] * m[10] * m[15] +
m[1] * m[11] * m[14] +
m[9] * m[2] * m[15] -
m[9] * m[3] * m[14] -
m[13] * m[2] * m[11] +
m[13] * m[3] * m[10];
inv[5] = m[0] * m[10] * m[15] -
m[0] * m[11] * m[14] -
m[8] * m[2] * m[15] +
m[8] * m[3] * m[14] +
m[12] * m[2] * m[11] -
m[12] * m[3] * m[10];
inv[9] = -m[0] * m[9] * m[15] +
m[0] * m[11] * m[13] +
m[8] * m[1] * m[15] -
m[8] * m[3] * m[13] -
m[12] * m[1] * m[11] +
m[12] * m[3] * m[9];
inv[13] = m[0] * m[9] * m[14] -
m[0] * m[10] * m[13] -
m[8] * m[1] * m[14] +
m[8] * m[2] * m[13] +
m[12] * m[1] * m[10] -
m[12] * m[2] * m[9];
inv[2] = m[1] * m[6] * m[15] -
m[1] * m[7] * m[14] -
m[5] * m[2] * m[15] +
m[5] * m[3] * m[14] +
m[13] * m[2] * m[7] -
m[13] * m[3] * m[6];
inv[6] = -m[0] * m[6] * m[15] +
m[0] * m[7] * m[14] +
m[4] * m[2] * m[15] -
m[4] * m[3] * m[14] -
m[12] * m[2] * m[7] +
m[12] * m[3] * m[6];
inv[10] = m[0] * m[5] * m[15] -
m[0] * m[7] * m[13] -
m[4] * m[1] * m[15] +
m[4] * m[3] * m[13] +
m[12] * m[1] * m[7] -
m[12] * m[3] * m[5];
inv[14] = -m[0] * m[5] * m[14] +
m[0] * m[6] * m[13] +
m[4] * m[1] * m[14] -
m[4] * m[2] * m[13] -
m[12] * m[1] * m[6] +
m[12] * m[2] * m[5];
inv[3] = -m[1] * m[6] * m[11] +
m[1] * m[7] * m[10] +
m[5] * m[2] * m[11] -
m[5] * m[3] * m[10] -
m[9] * m[2] * m[7] +
m[9] * m[3] * m[6];
inv[7] = m[0] * m[6] * m[11] -
m[0] * m[7] * m[10] -
m[4] * m[2] * m[11] +
m[4] * m[3] * m[10] +
m[8] * m[2] * m[7] -
m[8] * m[3] * m[6];
inv[11] = -m[0] * m[5] * m[11] +
m[0] * m[7] * m[9] +
m[4] * m[1] * m[11] -
m[4] * m[3] * m[9] -
m[8] * m[1] * m[7] +
m[8] * m[3] * m[5];
inv[15] = m[0] * m[5] * m[10] -
m[0] * m[6] * m[9] -
m[4] * m[1] * m[10] +
m[4] * m[2] * m[9] +
m[8] * m[1] * m[6] -
m[8] * m[2] * m[5];
det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];
if(fabsf(det) < TINY_FLOAT){
return false;
}
det = 1.0f / det;
for (i = 0; i < 16; i++)
invOut[i] = inv[i] * det;
return true;
}
/*
* generic matrix inverse code
*
* @param x, input nxn matrix
* @param y, Output inverted nxn matrix
* @param n, dimension of square matrix
* @returns false = matrix is Singular, true = matrix inversion successful
*/
bool inverse(float x[], float y[], uint16_t dim)
{
switch(dim){
case 3: return inverse3x3(x,y);
case 4: return inverse4x4(x,y);
default: return inversenxn(x,y,dim);
}
}
Write Preview
Loading…
Cancel
Save