# | Line 7 | Line 7 | void SquareMatrix3TestCase::testSetupRotationMatrix(){ | |
---|---|---|
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 | } |
– | Removed lines |
+ | Added lines |
< | Changed lines |
> | Changed lines |