# | Line 4 | Line 4 | CPPUNIT_TEST_SUITE_REGISTRATION( SquareMatrix3TestCase | |
---|---|---|
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 m5L; |
147 | + | RotMat3x3d m5R; |
148 | + | m4(0, 0) = 3.0; |
149 | + | m4(0, 1) = 4.0; |
150 | + | m4(0, 2) = 5.0; |
151 | + | m4(1, 0) = 4.0; |
152 | + | m4(1, 1) = 5.0; |
153 | + | m4(1, 2) = 6.0; |
154 | + | m4(2, 0) = 5.0; |
155 | + | m4(2, 1) = 6.0; |
156 | + | m4(2, 2) = 7.0; |
157 | + | a = m4; |
158 | + | |
159 | + | RotMat3x3d::diagonalize(a, w, m5L); |
160 | + | |
161 | + | m5R(0, 0) = 0.789067 ; |
162 | + | m5R(0, 1) = -0.408248; |
163 | + | m5R(0, 2) = 0.459028; |
164 | + | m5R(1, 0) = 0.090750; |
165 | + | m5R(1, 1) = 0.816497; |
166 | + | m5R(1, 2) = 0.570173; |
167 | + | m5R(2, 0) = -0.607567; |
168 | + | m5R(2, 1) = -0.408248 ; |
169 | + | m5R(2, 2) = 0.681319; |
170 | + | |
171 | + | CPPUNIT_ASSERT(m5L == m5R); |
172 | + | } |
– | Removed lines |
+ | Added lines |
< | Changed lines |
> | Changed lines |