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

more math stuff

Browse Source
This commit is contained in:
Felix Ruess
2009-07-28 12:34:52 +00:00
parent 49829f42dc
commit d278fef4e9
2 changed files with 23 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/math/pprz_algebra.h
+12 -1
View File
@@ -437,7 +437,18 @@
(_c).m[8] = (_a).m[8] - (_b).m[8]; \
}
/* multiply _vin by _rmat, store in _vout */
#define RMAT_VECT3_MUL(_vout, _rmat, _vin) { \
(_vout).x = RMAT_ELMT((_rmat), 0, 0) * (_vin).x + \
RMAT_ELMT((_rmat), 0, 1) * (_vin).y + \
RMAT_ELMT((_rmat), 0, 2) * (_vin).z; \
(_vout).y = RMAT_ELMT((_rmat), 1, 0) * (_vin).x + \
RMAT_ELMT((_rmat), 1, 1) * (_vin).y + \
RMAT_ELMT((_rmat), 1, 2) * (_vin).z; \
(_vout).z = RMAT_ELMT((_rmat), 2, 0) * (_vin).x + \
RMAT_ELMT((_rmat), 2, 1) * (_vin).y + \
RMAT_ELMT((_rmat), 2, 2) * (_vin).z; \
}
sw/airborne/math/pprz_algebra_float.h
+11 -11
View File
@@ -75,18 +75,15 @@ struct FloatRates {
#define FLOAT_VECT3_ZERO(_v) VECT3_ASSIGN(_v, 0., 0., 0.) \
#define FLOAT_VECT3_DIFF(_c, _a, _b) { \
(_c).x = (_a).x - (_b).x; \
(_c).y = (_a).y - (_b).y; \
(_c).z = (_a).z - (_b).z; \
}
#define FLOAT_VECT3_SMUL(_vo, _s, _vi) { \
(_vo).x = (_s) * (_vi).x; \
(_vo).y = (_s) * (_vi).y; \
(_vo).z = (_s) * (_vi).z; \
}
/* c = a - b */
#define FLOAT_VECT3_DIFF(_c, _a, _b) VECT3_DIFF(_c, _a, _b)
/* a -= b */
#define FLOAT_VECT3_SUB(_a, _b) VECT3_SUB(_a, _b)
/* _vo = _s * _vi */
#define FLOAT_VECT3_SMUL(_vo, _s, _vi) VECT3_SMUL(_vo, _s, _vi)
#define FLOAT_VECT3_NORM(_v) (sqrtf((_v).x*(_v).x + (_v).y*(_v).y + (_v).z*(_v).z))
@@ -166,6 +163,9 @@ struct FloatRates {
\
}
/* multiply _vin by _rmat, store in _vout */
#define FLOAT_RMAT_VECT3_MUL(_vout, _rmat, _vin) RMAT_VECT3_MUL(_vout, _rmat, _vin)
/* _m_a2c = _m_a2b comp _m_b2c , aka _m_a2c = _m_b2c * _m_a2b */
#define FLOAT_RMAT_COMP(_m_a2c, _m_a2b, _m_b2c) { \
_m_a2c.m[0] = (_m_b2c.m[0]*_m_a2b.m[0] + _m_b2c.m[1]*_m_a2b.m[3] + _m_b2c.m[2]*_m_a2b.m[6]); \