[python][math] some DoubleRMat additions

sw/airborne/math/pprz_algebra_double.c

@@ -110,3 +110,67 @@ void double_quat_vmult(struct DoubleVect3 *v_out, struct DoubleQuat *q, struct D
   v_out->y = 2 * (m10 * v_in->x + m11 * v_in->y + m12 * v_in->z);
   v_out->z = 2 * (m20 * v_in->x + m21 * v_in->y + m22 * v_in->z);
 }
+
+void double_rmat_inv(struct DoubleRMat *m_b2a, struct DoubleRMat *m_a2b)
+{
+  /*RMAT_ELMT(*m_b2a, 0, 0) = RMAT_ELMT(*m_a2b, 0, 0);*/
+  RMAT_ELMT(*m_b2a, 0, 1) = RMAT_ELMT(*m_a2b, 1, 0);
+  RMAT_ELMT(*m_b2a, 0, 2) = RMAT_ELMT(*m_a2b, 2, 0);
+  RMAT_ELMT(*m_b2a, 1, 0) = RMAT_ELMT(*m_a2b, 0, 1);
+  /*RMAT_ELMT(*m_b2a, 1, 1) = RMAT_ELMT(*m_a2b, 1, 1);*/
+  RMAT_ELMT(*m_b2a, 1, 2) = RMAT_ELMT(*m_a2b, 2, 1);
+  RMAT_ELMT(*m_b2a, 2, 0) = RMAT_ELMT(*m_a2b, 0, 2);
+  RMAT_ELMT(*m_b2a, 2, 1) = RMAT_ELMT(*m_a2b, 1, 2);
+  /*RMAT_ELMT(*m_b2a, 2, 2) = RMAT_ELMT(*m_a2b, 2, 2);*/
+}
+
+/** Composition (multiplication) of two rotation matrices.
+ * m_a2c = m_a2b comp m_b2c , aka  m_a2c = m_b2c * m_a2b
+ */
+void double_rmat_comp(struct DoubleRMat *m_a2c, struct DoubleRMat *m_a2b, struct DoubleRMat *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];
+  m_a2c->m[1] = m_b2c->m[0] * m_a2b->m[1] + m_b2c->m[1] * m_a2b->m[4] + m_b2c->m[2] * m_a2b->m[7];
+  m_a2c->m[2] = m_b2c->m[0] * m_a2b->m[2] + m_b2c->m[1] * m_a2b->m[5] + m_b2c->m[2] * m_a2b->m[8];
+  m_a2c->m[3] = m_b2c->m[3] * m_a2b->m[0] + m_b2c->m[4] * m_a2b->m[3] + m_b2c->m[5] * m_a2b->m[6];
+  m_a2c->m[4] = m_b2c->m[3] * m_a2b->m[1] + m_b2c->m[4] * m_a2b->m[4] + m_b2c->m[5] * m_a2b->m[7];
+  m_a2c->m[5] = m_b2c->m[3] * m_a2b->m[2] + m_b2c->m[4] * m_a2b->m[5] + m_b2c->m[5] * m_a2b->m[8];
+  m_a2c->m[6] = m_b2c->m[6] * m_a2b->m[0] + m_b2c->m[7] * m_a2b->m[3] + m_b2c->m[8] * m_a2b->m[6];
+  m_a2c->m[7] = m_b2c->m[6] * m_a2b->m[1] + m_b2c->m[7] * m_a2b->m[4] + m_b2c->m[8] * m_a2b->m[7];
+  m_a2c->m[8] = m_b2c->m[6] * m_a2b->m[2] + m_b2c->m[7] * m_a2b->m[5] + m_b2c->m[8] * m_a2b->m[8];
+}
+
+/** rotate 3D vector by rotation matrix.
+ * vb = m_a2b * va
+ */
+void double_rmat_vmult(struct DoubleVect3 *vb, struct DoubleRMat *m_a2b, struct DoubleVect3 *va)
+{
+  vb->x = m_a2b->m[0] * va->x + m_a2b->m[1] * va->y + m_a2b->m[2] * va->z;
+  vb->y = m_a2b->m[3] * va->x + m_a2b->m[4] * va->y + m_a2b->m[5] * va->z;
+  vb->z = m_a2b->m[6] * va->x + m_a2b->m[7] * va->y + m_a2b->m[8] * va->z;
+}
+
+/* C n->b rotation matrix */
+void double_rmat_of_quat(struct DoubleRMat *rm, struct DoubleQuat *q)
+{
+  const double _a = M_SQRT2 * q->qi;
+  const double _b = M_SQRT2 * q->qx;
+  const double _c = M_SQRT2 * q->qy;
+  const double _d = M_SQRT2 * q->qz;
+  const double a2_1 = _a * _a - 1;
+  const double ab = _a * _b;
+  const double ac = _a * _c;
+  const double ad = _a * _d;
+  const double bc = _b * _c;
+  const double bd = _b * _d;
+  const double cd = _c * _d;
+  RMAT_ELMT(*rm, 0, 0) = a2_1 + _b * _b;
+  RMAT_ELMT(*rm, 0, 1) = bc + ad;
+  RMAT_ELMT(*rm, 0, 2) = bd - ac;
+  RMAT_ELMT(*rm, 1, 0) = bc - ad;
+  RMAT_ELMT(*rm, 1, 1) = a2_1 + _c * _c;
+  RMAT_ELMT(*rm, 1, 2) = cd + ab;
+  RMAT_ELMT(*rm, 2, 0) = bd + ac;
+  RMAT_ELMT(*rm, 2, 1) = cd - ab;
+  RMAT_ELMT(*rm, 2, 2) = a2_1 + _d * _d;
+}

