Support more of mathlib function for easier swap in px4.

matrix/Matrix.hpp

@@ -41,6 +41,12 @@ public:
     {
     }
 
+    Matrix(const Type data_[][N]) :
+        _data()
+    {
+        memcpy(_data, data_, sizeof(_data));
+    }
+
     Matrix(const Type *data_) :
         _data()
     {
@@ -120,6 +126,20 @@ public:
         return res;
     }
 
+    Matrix<Type, M, N> edivide(const Matrix<Type, M, N> &other) const
+    {
+        Matrix<Type, M, N> res;
+        const Matrix<Type, M, N> &self = *this;
+
+        for (size_t i = 0; i < M; i++) {
+            for (size_t j = 0; j < N; j++) {
+                res(i , j) = self(i, j)/other(i, j);
+            }
+        }
+
+        return res;
+    }
+
     Matrix<Type, M, N> operator+(const Matrix<Type, M, N> &other) const
     {
         Matrix<Type, M, N> res;
@@ -334,6 +354,11 @@ public:
         memset(_data, 0, sizeof(_data));
     }
 
+    inline void zero()
+    {
+        setZero();
+    }
+
     void setAll(Type val)
     {
         Matrix<Type, M, N> &self = *this;
@@ -360,6 +385,11 @@ public:
         }
     }
 
+    inline void identity()
+    {
+        setIdentity();
+    }
+
     inline void swapRows(size_t a, size_t b)
     {
         if (a == b) {

matrix/Quaternion.hpp

@@ -292,6 +292,20 @@ public:
         (*this) = (*this) * res;
     }
 
+    Vector3f conjugate(const Vector3f &vec) {
+        Quaternion q = *this;
+        Quaternion v(0, vec(0), vec(1), vec(2));
+        Quaternion res = q*v*q.inversed();
+        return Vector3f(res(1), res(2), res(3));
+    }
+
+    Vector3f conjugate_inversed(const Vector3f &vec) {
+        Quaternion q = *this;
+        Quaternion v(0, vec(0), vec(1), vec(2));
+        Quaternion res = q.inversed()*v*q;
+        return Vector3f(res(1), res(2), res(3));
+    }
+
     /**
      * Rotation quaternion from vector
      *
@@ -367,6 +381,30 @@ public:
 
         return vec;
     }
+
+    /**
+     * Imaginary components of quaternion
+     */
+    Vector3<Type> imag()
+    {
+        Quaternion &q = *this;
+        return Vector3<Type>(q(1), q(2), q(3));
+    }
+
+    /**
+     * XXX DEPRECATED, can use assignment or ctor
+     */
+    Quaternion from_dcm(Matrix<Type, 3, 3> dcm) {
+        return Quaternion(Dcmf(dcm));
+    }
+
+    /**
+     * XXX DEPRECATED, can use assignment or ctor
+     */
+    Dcm<Type> to_dcm() {
+        return Dcm<Type>(*this);
+    }
+
 };
 
 typedef Quaternion<float> Quatf;

matrix/Vector.hpp

@@ -61,11 +61,24 @@ public:
         return r;
     }
 
+    inline Type operator*(const MatrixM1 & b) const {
+        const Vector &a(*this);
+        return a.dot(b);
+    }
+
+    inline Vector operator*(float b) const {
+        return Vector(MatrixM1::operator*(b));
+    }
+
     Type norm() const {
         const Vector &a(*this);
         return Type(sqrt(a.dot(a)));
     }
 
+    inline Type length() const {
+        return norm();
+    }
+
     inline void normalize() {
         (*this) /= norm();
     }
@@ -74,6 +87,10 @@ public:
         return (*this) / norm();
     }
 
