Updated vector class.

matrix/Quaternion.hpp

@@ -64,7 +64,7 @@ public:
                cosPhi_2 * sinTheta_2 * sinPsi_2;
         q(2) = cosPhi_2 * sinTheta_2 * cosPsi_2 +
                sinPhi_2 * cosTheta_2 * sinPsi_2;
-        q(3) = cosPhi_2 * cosTheta_2 * sinPsi_2 +
+        q(3) = cosPhi_2 * cosTheta_2 * sinPsi_2 -
                sinPhi_2 * sinTheta_2 * cosPsi_2;
     }
 

matrix/Vector.hpp

@@ -12,25 +12,25 @@
 namespace matrix
 {
 
-template<typename Type, size_t N>
-class Vector : public Matrix<Type, N, 1>
+template<typename Type, size_t M>
+class Vector : public Matrix<Type, M, 1>
 {
 public:
     virtual ~Vector() {};
 
-    Vector() : Matrix<Type, N, 1>()
+    Vector() : Matrix<Type, M, 1>()
     {
     }
 
     inline Type operator()(size_t i) const
     {
-        const Matrix<Type, N, 1> &v = *this;
+        const Matrix<Type, M, 1> &v = *this;
         return v(i, 0);
     }
 
     inline Type &operator()(size_t i)
     {
-        Matrix<Type, N, 1> &v = *this;
+        Matrix<Type, M, 1> &v = *this;
         return v(i, 0);
     }
 
@@ -47,6 +47,110 @@ public:
         Vector &a(*this);
         return sqrt(a.dot(a));
     }
+
+    /**
+     * Vector Operations
+     */
+
+    Vector<Type, M> operator+(const Vector<Type, M> &other) const
+    {
+        Vector<Type, M> res;
+        const Vector<Type, M> &self = *this;
+
+        for (size_t i = 0; i < M; i++) {
+            res(i) = self(i) + other(i);
+        }
+
+        return res;
+    }
+
+    bool operator==(const Vector<Type, M> &other) const
+    {
+        Vector<Type, M> res;
+        const Vector<Type, M> &self = *this;
+
+        for (size_t i = 0; i < M; i++) {
+            if (self(i) != other(i)) {
+                return false;
+            }
+        }
+
+        return true;
+    }
+
+    Vector<Type, M> operator-(const Vector<Type, M> &other) const
+    {
+        Vector<Type, M> res;
+        const Vector<Type, M> &self = *this;
+
+        for (size_t i = 0; i < M; i++) {
+            res(i) = self(i) - other(i);
+        }
+
+        return res;
+    }
+
+    void operator+=(const Vector<Type, M> &other)
+    {
+        Vector<Type, M> &self = *this;
+        self = self + other;
+    }
+
+    void operator-=(const Vector<Type, M> &other)
+    {
+        Vector<Type, M> &self = *this;
+        self = self - other;
+    }
+
+    void operator*=(const Vector<Type, M> &other)
+    {
+        Vector<Type, M> &self = *this;
+        self = self * other;
+    }
+
+    /**
+     * Scalar Operations
+     */
+
+    Vector<Type, M> operator*(Type scalar) const
+    {
+        Vector<Type, M> res;
+        const Vector<Type, M> &self = *this;
+
+        for (size_t i = 0; i < M; i++) {
+            res(i) = self(i) * scalar;
+        }
+
+        return res;
+    }
+
+    Vector<Type, M> operator+(Type scalar) const
+    {
+        Vector<Type, M> res;
+        Vector<Type, M> &self = *this;
+
+        for (size_t i = 0; i < M; i++) {
+            res(i) = self(i) + scalar;
+        }
+
+        return res;
+    }
+
+    void operator*=(Type scalar)
+    {
+        Vector<Type, M> &self = *this;
+
+        for (size_t i = 0; i < M; i++) {
+            self(i) = self(i) * scalar;
+        }
+    }
+
+    void operator/=(Type scalar)
+    {
+        Vector<Type, M> &self = *this;
+        self = self * (1.0f / scalar);
+    }
+
 };
 
 }; // namespace matrix

test/quaternion.cpp

