Add cholesky decomp, Closes #30, and dynamic print buf

test/attitude.cpp

@@ -99,12 +99,12 @@ int main()
     // dcm renormalize
     Dcmf A = eye<float, 3>();
     Dcmf R(euler_check);
-    for (int i = 0; i < 1000; i++) {
+    for (size_t i = 0; i < 1000; i++) {
         A = R * A;
     }
     A.renormalize();
     float err = 0.0f;
-    for (int row = 0; row < 3; row++) {
+    for (size_t row = 0; row < 3; row++) {
         matrix::Vector3f rvec(A._data[row]);
         err += fabsf(1.0f - rvec.length());
     }
@@ -179,12 +179,18 @@ int main()
     Quatf q_from_m(m4);
     TEST(isEqual(q_from_m, m4));
 
-    // quaternion derivate
+    // quaternion derivative in frame 1
     Quatf q1(0, 1, 0, 0);
-    Vector<float, 4> q1_dot = q1.derivative(Vector3f(1, 2, 3));
-    float data_q_dot_check[] = { -0.5f, 0.0f, -1.5f, 1.0f};
-    Vector<float, 4> q1_dot_check(data_q_dot_check);
-    TEST(isEqual(q1_dot, q1_dot_check));
+    Vector<float, 4> q1_dot1 = q1.derivative1(Vector3f(1, 2, 3));
+    float data_q_dot1_check[] = { -0.5f, 0.0f, 1.5f, -1.0f};
+    Vector<float, 4> q1_dot1_check(data_q_dot1_check);
+    TEST(isEqual(q1_dot1, q1_dot1_check));
+
+    // quaternion derivative in frame 2
+    Vector<float, 4> q1_dot2 = q1.derivative2(Vector3f(1, 2, 3));
+    float data_q_dot2_check[] = { -0.5f, 0.0f, -1.5f, 1.0f};
+    Vector<float, 4> q1_dot2_check(data_q_dot2_check);
+    TEST(isEqual(q1_dot2, q1_dot2_check));
 
     // quaternion product
     Quatf q_prod_check(
@@ -220,8 +226,12 @@ int main()
     TEST(fabsf(q_check(2) + q(2)) < eps);
     TEST(fabsf(q_check(3) + q(3)) < eps);
 
-    // rotate quaternion (nonzero rotation)
+    // non-unit quaternion invese
     Quatf qI(1.0f, 0.0f, 0.0f, 0.0f);
+    Quatf q_nonunit(0.1f, 0.2f, 0.3f, 0.4f);
+    TEST(isEqual(qI, q_nonunit*q_nonunit.inversed()));
+
+    // rotate quaternion (nonzero rotation)
     Vector<float, 3> rot;
     rot(0) = 1.0f;
     rot(1) = rot(2) = 0.0f;
@@ -270,7 +280,7 @@ int main()
     float q_array[] = {0.9833f, -0.0343f, -0.1060f, -0.1436f};
     Quaternion<float>q_from_array(q_array);
 
-    for (int i = 0; i < 4; i++) {
+    for (size_t i = 0; i < 4; i++) {
         TEST(fabsf(q_from_array(i) - q_array[i]) < eps);
     }
 
@@ -333,6 +343,6 @@ int main()
     q = Quatf(0,0,0,1); // 180 degree rotation around the z axis
     R = Dcmf(q);
     TEST(isEqual(q, Quatf(R)));
-};
+}
 
 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

test/inverse.cpp

@@ -106,6 +106,26 @@ int main()
     TEST(isEqual(A3_I, Z3));
     TEST(isEqual(A3.I(), Z3));
 
+    float data4[9] = {
+        1.33471626f,  0.74946721f, -0.0531679f,
+        0.74946721f,  1.07519593f,  0.08036323f,
+        -0.0531679f,  0.08036323f,  1.01618474f
+    };
+    SquareMatrix<float, 3> A4(data4);
+
+    float data4_cholesky[9] = {
+        1.15529921f,  0.f,  0.f,
+        0.6487213f ,  0.80892311f,  0.f,
+        -0.04602089f,  0.13625271f,  0.99774847f
+    };
+    SquareMatrix<float, 3> A4_cholesky_check(data4_cholesky);
+    SquareMatrix<float, 3> A4_cholesky = cholesky(A4);
+    TEST(isEqual(A4_cholesky_check, A4_cholesky));
+
+    SquareMatrix<float, 3> I3;
+    I3.setIdentity();
+    TEST(isEqual(choleskyInv(A4)*A4, I3));
+    TEST(isEqual(cholesky(Z3), Z3));
     return 0;
 }
 

test/matrixAssignment.cpp

@@ -101,10 +101,8 @@ int main()
 
     Scalar<float> s;
     s = 1;
-    float const & s_float = (const Scalar<float>)s;
     const Vector<float, 1> & s_vect = s;
     TEST(fabs(s - 1) < 1e-5);
-    TEST(fabs(s_float - 1.0f) < 1e-5);
     TEST(fabs(s_vect(0) - 1.0f) < 1e-5);
 
     Matrix<float, 1, 1> m5 = s;

test/setIdentity.cpp

@@ -10,8 +10,8 @@ int main()
     Matrix3f A;
     A.setIdentity();
 
-    for (int i = 0; i < 3; i++) {
-        for (int j = 0; j < 3; j++) {
+    for (size_t i = 0; i < 3; i++) {
+        for (size_t j = 0; j < 3; j++) {
             if (i == j) {
                 TEST( fabs(A(i, j) -  1) < 1e-7);
 
@@ -24,8 +24,8 @@ int main()
     Matrix3f B;
     B.identity();
 
-    for (int i = 0; i < 3; i++) {
-        for (int j = 0; j < 3; j++) {
+    for (size_t i = 0; i < 3; i++) {
+        for (size_t j = 0; j < 3; j++) {
             if (i == j) {
                 TEST( fabs(B(i, j) -  1) < 1e-7);