+    inline Vector normalized() const {
+        return unit();
+    }
+
     Vector pow(Type v) const {
         const Vector &a(*this);
         Vector r;

matrix/Vector3.hpp

@@ -13,6 +13,9 @@
 namespace matrix
 {
 
+template <typename Type, size_t M, size_t N>
+class Matrix;
+
 template <typename Type, size_t M>
 class Vector;
 
@@ -60,10 +63,51 @@ public:
         return c;
     }
 
-    Vector3 operator%(const Matrix31 & b) const {
+    /**
+     * Override matrix ops so Vector3 type is returned
+     */
+
+    inline Vector3 operator+(Vector3 other) const
+    {
+        return Matrix31::operator+(other);
+    }
+
+    inline Vector3 operator-(Vector3 other) const
+    {
+        return Matrix31::operator-(other);
+    }
+
+    inline Vector3 operator-() const
+    {
+        return Matrix31::operator-();
+    }
+
+    inline Vector3 operator*(Type scalar) const
+    {
+        return Matrix31::operator*(scalar);
+    }
+
+    inline Type operator*(Vector3 b) const
+    {
+        return Vector<Type, 3>::operator*(b);
+    }
+
+    inline Vector3 operator%(const Matrix31 & b) const {
         return (*this).cross(b);
     }
 
+    /**
+     * Override vector ops so Vector3 type is returned
+     */
+    inline Vector3 unit() const {
+        return Vector3(Vector<Type, 3>::unit());
+    }
+
+    inline Vector3 normalized() const {
+        return unit();
+    }
+
+
     Dcm<Type> hat() const {    // inverse to Dcm.vee() operation
         const Vector3 &v(*this);
         Dcm<Type> A;

test/attitude.cpp

@@ -61,6 +61,7 @@ int main()
     TEST(isEqual(q, Quatf(0.18257419f,  0.36514837f,
                           0.54772256f,  0.73029674f)));
     TEST(isEqual(q0.unit(), q));
+    TEST(isEqual(q0.unit(), q0.normalized()));
 
     // quat default ctor
     q = Quatf();
@@ -265,6 +266,19 @@ int main()
     TEST(isEqual(aa_data_init, AxisAnglef(4.0f, 5.0f, 6.0f)));
 
     q = Quatf(-0.29555112749297824f, 0.25532186f,  0.51064372f,  0.76596558f);
+    TEST(isEqual(q.imag(), Vector3f(0.25532186f,  0.51064372f,  0.76596558f)));
+
+    // from dcm
+    TEST(isEqual(Eulerf(q.from_dcm(Dcmf(q))), Eulerf(q)));
+
+    // to dcm
+    TEST(isEqual(Dcmf(q), q.to_dcm()));
+
+    // conjugate
+    Vector3f v1(1.5f, 2.2f, 3.2f);
+    TEST(isEqual(q.conjugate(v1), Dcmf(q)*v1));
+    TEST(isEqual(q.conjugate_inversed(v1), Dcmf(q).T()*v1));
+
     AxisAnglef aa_q_init(q);
     TEST(isEqual(aa_q_init, AxisAnglef(1.0f, 2.0f, 3.0f)));
 
@@ -285,7 +299,7 @@ int main()
     Dcmf dcm3(Eulerf(1, 2, 3));
     Dcmf dcm4(Eulerf(4, 5, 6));
     Dcmf dcm34 = dcm3*dcm4;
-    TEST(isEqual(Eulerf(Quatf(dcm3)*Quatf(dcm4)), Eulerf(Quatf(dcm34))));
+    TEST(isEqual(Eulerf(Quatf(dcm3)*Quatf(dcm4)), Eulerf(dcm34)));
 };
 
 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

test/matrixAssignment.cpp

@@ -9,6 +9,7 @@ int main()
 {
     Matrix3f m;
     m.setZero();
+    m.zero();
     m(0, 0) = 1;
     m(0, 1) = 2;
     m(0, 2) = 3;
@@ -26,6 +27,15 @@ int main()
         TEST(fabs(data[i] - m2.data()[i]) < 1e-6f);
     }
 
+    float data2d[3][3] = {
+        {1, 2, 3},
+        {4, 5, 6},
+        {7, 8, 9}};
+    m2 = Matrix3f(data2d);
+    for(int i=0; i<9; i++) {
+        TEST(fabs(data[i] - m2.data()[i]) < 1e-6f);
+    }
+
     float data_times_2[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
     Matrix3f m3(data_times_2);
 

test/matrixMult.cpp

@@ -24,7 +24,10 @@ int main()
     Matrix3f A2 = eye<float, 3>()*2;
     Matrix3f B = A2.emult(A2);
     Matrix3f B_check = eye<float, 3>()*4;
+    Matrix3f C_check = eye<float, 3>()*2;
     TEST(isEqual(B, B_check));
+    Matrix3f C = B_check.edivide(C_check);
+    TEST(isEqual(C, C_check));
     return 0;
 }
 

test/setIdentity.cpp

@@ -21,6 +21,19 @@ int main()
         }
     }
 
+    Matrix3f B;
+    B.identity();
+
+    for (int i = 0; i < 3; i++) {
+        for (int j = 0; j < 3; j++) {
+            if (i == j) {
+                TEST( fabs(B(i, j) -  1) < 1e-7);
+
+            } else {
+                TEST( fabs(B(i, j) -  0) < 1e-7);
+            }
+        }
+    }
     return 0;
 }
 

test/vector.cpp

@@ -12,15 +12,22 @@ int main()
     float data1[] = {1,2,3,4,5};
     float data2[] = {6,7,8,9,10};
     Vector<float, 5> v1(data1);
-    TEST(fabs(v1.norm() - 7.416198487095663f) < 1e-5);
+    TEST(isEqualF(v1.norm(), 7.416198487095663f));
+    TEST(isEqualF(v1.norm(), v1.length()));
     Vector<float, 5> v2(data2);
-    TEST(fabs(v1.dot(v2) - 130.0f) < 1e-5);
+    TEST(isEqualF(v1.dot(v2), 130.0f));
     v2.normalize();
     Vector<float, 5> v3(v2);
     TEST(isEqual(v2, v3));
     float data1_sq[] = {1,4,9,16,25};
     Vector<float, 5> v4(data1_sq);
     TEST(isEqual(v1, v4.pow(0.5)));
+
+    // dot product operator
+    v1 = Vector<float, 5>(data1);
+    v2 = Vector<float, 5>(data2);
+    float dprod = v1 * v2;
+    TEST(isEqualF(dprod, 130.0f));
     return 0;
 }
 

test/vector3.cpp

@@ -21,6 +21,14 @@ int main()
     float data[] = {4, 5, 6};
     Vector3f f(data);
     TEST(isEqual(f, Vector3f(4, 5, 6)));
+
+    TEST(isEqual(a + b, Vector3f(1, 1, 0)));
+    TEST(isEqual(a - b, Vector3f(1, -1, 0)));
+    TEST(isEqualF(a * b, 0.0f));
+    TEST(isEqual(-a, Vector3f(-1, 0, 0)));
+    TEST(isEqual(a.unit(), a));
+    TEST(isEqual(a.unit(), a.normalized()));
+    TEST(isEqual(a*2.0, Vector3f(2, 0, 0)));
     return 0;
 }