@@ -6,16 +6,32 @@ using namespace matrix;
 
 int main()
 {
-    Quatf p(1, 2, 3, 4);
-    assert(p(0) == 1);
-    assert(p(1) == 2);
-    assert(p(2) == 3);
-    assert(p(3) == 4);
+    // test default ctor
+    Quatf q;
+    assert(q(0) == 1);
+    assert(q(1) == 0);
+    assert(q(2) == 0);
+    assert(q(3) == 0);
+    
+    q = Quatf(0.1825742f, 0.3651484f, 0.5477226f, 0.7302967f);
+    assert(q(0) == 0.1825742f);
+    assert(q(1) == 0.3651484f);
+    assert(q(2) == 0.5477226f);
+    assert(q(3) == 0.7302967f);
 
-    Quatf q(0, 1, 0, 0);
-    Quatf r = p*q;
-    Dcmf dcm = Dcmf(p);
-    Eulerf e = Eulerf(p);
+    // test euler ctor
+    q = Quatf(Eulerf(0.1f, 0.2f, 0.3f));
+    assert((q - Quatf(0.983347f, 0.034271f, 0.106021f, 0.143572f)).norm() < 1e-3);
+    // test dcm ctor
+    //q = Quatf(Dcmf());
+    //assert(q == Quatf(1.0f, 0.0f, 0.0f, 0.0f));
+    // TODO test derivative
+    // test accessors
+    //q(0) = 0.1f;
+    //q(1) = 0.2f;
+    //q(2) = 0.3f;
+    //q(3) = 0.4f;
+    //assert(q == Quatf(0.1f, 0.2f, 0.3f, 0.4f));
     return 0;
 }
 

test/test_math.py

@@ -0,0 +1,66 @@
+from __future__ import print_function
+from pylab import *
+from pprint import pprint
+
+#pylint: disable=all
+
+phi = 0.1
+theta = 0.2
+psi = 0.3
+
+cosPhi = cos(phi)
+cosPhi_2 = cos(phi/2)
+sinPhi = sin(phi)
+sinPhi_2 = sin(phi/2)
+
+cosTheta = cos(theta)
+cosTheta_2 = cos(theta/2)
+sinTheta = sin(theta)
+sinTheta_2 = sin(theta/2)
+
+cosPsi = cos(psi)
+cosPsi_2 = cos(psi/2)
+sinPsi = sin(psi)
+sinPsi_2 = sin(psi/2)
+
+C_nb = array([
+    [cosTheta*cosPsi, -cosPhi*sinPsi + sinPhi*sinTheta*cosPsi, sinPhi*sinPsi + cosPhi*sinTheta*cosPsi],
+    [cosTheta*sinPsi, cosPhi*cosPsi + sinPhi*sinTheta*sinPsi, -sinPhi*cosPsi + cosPhi*sinTheta*sinPsi],
+    [-sinTheta, sinPhi*cosTheta, cosPhi*cosTheta]])
+    
+print('\nC_nb')
+pprint(C_nb)
+
+theta = arcsin(-C_nb[2,0])
+phi = arctan(C_nb[2,1]/ C_nb[2,2])
+psi = arctan(C_nb[1,0]/ C_nb[0,0])
+print('\nphi {:f}, theta {:f}, psi {:f}\n'.format(phi, theta, psi))
+
+q = array([
+    cosPhi_2*cosTheta_2*cosPsi_2 + sinPhi_2*sinTheta_2*sinPsi_2,
+    sinPhi_2*cosTheta_2*cosPsi_2 - cosPhi_2*sinTheta_2*sinPsi_2,
+    cosPhi_2*sinTheta_2*cosPsi_2 + sinPhi_2*cosTheta_2*sinPsi_2,
+    cosPhi_2*cosTheta_2*sinPsi_2 - sinPhi_2*sinTheta_2*cosPsi_2])
+     
+a = q[0]
+b = q[1]
+c = q[2]
+d = q[3]
+
+print('\nq')
+pprint(q.T)
+
+a2 = a*a
+b2 = b*b
+c2 = c*c
+d2 = d*d
+
+C2_nb = array([
+    [a2 + b2 - c2 - d2, 2*(b*c - a*d), 2*(b*d + a*c)],
+    [2*(b*c + a*d), a2 - b2 + c2 - d2, 2*(c*d - a*b)],
+    [2*(b*d - a*c), 2*(c*d + a*b), a2 - b2 - c2 + d2]])
+
+print('\nC2_nb')
+pprint(C2_nb)
+
+# vim: set et ft=python fenc=utf-8 ff=unix sts=4 sw=4 ts=8 :