1 /*=========================================================================
2
3 Program: Visualization Toolkit
4 Module: TestQuaternion.cxx
5
6 Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
7 All rights reserved.
8 See Copyright.txt or http://www.kitware.com/Copyright.htm for details.
9
10 This software is distributed WITHOUT ANY WARRANTY; without even
11 the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
12 PURPOSE. See the above copyright notice for more information.
13
14 =========================================================================*/
15
16 #include "vtkSetGet.h"
17 #include "vtkQuaternion.h"
18 #include "vtkMathUtilities.h"
19
20 // Pre-declarations of the test functions
21 static int TestQuaternionSetGet();
22 static int TestQuaternionNormalization();
23 static int TestQuaternionConjugationAndInversion();
24 static int TestQuaternionRotation();
25 static int TestQuaternionMatrixConversions();
26 static int TestQuaternionConversions();
27 static int TestQuaternionSlerp();
28
29 //----------------------------------------------------------------------------
TestQuaternion(int,char * [])30 int TestQuaternion(int, char*[])
31 {
32 // Store up any errors, return non-zero if something fails.
33 int retVal = 0;
34
35 retVal += TestQuaternionSetGet();
36 retVal += TestQuaternionNormalization();
37 retVal += TestQuaternionConjugationAndInversion();
38 retVal += TestQuaternionRotation();
39 retVal += TestQuaternionMatrixConversions();
40 retVal += TestQuaternionConversions();
41 retVal += TestQuaternionSlerp();
42
43 return retVal;
44 }
45
46 // Test if the access and set methods are valids
47 //----------------------------------------------------------------------------
TestQuaternionSetGet()48 int TestQuaternionSetGet() //use of vtkQuaternionf for this test
49 {
50 int retVal = 0;
51 //
52 // Test out the general vector data types, give nice API and great memory use
53 vtkQuaternionf qf(1.0f);
54 float zeroArrayf[4] = { 0.0f, 0.0f, 0.0f, 0.0f };
55 qf.Set(zeroArrayf[0], zeroArrayf[1], zeroArrayf[2], zeroArrayf[3]);
56
57 if (sizeof(qf) != sizeof(zeroArrayf))
58 {
59 // The two should be the same size and memory layout - error out if not
60 std::cerr << "vtkQuaternionf should be the same size as float[4]."
61 << std::endl
62 << "sizeof(vtkQuaternionf) = " << sizeof(qf) << std::endl
63 << "sizeof(float[4]) = " << sizeof(zeroArrayf) << std::endl;
64 ++retVal;
65 }
66 if (qf.GetSize() != 4)
67 {
68 std::cerr << "Incorrect size of vtkQuaternionf, should be 4, but is "
69 << qf.GetSize() << std::endl;
70 ++retVal;
71 }
72
73 //
74 // Test out vtkQuaternionf and ensure the various access methods are the same
75 qf.Set(0.0f, 6.0f, 9.0f, 15.0f);
76 if (qf.GetW() != qf[0]
77 || !vtkMathUtilities::FuzzyCompare<float>(qf.GetW(), 0.0f))
78 {
79 std::cerr << "qf.GetW() should equal qf.GetData()[0] which should equal 0."
80 << "\nqf.W() = " << qf.GetW() << std::endl
81 << "qf[0] = " << qf[0] << std::endl;
82 ++retVal;
83 }
84 if (qf.GetX() != qf[1]
85 || !vtkMathUtilities::FuzzyCompare<float>(qf.GetX(), 6.0f))
86 {
87 std::cerr << "qf.GetX() should equal qf.GetData()[1] "
88 << "which should equal 6.0. \nqf.GetX() = " << qf.GetX() << std::endl
89 << "qf[1] = " << qf[1] << std::endl;
90 ++retVal;
91 }
92 if (qf.GetY() != qf[2]
93 || !vtkMathUtilities::FuzzyCompare<float>(qf.GetY(), 9.0f))
94 {
95 std::cerr << "qf.GetY() should equal qf.GetData()[2]"
96 <<" which should equal 9.0.\nqf.GetY() = " << qf.GetY() <<std:: endl
97 << "qf[2] = " << qf[2] << std::endl;
98 ++retVal;
99 }
100 if (qf.GetZ() != qf[3]
101 || !vtkMathUtilities::FuzzyCompare<float>(qf.GetZ(), 15.0f))
102 {
103 std::cerr << "qf.GetZ() should equal qf.GetData()[3] "
104 << "which should equal 15.0.\nqf.Z() = " << qf.GetZ() << std::endl
105 << "qf[3] = " << qf[3] << std::endl;
106 ++retVal;
107 }
108
109 //
110 // Assign the data to an float array and ensure the two ways of
111 // referencing are the same.
112 float *floatPtr = qf.GetData();
113 for (int i = 0; i < 3; ++i)
114 {
115 if (qf[i] != floatPtr[i] || qf(i) != qf[i])
116 {
117 std::cerr << "Error: qf[i] != floatPtr[i]" << std::endl
118 << "qf[i] = " << qf[i] << std::endl
119 << "floatPtr[i] = " << floatPtr[i] << std::endl;
120 ++retVal;
121 }
122 }
123
124 //To and from float[4]
125 float setArray[4] = {1.0, -38.0, 42.0, 0.0001};
126 qf.Set(setArray);
127 if (!qf.Compare( vtkQuaternionf(1.0, -38.0, 42.0, 0.0001), 0.0001))
128 {
129 std::cerr << "Error vtkQuaterniond::Set(float[4]) failed: "
130 << qf << std::endl;
131 ++retVal;
132 }
133
134 float arrayToCompare[4];
135 qf.Get(arrayToCompare);
136 for (int i = 0; i < 4; ++i)
137 {
138 if (!vtkMathUtilities::FuzzyCompare(setArray[i], arrayToCompare[i]))
139 {
140 std::cerr << "Error vtkQuaterniond::Get(float[4]) failed: "
141 << setArray[i] << "!= " << arrayToCompare[i] << std::endl;
142 ++retVal;
143 }
144 }
145
146 return retVal;
147 }
148
149 // Test the normalize and normalized functions.
150 //----------------------------------------------------------------------------
TestQuaternionNormalization()151 int TestQuaternionNormalization() //This test use vtkQuaterniond
152 {
153 int retVal = 0;
154
155 vtkQuaterniond normy(1.0, 2.0, 3.0, 4.0);
156 vtkQuaterniond normed = normy.Normalized();
157 if (!normed.Compare(
158 vtkQuaterniond(0.182574, 0.365148, 0.547723, 0.730297),
159 0.0001))
160 {
161 std::cerr << "Error vtkQuaterniond::Normalized() failed: "
162 << normed << std::endl;
163 ++retVal;
164 }
165 normy.Normalize();
166 if (!normy.Compare(normed, 0.0001))
167 {
168 std::cerr << "Error vtkQuaterniond::Normalize() failed: "
169 << normy << std::endl;
170 }
171 if (!vtkMathUtilities::FuzzyCompare(normy.Norm(), 1.0, 0.0001))
172 {
173 std::cerr << "Normalized length should always be ~= 1.0, value is "
174 << normy.Norm() << std::endl;
175 ++retVal;
176 }
177
178 return retVal;
179 }
180
181 // This tests the conjugation and inversion at the same time.
182 // Since inversion depends on normalization, this will probably fail
183 // if TestQuaternionNormalisation() fails.
184 //----------------------------------------------------------------------------
TestQuaternionConjugationAndInversion()185 int TestQuaternionConjugationAndInversion() //this test uses vtkQuaternionf
186 {
187 int retVal = 0;
188
189 //
190 // Test conjugate and inverse at the same time.
191 // [inv(q) = conj(q)/norm2(q)]
192 vtkQuaternionf toConjugate(2.0f);
193 vtkQuaternionf conjugate = toConjugate.Conjugated();
194 if (!conjugate.Compare(
195 vtkQuaternionf(2.0f, -2.0f, -2.0f, -2.0f),
196 0.0001))
197 {
198 std::cerr << "Error vtkQuaternionf::Conjugated() failed: "
199 << conjugate << std::endl;
200 ++retVal;
201 }
202 float squaredNorm = conjugate.SquaredNorm();
203 vtkQuaternionf invToConjugate = conjugate / squaredNorm;
204 if (!invToConjugate.Compare(
205 vtkQuaternionf(0.125f, -0.125f, -0.125f, -0.125f),
206 0.0001))
207 {
208 std::cerr << "Error vtkQuaternionf Divide by Scalar() failed: "
209 << invToConjugate << std::endl;
210 ++retVal;
211 }
212
213 vtkQuaternionf shouldBeIdentity = invToConjugate * toConjugate;
214 vtkQuaternionf identity;
215 identity.ToIdentity();
216 if (!shouldBeIdentity.Compare(identity, 0.0001))
217 {
218 std::cerr << "Error vtkQuaternionf multiplication failed: "
219 << shouldBeIdentity << std::endl;
220 ++retVal;
221 }
222 toConjugate.Invert();
223 if (!invToConjugate.Compare(toConjugate, 0.0001))
224 {
225 std::cerr << "Error vtkQuaternionf::Inverse failed: "
226 << toConjugate << std::endl;
227 ++retVal;
228 }
229 shouldBeIdentity.Invert();
230 if (!shouldBeIdentity.Compare(identity, 0.0001))
231 {
232 std::cerr << "Error vtkQuaternionf::Inverse failed: "
233 << shouldBeIdentity << std::endl;
234 ++retVal;
235 }
236
237 return retVal;
238 }
239
240 // Test the rotations
241 //----------------------------------------------------------------------------
TestQuaternionRotation()242 int TestQuaternionRotation() //this test uses vtkQuaterniond
243 {
244 int retVal = 0;
245
246 //
247 //Test rotations
248 vtkQuaterniond rotation;
249 rotation.SetRotationAngleAndAxis(
250 vtkMath::RadiansFromDegrees(10.0),
251 1.0, 1.0, 1.0);
252
253 if (!rotation.Compare(
254 vtkQuaterniond(0.996195, 0.0290519, 0.0290519, 0.0290519),
255 0.0001))
256 {
257 std::cerr << "Error vtkQuaterniond::SetRotation Angle()"
258 <<" and Axis() failed: "<< rotation << std::endl;
259 ++retVal;
260 }
261
262 vtkQuaterniond secondRotation;
263 secondRotation.SetRotationAngleAndAxis(
264 vtkMath::RadiansFromDegrees(-20.0),
265 1.0, -1.0, 1.0);
266 if (!secondRotation.Compare(
267 vtkQuaterniond(0.984808, -0.0578827, 0.0578827, -0.0578827),
268 0.0001))
269 {
270 std::cerr << "Error vtkQuaterniond::SetRotation Angle()"
271 <<" and Axis() failed: "<< secondRotation << std::endl;
272 ++retVal;
273 }
274
275 vtkQuaterniond resultRotation = rotation * secondRotation;
276 double axis[3];
277 double supposedAxis[3] = {-0.338805, 0.901731, -0.2685};
278 double angle = resultRotation.GetRotationAngleAndAxis(axis);
279
280 if (!vtkMathUtilities::FuzzyCompare(axis[0], supposedAxis[0], 0.0001)
281 || !vtkMathUtilities::FuzzyCompare(axis[1], supposedAxis[1], 0.0001)
282 || !vtkMathUtilities::FuzzyCompare(axis[2], supposedAxis[2], 0.0001))
283 {
284 std::cerr << "Error vtkQuaterniond::GetRotationAxis() failed: "
285 << axis[0] << " " << axis[1] << " " << axis[2] << std::endl;
286 ++retVal;
287 }
288 if (!vtkMathUtilities::FuzzyCompare(
289 vtkMath::DegreesFromRadians(angle), 11.121, 0.0001))
290 {
291 std::cerr << "Error vtkQuaterniond::GetRotationAngle() failed: "
292 << vtkMath::DegreesFromRadians(angle) << std::endl;
293 ++retVal;
294 }
295
296 return retVal;
297 }
298
299 // Test the matrix conversions
300 //----------------------------------------------------------------------------
TestQuaternionMatrixConversions()301 int TestQuaternionMatrixConversions() //this test uses vtkQuaternionf
302 {
303 int retVal = 0;
304
305 vtkQuaternionf quat;
306 float M[3][3];
307 M[0][0] = 0.98420; M[0][1] = 0.17354; M[0][2] = 0.03489;
308 M[1][0] = -0.17327; M[1][1] = 0.90415; M[1][2] = 0.39049;
309 M[2][0] = 0.03621; M[2][1] = -0.39037; M[2][2] = 0.91994;
310 quat.FromMatrix3x3(M);
311
312 if (!(quat.Compare(
313 vtkQuaternionf(-0.975744, 0.200069, 0.000338168, 0.0888578),
314 0.001)))
315 {
316 std::cerr << "Error vtkQuaternionf FromMatrix3x3 failed: "
317 << quat << std::endl;
318 ++retVal;
319 }
320
321 //an easy one, just to make sure !
322 float newM[3][3];
323 quat.ToMatrix3x3(newM);
324 for (int i = 0; i < 3; ++i)
325 {
326 for (int j = 0; j < 3; ++j)
327 {
328 if (!vtkMathUtilities::FuzzyCompare(M[i][j], newM[i][j], 0.001f))
329 {
330 std::cerr << "Error vtkQuaternionf ToMatrix3x3 failed: "
331 << M[i][j] <<" != " << newM[i][j] << std::endl;
332 ++retVal;
333 }
334 }
335 }
336
337 //Rotate -23 degrees around X
338 M[0][0] = 1.0; M[0][1] = 0.0; M[0][2] = 0.0;
339 M[1][0] = 0.0; M[1][1] = 0.92050; M[1][2] = 0.39073;
340 M[2][0] = 0.0; M[2][1] = -0.39073; M[2][2] = 0.92050;
341 //Let's also make the quaternion
342 quat.SetRotationAngleAndAxis(
343 vtkMath::RadiansFromDegrees(-23.0),
344 1.0, 0.0, 0.0);
345
346 //just in case, it makes another test
347 vtkQuaternionf newQuat;
348 newQuat.FromMatrix3x3(M);
349 if (!(newQuat.Compare(quat, 0.00001)))
350 {
351 std::cerr << "Error vtkQuaternionf FromMatrix3x3 failed: "
352 << newQuat <<" != " << quat << std::endl;
353 ++retVal;
354 }
355
356 //And compare again !
357 quat.ToMatrix3x3(newM);
358 for (int i = 0; i < 3; ++i)
359 {
360 for (int j = 0; j < 3; ++j)
361 {
362 if (!vtkMathUtilities::FuzzyCompare( M[i][j], newM[i][j], 0.001f))
363 {
364 std::cerr << "Error vtkQuaternionf ToMatrix3x3 failed: "
365 << M[i][j] <<" != " << newM[i][j] << std::endl;
366 ++retVal;
367 }
368 }
369 }
370
371 return retVal;
372 }
373
374 // Test the quaternion's conversions
375 //----------------------------------------------------------------------------
TestQuaternionConversions()376 int TestQuaternionConversions() //this test uses vtkQuaterniond
377 {
378 int retVal = 0;
379 vtkQuaterniond quat(15.0, -3.0, 2.0, 0.001);
380
381 // Logarithm
382 vtkQuaterniond logQuat;
383 logQuat = quat.UnitLog();
384 if (!(logQuat.Compare(vtkQuaterniond(0, -0.378151, 0.252101, 0.00012),
385 0.00001)))
386 {
387 std::cerr << "Error vtkQuaterniond UnitLogQuaternion() failed: "
388 << logQuat << std::endl;
389 ++retVal;
390 }
391
392 // Exponential
393 vtkQuaterniond expQuat = quat.UnitExp();
394 if (!(expQuat.Compare(vtkQuaterniond(0.89075, -0.37815, 0.25210, 0.00012),
395 0.00001)))
396 {
397 std::cerr << "Error vtkQuaterniond UnitExpQuaternion() failed: "
398 << expQuat << std::endl;
399 ++retVal;
400 }
401
402 //To VTK
403 vtkQuaterniond vtkQuat = quat.NormalizedWithAngleInDegrees();
404 if (!(vtkQuat.Compare(
405 vtkQuaterniond(55.709, -0.194461, 0.129641, 6.48204e-005),
406 0.00001)))
407 {
408 std::cerr << "Error vtkQuaterniond UnitForVTKQuaternion() failed: "
409 << vtkQuat << std::endl;
410 ++retVal;
411 }
412
413 return retVal;
414 }
415
416 // Test the quaternion's slerp
417 //----------------------------------------------------------------------------
TestQuaternionSlerp()418 int TestQuaternionSlerp() //this test uses vtkQuaternionf
419 {
420 int retVal = 0;
421
422 vtkQuaternionf q0, q1;
423 q0.SetRotationAngleAndAxis(vtkMath::RadiansFromDegrees(-2.0),
424 -3.0, 2.0, 1.0);
425 q1.SetRotationAngleAndAxis(vtkMath::RadiansFromDegrees(183.0),
426 10.0, 0.0, 1.0);
427 vtkQuaternionf halfQ1 = q0.Slerp(0.5, q1);
428 halfQ1.Normalize();
429
430 if (!(halfQ1.Compare(vtkQuaternionf(0.99444, 0.104907, -0.00254, 0.00883),
431 0.00001)))
432 {
433 std::cerr << "Error vtkQuaternionf Slerp() failed: "
434 << halfQ1 << std::endl;
435 ++retVal;
436 }
437
438 return retVal;
439 }
440