| 4 | 
  | 
CPPUNIT_TEST_SUITE_REGISTRATION( SquareMatrix3TestCase ); | 
| 5 | 
  | 
 | 
| 6 | 
  | 
 | 
| 7 | 
< | 
void SquareMatrix3TestCase::setUp(){ | 
| 8 | 
< | 
} | 
| 7 | 
> | 
void SquareMatrix3TestCase::testSetupRotationMatrix(){ | 
| 8 | 
> | 
    //test setupRotationMatrix by quaternion | 
| 9 | 
  | 
 | 
| 10 | 
< | 
void SquareMatrix3TestCase::tearDown(){ | 
| 11 | 
< | 
} | 
| 10 | 
> | 
    RotMat3x3d m1L(0.0, -0.6, 0.0, -0.8); | 
| 11 | 
> | 
    RotMat3x3d m1R; | 
| 12 | 
> | 
    m1R(0,0) = -0.28; | 
| 13 | 
> | 
    m1R(0,1) =  0; | 
| 14 | 
> | 
    m1R(0,2) =  0.96; | 
| 15 | 
> | 
    m1R(1,0) =  0.0; | 
| 16 | 
> | 
    m1R(1,1) = -1.0; | 
| 17 | 
> | 
    m1R(1,2) =  0.0; | 
| 18 | 
> | 
    m1R(2,0) = 0.96; | 
| 19 | 
> | 
    m1R(2,1) = 0; | 
| 20 | 
> | 
    m1R(2,2) = 0.28; | 
| 21 | 
  | 
 | 
| 22 | 
< | 
void SquareMatrix3TestCase::testConstructors(){ | 
| 14 | 
< | 
} | 
| 22 | 
> | 
    CPPUNIT_ASSERT(m1L == m1R); | 
| 23 | 
  | 
 | 
| 24 | 
< | 
void SquareMatrix3TestCase::testArithmetic(){ | 
| 25 | 
< | 
} | 
| 24 | 
> | 
    Quat4d q1(0.0, -0.6, 0.0, -0.8); | 
| 25 | 
> | 
    RotMat3x3d m2L(q1); | 
| 26 | 
> | 
    RotMat3x3d m2R(m1R); | 
| 27 | 
> | 
    CPPUNIT_ASSERT(m2L == m2R); | 
| 28 | 
> | 
     | 
| 29 | 
> | 
    //test setupRotationMatrix by euler angles | 
| 30 | 
> | 
    Vector3d v1(0.0, M_PI/2.0, 0.0); | 
| 31 | 
> | 
    RotMat3x3d m3L(v1); | 
| 32 | 
> | 
    RotMat3x3d m3R; | 
| 33 | 
> | 
    m3R(0,0) = 1.0; | 
| 34 | 
> | 
    m3R(0,1) = 0; | 
| 35 | 
> | 
    m3R(0,2) = 0.0; | 
| 36 | 
> | 
    m3R(1,0) = 0.0; | 
| 37 | 
> | 
    m3R(1,1) = 0.0; | 
| 38 | 
> | 
    m3R(1,2) = 1.0; | 
| 39 | 
> | 
    m3R(2,0) = 0.0; | 
| 40 | 
> | 
    m3R(2,1) = -1.0; | 
| 41 | 
> | 
    m3R(2,2) = 0.0;     | 
| 42 | 
> | 
    CPPUNIT_ASSERT( m3L == m3R); | 
| 43 | 
  | 
 | 
| 44 | 
< | 
void SquareMatrix3TestCase::testOperators(){ | 
| 45 | 
< | 
} | 
| 44 | 
> | 
    RotMat3x3d m4L(0.0, M_PI/2.0, 0.0); | 
| 45 | 
> | 
    RotMat3x3d m4R = m3R;     | 
| 46 | 
> | 
    CPPUNIT_ASSERT( m4L == m4R);     | 
| 47 | 
  | 
 | 
| 48 | 
< | 
void SquareMatrix3TestCase::testAccessEntries(){ | 
| 48 | 
> | 
    Vector3d v2(M_PI/4.0, M_PI/4.0, M_PI/4.0); | 
| 49 | 
> | 
    RotMat3x3d m5L(v2); | 
| 50 | 
> | 
    RotMat3x3d m5R; | 
| 51 | 
> | 
    double root2Over4 = sqrt(2)/4.0; | 
| 52 | 
> | 
    m5R(0,0) = 0.5 - root2Over4; | 
| 53 | 
> | 
    m5R(0,1) = 0.5 + root2Over4; | 
| 54 | 
> | 
    m5R(0,2) = 0.5; | 
| 55 | 
> | 
    m5R(1,0) = -0.5 -root2Over4; | 
| 56 | 
> | 
    m5R(1,1) = -0.5 + root2Over4; | 
| 57 | 
> | 
    m5R(1,2) = 0.5; | 
| 58 | 
> | 
    m5R(2,0) = 0.5; | 
| 59 | 
> | 
    m5R(2,1) = -0.5; | 
| 60 | 
> | 
    m5R(2,2) = sqrt(2)/2.0;     | 
| 61 | 
> | 
    CPPUNIT_ASSERT( m5L == m5R); | 
| 62 | 
> | 
 | 
| 63 | 
> | 
     | 
| 64 | 
  | 
} | 
| 65 | 
  | 
 | 
| 66 | 
< | 
void SquareMatrix3TestCase::testOtherTemplateFunctions(){         | 
| 66 | 
> | 
void SquareMatrix3TestCase::testOtherMemberFunctions() { | 
| 67 | 
> | 
    //test inverse | 
| 68 | 
> | 
    RotMat3x3d ident = RotMat3x3d::identity(); | 
| 69 | 
> | 
    CPPUNIT_ASSERT(ident == ident.inverse()); | 
| 70 | 
  | 
 | 
| 71 | 
+ | 
    RotMat3x3d m1; | 
| 72 | 
+ | 
    m1(0,0) = 1.0; | 
| 73 | 
+ | 
    m1(0,1) = 5.0; | 
| 74 | 
+ | 
    m1(0,2) = 3.0; | 
| 75 | 
+ | 
    m1(1,0) = 3.0; | 
| 76 | 
+ | 
    m1(1,1) = 1.0; | 
| 77 | 
+ | 
    m1(1,2) = 2.0; | 
| 78 | 
+ | 
    m1(2,0) = 0.0; | 
| 79 | 
+ | 
    m1(2,1) = -21.0; | 
| 80 | 
+ | 
    m1(2,2) = -81.0;  | 
| 81 | 
+ | 
     | 
| 82 | 
+ | 
    CPPUNIT_ASSERT(m1 == (m1.inverse()).inverse()); | 
| 83 | 
+ | 
     | 
| 84 | 
+ | 
    //test determinant | 
| 85 | 
+ | 
    RotMat3x3d m2; | 
| 86 | 
+ | 
    m2(0,0) = 1.0; | 
| 87 | 
+ | 
    m2(0,1) = 5.0; | 
| 88 | 
+ | 
    m2(0,2) = 3.0; | 
| 89 | 
+ | 
    m2(1,0) = 6.0; | 
| 90 | 
+ | 
    m2(1,1) = 0.0; | 
| 91 | 
+ | 
    m2(1,2) = 2.0; | 
| 92 | 
+ | 
    m2(2,0) = 0.0; | 
| 93 | 
+ | 
    m2(2,1) = -1.0; | 
| 94 | 
+ | 
    m2(2,2) = 1.0;  | 
| 95 | 
+ | 
    CPPUNIT_ASSERT_DOUBLES_EQUAL(m2.determinant(), -46.0, oopse::epsilon); | 
| 96 | 
  | 
} | 
| 97 | 
+ | 
void SquareMatrix3TestCase::testTransformation(){ | 
| 98 | 
  | 
 | 
| 99 | 
+ | 
    //test toQuaternion | 
| 100 | 
+ | 
    RotMat3x3d m1; | 
| 101 | 
+ | 
    Quat4d q1L; | 
| 102 | 
+ | 
    Quat4d q1R(0.0, -0.6, 0.0, -0.8); | 
| 103 | 
+ | 
    m1(0,0) = -0.28; | 
| 104 | 
+ | 
    m1(0,1) =  0; | 
| 105 | 
+ | 
    m1(0,2) =  0.96; | 
| 106 | 
+ | 
    m1(1,0) =  0.0; | 
| 107 | 
+ | 
    m1(1,1) = -1.0; | 
| 108 | 
+ | 
    m1(1,2) =  0.0; | 
| 109 | 
+ | 
    m1(2,0) = 0.96; | 
| 110 | 
+ | 
    m1(2,1) = 0; | 
| 111 | 
+ | 
    m1(2,2) = 0.28; | 
| 112 | 
+ | 
    q1L = m1.toQuaternion(); | 
| 113 | 
+ | 
    //CPPUNIT_ASSERT( q1L == q1R);     | 
| 114 | 
+ | 
 | 
| 115 | 
+ | 
    RotMat3x3d m2; | 
| 116 | 
+ | 
    Quat4d q2L(0.4, -0.6, 0.3, -0.8); | 
| 117 | 
+ | 
    Quat4d q2R; | 
| 118 | 
+ | 
    q2L.normalize(); | 
| 119 | 
+ | 
 | 
| 120 | 
+ | 
    m2 = q2L.toRotationMatrix3();     | 
| 121 | 
+ | 
    q2R = m2.toQuaternion(); | 
| 122 | 
+ | 
    CPPUNIT_ASSERT( q2L == q2R);     | 
| 123 | 
+ | 
     | 
| 124 | 
+ | 
    //test toEuler | 
| 125 | 
+ | 
    Vector3d v1L; | 
| 126 | 
+ | 
    Vector3d v1R(M_PI/4.0, M_PI/4.0, M_PI/4.0); | 
| 127 | 
+ | 
    RotMat3x3d m3; | 
| 128 | 
+ | 
    double root2Over4 = sqrt(2)/4.0; | 
| 129 | 
+ | 
    m3(0,0) = 0.5 - root2Over4; | 
| 130 | 
+ | 
    m3(0,1) = 0.5 + root2Over4; | 
| 131 | 
+ | 
    m3(0,2) = 0.5; | 
| 132 | 
+ | 
    m3(1,0) = -0.5 -root2Over4; | 
| 133 | 
+ | 
    m3(1,1) = -0.5 + root2Over4; | 
| 134 | 
+ | 
    m3(1,2) = 0.5; | 
| 135 | 
+ | 
    m3(2,0) = 0.5; | 
| 136 | 
+ | 
    m3(2,1) = -0.5; | 
| 137 | 
+ | 
    m3(2,2) = sqrt(2)/2.0;     | 
| 138 | 
+ | 
    v1L = m3.toEulerAngles(); | 
| 139 | 
+ | 
    CPPUNIT_ASSERT( v1L == v1R); | 
| 140 | 
+ | 
 | 
| 141 | 
+ | 
    //test diagonalize | 
| 142 | 
+ | 
 | 
| 143 | 
+ | 
    RotMat3x3d m4; | 
| 144 | 
+ | 
    RotMat3x3d a; | 
| 145 | 
+ | 
    Vector3d w; | 
| 146 | 
+ | 
    RotMat3x3d v; | 
| 147 | 
+ | 
    m4(0,0) = 1.0; | 
| 148 | 
+ | 
    m4(0,1) = 5.0; | 
| 149 | 
+ | 
    m4(0,2) = 3.0; | 
| 150 | 
+ | 
    m4(1,0) = 3.0; | 
| 151 | 
+ | 
    m4(1,1) = 1.0; | 
| 152 | 
+ | 
    m4(1,2) = 2.0; | 
| 153 | 
+ | 
    m4(2,0) = 0.0; | 
| 154 | 
+ | 
    m4(2,1) = -21.0; | 
| 155 | 
+ | 
    m4(2,2) = -81.0;  | 
| 156 | 
+ | 
    m4.diagonalize(a, w, v); | 
| 157 | 
+ | 
 | 
| 158 | 
+ | 
    std::cout << std::endl; | 
| 159 | 
+ | 
    std::cout << a << std::endl;     | 
| 160 | 
+ | 
 | 
| 161 | 
+ | 
    std::cout << std::endl; | 
| 162 | 
+ | 
    std::cout << w << std::endl;     | 
| 163 | 
+ | 
 | 
| 164 | 
+ | 
    std::cout << std::endl; | 
| 165 | 
+ | 
    std::cout << v << std::endl;     | 
| 166 | 
+ | 
 | 
| 167 | 
+ | 
} |