[paparazzi-commits] [4727] Add new function for quat/rmat multiply

[Allen Ibara]

Revision: 4727
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4727
Author:   aibara
Date:     2010-03-25 06:45:50 +0000 (Thu, 25 Mar 2010)
Log Message:
-----------
Add new function for quat/rmat multiply

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      2010-03-24 
22:08:28 UTC (rev 4726)
+++ paparazzi3/trunk/sw/airborne/math/pprz_algebra_float.h      2010-03-25 
06:45:50 UTC (rev 4727)
@@ -207,6 +207,12 @@
     (_vb).r = ( (_m_b2a).m[2]*(_va).p + (_m_b2a).m[5]*(_va).q + 
(_m_b2a).m[8]*(_va).r); \
   }
 
+#define FLOAT_RMAT_RATEMULT(_vb, _m_a2b, _va) {                                
                         \
+    (_vb).p = ( (_m_a2b).m[0]*(_va).p + (_m_a2b).m[1]*(_va).q + 
(_m_a2b).m[2]*(_va).r); \
+    (_vb).q = ( (_m_a2b).m[3]*(_va).p + (_m_a2b).m[4]*(_va).q + 
(_m_a2b).m[5]*(_va).r); \
+    (_vb).r = ( (_m_a2b).m[6]*(_va).p + (_m_a2b).m[7]*(_va).q + 
(_m_a2b).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]); \