[paparazzi-commits] [4047] Add FLOAT_RMAT_TRANSP_RATEMULT macro

[Allen Ibara]

Revision: 4047
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4047
Author:   aibara
Date:     2009-09-02 23:34:23 +0000 (Wed, 02 Sep 2009)
Log Message:
-----------
Add FLOAT_RMAT_TRANSP_RATEMULT macro

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/math/pprz_algebra_float.h

Modified: paparazzi3/trunk/sw/airborne/math/pprz_algebra_float.h
===================================================================
--- paparazzi3/trunk/sw/airborne/math/pprz_algebra_float.h      2009-09-02 
13:02:02 UTC (rev 4046)
+++ paparazzi3/trunk/sw/airborne/math/pprz_algebra_float.h      2009-09-02 
23:34:23 UTC (rev 4047)
@@ -171,6 +171,12 @@
 /* multiply _vin by _rmat, store in _vout */
 #define FLOAT_RMAT_VECT3_MUL(_vout, _rmat, _vin) RMAT_VECT3_MUL(_vout, _rmat, 
_vin)
 
+#define FLOAT_RMAT_TRANSP_RATEMULT(_vb, _m_b2a, _va) {                         
                         \
+    (_vb).p = ( (_m_b2a).m[0]*(_va).p + (_m_b2a).m[3]*(_va).q + 
(_m_b2a).m[6]*(_va).r); \
+    (_vb).q = ( (_m_b2a).m[1]*(_va).p + (_m_b2a).m[4]*(_va).q + 
(_m_b2a).m[7]*(_va).r); \
+    (_vb).r = ( (_m_b2a).m[2]*(_va).p + (_m_b2a).m[5]*(_va).q + 
(_m_b2a).m[8]*(_va).r); \
+  }
+
 /* _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]); \