altos: Add some comments describing quaternion multiplication

[fw/altos] / src / core / ao_quaternion.h

diff --git a/src/core/ao_quaternion.h b/src/core/ao_quaternion.h
index f4b8aaa49a40f99ea572521722c523e7055096f7..6a701a1877aca163decc06ccd58578b5f8952873 100644 (file)
--- a/src/core/ao_quaternion.h
+++ b/src/core/ao_quaternion.h
@@ -26,11 +26,44 @@ struct ao_quaternion {
 };
 
 static inline void ao_quaternion_multiply(struct ao_quaternion *r,
-                                         struct ao_quaternion *a,
-                                         struct ao_quaternion *b)
+                                         const struct ao_quaternion *a,
+                                         const struct ao_quaternion *b)
 {
        struct ao_quaternion    t;
 #define T(_a,_b)       (((a)->_a) * ((b)->_b))
+
+/*
+ * Quaternions
+ *
+ *     ii = jj = kk = ijk = -1;
+ *
+ *     kji = 1;
+ *
+ *     ij = k;         ji = -k;
+ *     kj = -i;        jk = i;
+ *     ik = -j;        ki = j;
+ *
+ * Multiplication p * q:
+ *
+ *     (pr + ipx + jpy + kpz) (qr + iqx + jqy + kqz) =
+ *
+ *             ( pr * qr +  pr * iqx +  pr * jqy +  pr * kqz) +
+ *             (ipx * qr + ipx * iqx + ipx * jqy + ipx * kqz) +
+ *             (jpy * qr + jpy * iqx + jpy * jqy + jpy * kqz) +
+ *             (kpz * qr + kpz * iqx + kpz * jqy + kpz * kqz) =
+ *
+ *
+ *              (pr * qr) + i(pr * qx) + j(pr * qy) + k(pr * qz) +
+ *             i(px * qr) -  (px * qx) + k(px * qy) - j(px * qz) +
+ *             j(py * qr) - k(py * qx) -  (py * qy) + i(py * qz) +
+ *             k(pz * qr) + j(pz * qx) - i(pz * qy) -  (pz * qz) =
+ *
+ *             1 * ( (pr * qr) - (px * qx) - (py * qy) - (pz * qz) ) +
+ *             i * ( (pr * qx) + (px * qr) + (py * qz) - (pz * qy) ) +
+ *             j * ( (pr * qy) - (px * qz) + (py * qr) + (pz * qx) ) +
+ *             k * ( (pr * qz) + (px * qy) - (py * qx) + (pz * qr);
+ */
+
        t.r = T(r,r) - T(x,x) - T(y,y) - T(z,z);
        t.x = T(r,x) + T(x,r) + T(y,z) - T(z,y);
        t.y = T(r,y) - T(x,z) + T(y,r) + T(z,x);
@@ -40,7 +73,7 @@ static inline void ao_quaternion_multiply(struct ao_quaternion *r,
 }
 
 static inline void ao_quaternion_conjugate(struct ao_quaternion *r,
-                                          struct ao_quaternion *a)
+                                          const struct ao_quaternion *a)
 {
        r->r = a->r;
        r->x = -a->x;
@@ -48,7 +81,7 @@ static inline void ao_quaternion_conjugate(struct ao_quaternion *r,
        r->z = -a->z;
 }
 
-static inline float ao_quaternion_normal(struct ao_quaternion *a)
+static inline float ao_quaternion_normal(const struct ao_quaternion *a)
 {
 #define S(_a)  (((a)->_a) * ((a)->_a))
        return S(r) + S(x) + S(y) + S(z);
@@ -56,7 +89,7 @@ static inline float ao_quaternion_normal(struct ao_quaternion *a)
 }
 
 static inline void ao_quaternion_scale(struct ao_quaternion *r,
-                                      struct ao_quaternion *a,
+                                      const struct ao_quaternion *a,
                                       float b)
 {
        r->r = a->r * b;
@@ -66,7 +99,7 @@ static inline void ao_quaternion_scale(struct ao_quaternion *r,
 }
 
 static inline void ao_quaternion_normalize(struct ao_quaternion *r,
-                                          struct ao_quaternion *a)
+                                          const struct ao_quaternion *a)
 {
        float   n = ao_quaternion_normal(a);