sw/airborne/math/pprz_algebra_double.h

@@ -153,6 +153,29 @@ extern void double_quat_of_eulers(struct DoubleQuat *q, struct DoubleEulers *e);
 extern void double_eulers_of_quat(struct DoubleEulers *e, struct DoubleQuat *q);
 extern void double_quat_vmult(struct DoubleVect3 *v_out, struct DoubleQuat *q, struct DoubleVect3 *v_in);
 
+/** initialises a rotation matrix to identity */
+static inline void double_rmat_identity(struct DoubleRMat *rm)
+{
+  FLOAT_MAT33_DIAG(*rm, 1., 1., 1.);
+}
+
+/** Inverse/transpose of a rotation matrix.
+ * m_b2a = inv(_m_a2b) = transp(_m_a2b)
+ */
+extern void double_rmat_inv(struct DoubleRMat *m_b2a, struct DoubleRMat *m_a2b);
+
+/** Composition (multiplication) of two rotation matrices.
+ * m_a2c = m_a2b comp m_b2c , aka  m_a2c = m_b2c * m_a2b
+ */
+extern void double_rmat_comp(struct DoubleRMat *m_a2c, struct DoubleRMat *m_a2b,
+                             struct DoubleRMat *m_b2c);
+
+/** rotate 3D vector by rotation matrix.
+ * vb = m_a2b * va
+ */
+extern void double_rmat_vmult(struct DoubleVect3 *vb, struct DoubleRMat *m_a2b,
+                              struct DoubleVect3 *va);
+extern void double_rmat_of_quat(struct DoubleRMat *rm, struct DoubleQuat *q);
 static inline void double_rmat_of_eulers(struct DoubleRMat *rm, struct DoubleEulers *e)
 {
   double_rmat_of_eulers_321(rm, e);

sw/airborne/test/math/compare_utm_enu.py

@@ -8,6 +8,7 @@ PPRZ_SRC = os.getenv("PAPARAZZI_SRC", "../../../..")
 sys.path.append(PPRZ_SRC + "/sw/lib/python")
 
 from pprz_math.geodetic import *
+from pprz_math.algebra import DoubleRMat
 import matplotlib.pyplot as plt
 import numpy as np
 

sw/lib/python/pprz_math/pprz_algebra_double.i

@@ -177,6 +177,51 @@ struct DoubleEulers {
   }
 };
 
+%extend DoubleRMat {
+  char *__str__() {
+    static char tmp[1024];
+    sprintf(tmp,"RMat[% .5f, % .5f, % .5f]\n    [% .5f, % .5f, % .5f]\n    [% .5f, % .5f, % .5f]",
+            $self->m[0], $self->m[1], $self->m[2], $self->m[3], $self->m[4],
+            $self->m[5], $self->m[6], $self->m[7], $self->m[8]);
+    return tmp;
+  }
+  DoubleRMat() {
+    struct DoubleRMat *rm = (struct DoubleRMat *) malloc(sizeof(struct DoubleRMat));
+    double_rmat_identity(rm);
+    return rm;
+  }
+  struct DoubleRMat __mul__(struct DoubleRMat *m_b2c) {
+    struct DoubleRMat m_a2c;
+    double_rmat_comp(&m_a2c, $self, m_b2c);
+    return m_a2c;
+  }
+  struct DoubleRMat __mul__(struct DoubleQuat *q) {
+    struct DoubleRMat m_b2c;
+    double_rmat_of_quat(&m_b2c, q);
+    struct DoubleRMat m_a2c;
+    double_rmat_comp(&m_a2c, $self, &m_b2c);
+    return m_a2c;
+  }
+  struct DoubleVect3 __mul__(struct DoubleVect3 *va) {
+    struct DoubleVect3 v;
+    double_rmat_vmult(&v, $self, va);
+    return v;
+  }
+  void set_identity() {
+    double_rmat_identity($self);
+  }
+  struct DoubleRMat inverse() {
+    struct DoubleRMat inv;
+    double_rmat_inv(&inv, $self);
+    return inv;
+  }
+  struct DoubleRMat transposed() {
+    struct DoubleRMat inv;
+    double_rmat_inv(&inv, $self);
+    return inv;
+  }
+};
+
 %extend DoubleEulers {
   char *__str__() {
     static char tmp[1024];

sw/lib/python/pprz_math/pprz_algebra_float.i

@@ -235,6 +235,9 @@ struct FloatEulers {
     float_quat_of_rmat(&q, $self);
     return q;
   }
+  double reorthogonalize() {
+    return float_rmat_reorthogonalize($self);
+  }
 };
 
 %extend FloatEulers {