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 29 Packages Projects Releases Wiki Activity

[math] untabify

Browse Source
This commit is contained in:
Felix Ruess
2013-08-21 13:43:05 +02:00
parent 53d1c1907b
commit 3ccb8ea475
1 changed files with 44 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/math/pprz_simple_matrix.h
+44 -44
View File
@@ -13,38 +13,38 @@
//
// C = A*B A:(i,k) B:(k,j) C:(i,j)
//
#define MAT_MUL(_i, _k, _j, C, A, B) { \
int l,c,m; \
for (l=0; l<_i; l++) \
for (c=0; c<_j; c++) { \
C[l][c] = 0.; \
for (m=0; m<_k; m++) \
C[l][c] += A[l][m]*B[m][c]; \
} \
#define MAT_MUL(_i, _k, _j, C, A, B) { \
int l,c,m; \
for (l=0; l<_i; l++) \
for (c=0; c<_j; c++) { \
C[l][c] = 0.; \
for (m=0; m<_k; m++) \
C[l][c] += A[l][m]*B[m][c]; \
} \
}
//
// C = A*B' A:(i,k) B:(j,k) C:(i,j)
//
#define MAT_MUL_T(_i, _k, _j, C, A, B) { \
int l,c,m; \
for (l=0; l<_i; l++) \
for (c=0; c<_j; c++) { \
C[l][c] = 0.; \
for (m=0; m<_k; m++) \
C[l][c] += A[l][m]*B[c][m]; \
} \
#define MAT_MUL_T(_i, _k, _j, C, A, B) { \
int l,c,m; \
for (l=0; l<_i; l++) \
for (c=0; c<_j; c++) { \
C[l][c] = 0.; \
for (m=0; m<_k; m++) \
C[l][c] += A[l][m]*B[c][m]; \
} \
}
//
// C = A-B
//
#define MAT_SUB(_i, _j, C, A, B) { \
int l,c; \
for (l=0; l<_i; l++) \
for (c=0; c<_j; c++) \
C[l][c] = A[l][c] - B[l][c]; \
#define MAT_SUB(_i, _j, C, A, B) { \
int l,c; \
for (l=0; l<_i; l++) \
for (c=0; c<_j; c++) \
C[l][c] = A[l][c] - B[l][c]; \
}
@@ -53,31 +53,31 @@
//
// invS = 1/det(S) com(S)'
//
#define MAT_INV33(_invS, _S) { \
const float m00 = _S[1][1]*_S[2][2] - _S[1][2]*_S[2][1]; \
const float m10 = _S[0][1]*_S[2][2] - _S[0][2]*_S[2][1]; \
const float m20 = _S[0][1]*_S[1][2] - _S[0][2]*_S[1][1]; \
const float m01 = _S[1][0]*_S[2][2] - _S[1][2]*_S[2][0]; \
const float m11 = _S[0][0]*_S[2][2] - _S[0][2]*_S[2][0]; \
const float m21 = _S[0][0]*_S[1][2] - _S[0][2]*_S[1][0]; \
const float m02 = _S[1][0]*_S[2][1] - _S[1][1]*_S[2][0]; \
const float m12 = _S[0][0]*_S[2][1] - _S[0][1]*_S[2][0]; \
const float m22 = _S[0][0]*_S[1][1] - _S[0][1]*_S[1][0]; \
float det = _S[0][0]*m00 - _S[1][0]*m10 + _S[2][0]*m20; \
if (fabs(det) < FLT_EPSILON) { \
#define MAT_INV33(_invS, _S) { \
const float m00 = _S[1][1]*_S[2][2] - _S[1][2]*_S[2][1]; \
const float m10 = _S[0][1]*_S[2][2] - _S[0][2]*_S[2][1]; \
const float m20 = _S[0][1]*_S[1][2] - _S[0][2]*_S[1][1]; \
const float m01 = _S[1][0]*_S[2][2] - _S[1][2]*_S[2][0]; \
const float m11 = _S[0][0]*_S[2][2] - _S[0][2]*_S[2][0]; \
const float m21 = _S[0][0]*_S[1][2] - _S[0][2]*_S[1][0]; \
const float m02 = _S[1][0]*_S[2][1] - _S[1][1]*_S[2][0]; \
const float m12 = _S[0][0]*_S[2][1] - _S[0][1]*_S[2][0]; \
const float m22 = _S[0][0]*_S[1][1] - _S[0][1]*_S[1][0]; \
float det = _S[0][0]*m00 - _S[1][0]*m10 + _S[2][0]*m20; \
if (fabs(det) < FLT_EPSILON) { \
/* If the determinant is too small then set it to epsilon preserving sign. */ \
warn_message("warning: %s:%d MAT_INV33 trying to invert non-invertable matrix '%s' and put result in '%s'.\n", __FILE__, __LINE__, #_S, #_invS); \
det = copysignf(FLT_EPSILON, det); \
} \
_invS[0][0] = m00 / det; \
_invS[1][0] = -m01 / det; \
_invS[2][0] = m02 / det; \
_invS[0][1] = -m10 / det; \
_invS[1][1] = m11 / det; \
_invS[2][1] = -m12 / det; \
_invS[0][2] = m20 / det; \
_invS[1][2] = -m21 / det; \
_invS[2][2] = m22 / det; \
det = copysignf(FLT_EPSILON, det); \
} \
_invS[0][0] = m00 / det; \
_invS[1][0] = -m01 / det; \
_invS[2][0] = m02 / det; \
_invS[0][1] = -m10 / det; \
_invS[1][1] = m11 / det; \
_invS[2][1] = -m12 / det; \
_invS[0][2] = m20 / det; \
_invS[1][2] = -m21 / det; \
_invS[2][2] = m22 / det; \
}