Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-07 09:36:19 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

Add optional printf debug to 3x3 matrix inv

Browse Source
This commit is contained in:
Allen Ibara
2010-10-06 04:49:29 +00:00
parent 6ac4fa1069
commit 3593fee945
1 changed files with 30 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/math/pprz_algebra.h
+30
View File
@@ -25,6 +25,8 @@
#ifndef PPRZ_ALGEBRA_H
#define PPRZ_ALGEBRA_H
#include <float.h> /* for FLT_EPSILON */
#define SQUARE(_a) ((_a)*(_a))
/*
@@ -371,6 +373,34 @@
MAT33_ELMT((_mat), 2, 2) * (_vin).z; \
}
/* invS = 1/det(S) com(S)' */
#define MAT33_INV(_minv, _m) { \
const float m00 = MAT33_ELMT((_m),1,1)*MAT33_ELMT((_m),2,2) - MAT33_ELMT((_m),1,2)*MAT33_ELMT((_m),2,1); \
const float m10 = MAT33_ELMT((_m),0,1)*MAT33_ELMT((_m),2,2) - MAT33_ELMT((_m),0,2)*MAT33_ELMT((_m),2,1); \
const float m20 = MAT33_ELMT((_m),0,1)*MAT33_ELMT((_m),1,2) - MAT33_ELMT((_m),0,2)*MAT33_ELMT((_m),1,1); \
const float m01 = MAT33_ELMT((_m),1,0)*MAT33_ELMT((_m),2,2) - MAT33_ELMT((_m),1,2)*MAT33_ELMT((_m),2,0); \
const float m11 = MAT33_ELMT((_m),0,0)*MAT33_ELMT((_m),2,2) - MAT33_ELMT((_m),0,2)*MAT33_ELMT((_m),2,0); \
const float m21 = MAT33_ELMT((_m),0,0)*MAT33_ELMT((_m),1,2) - MAT33_ELMT((_m),0,2)*MAT33_ELMT((_m),1,0); \
const float m02 = MAT33_ELMT((_m),1,0)*MAT33_ELMT((_m),2,1) - MAT33_ELMT((_m),1,1)*MAT33_ELMT((_m),2,0); \
const float m12 = MAT33_ELMT((_m),0,0)*MAT33_ELMT((_m),2,1) - MAT33_ELMT((_m),0,1)*MAT33_ELMT((_m),2,0); \
const float m22 = MAT33_ELMT((_m),0,0)*MAT33_ELMT((_m),1,1) - MAT33_ELMT((_m),0,1)*MAT33_ELMT((_m),1,0); \
const float det = MAT33_ELMT((_m),0,0)*m00 - MAT33_ELMT((_m),1,0)*m10 + MAT33_ELMT((_m),2,0)*m20; \
if (fabs(det) > FLT_EPSILON) { \
MAT33_ELMT((_minv),0,0) = m00 / det; \
MAT33_ELMT((_minv),1,0) = -m01 / det; \
MAT33_ELMT((_minv),2,0) = m02 / det; \
MAT33_ELMT((_minv),0,1) = -m10 / det; \
MAT33_ELMT((_minv),1,1) = m11 / det; \
MAT33_ELMT((_minv),2,1) = -m12 / det; \
MAT33_ELMT((_minv),0,2) = m20 / det; \
MAT33_ELMT((_minv),1,2) = -m21 / det; \
MAT33_ELMT((_minv),2,2) = m22 / det; \
} \
}
//
//
// Quaternions