Support more of mathlib function for easier swap in px4.

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;
 }