| 7 |  | void SquareMatrix3TestCase::testSetupRotationMatrix(){ | 
| 8 |  | //test setupRotationMatrix by quaternion | 
| 9 |  |  | 
| 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 | + | CPPUNIT_ASSERT(m1L == m1R); | 
| 23 | + |  | 
| 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 | + | RotMat3x3d m4L(0.0, M_PI/2.0, 0.0); | 
| 45 | + | RotMat3x3d m4R = m3R; | 
| 46 | + | CPPUNIT_ASSERT( m4L == m4R); | 
| 47 | + |  | 
| 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::testOtherMemberFunctions() { | 
| 67 |  | //test inverse | 
| 68 | + | RotMat3x3d ident = RotMat3x3d::identity(); | 
| 69 | + | CPPUNIT_ASSERT(ident == ident.inverse()); | 
| 70 |  |  | 
| 71 | < |  | 
| 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 |  | } |