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 "vtkMathUtilities.h"
17 #include "vtkQuaternion.h"
18 #include "vtkSetGet.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]." << std::endl
61 << "sizeof(vtkQuaternionf) = " << sizeof(qf) << std::endl
62 << "sizeof(float[4]) = " << sizeof(zeroArrayf) << std::endl;
63 ++retVal;
64 }
65 if (qf.GetSize() != 4)
66 {
67 std::cerr << "Incorrect size of vtkQuaternionf, should be 4, but is " << qf.GetSize()
68 << std::endl;
69 ++retVal;
70 }
71
72 //
73 // Test out vtkQuaternionf and ensure the various access methods are the same
74 qf.Set(0.0f, 6.0f, 9.0f, 15.0f);
75 if (qf.GetW() != qf[0] || !vtkMathUtilities::FuzzyCompare<float>(qf.GetW(), 0.0f))
76 {
77 std::cerr << "qf.GetW() should equal qf.GetData()[0] which should equal 0."
78 << "\nqf.W() = " << qf.GetW() << std::endl
79 << "qf[0] = " << qf[0] << std::endl;
80 ++retVal;
81 }
82 if (qf.GetX() != qf[1] || !vtkMathUtilities::FuzzyCompare<float>(qf.GetX(), 6.0f))
83 {
84 std::cerr << "qf.GetX() should equal qf.GetData()[1] "
85 << "which should equal 6.0. \nqf.GetX() = " << qf.GetX() << std::endl
86 << "qf[1] = " << qf[1] << std::endl;
87 ++retVal;
88 }
89 if (qf.GetY() != qf[2] || !vtkMathUtilities::FuzzyCompare<float>(qf.GetY(), 9.0f))
90 {
91 std::cerr << "qf.GetY() should equal qf.GetData()[2]"
92 << " which should equal 9.0.\nqf.GetY() = " << qf.GetY() << std::endl
93 << "qf[2] = " << qf[2] << std::endl;
94 ++retVal;
95 }
96 if (qf.GetZ() != qf[3] || !vtkMathUtilities::FuzzyCompare<float>(qf.GetZ(), 15.0f))
97 {
98 std::cerr << "qf.GetZ() should equal qf.GetData()[3] "
99 << "which should equal 15.0.\nqf.Z() = " << qf.GetZ() << std::endl
100 << "qf[3] = " << qf[3] << std::endl;
101 ++retVal;
102 }
103
104 //
105 // Assign the data to an float array and ensure the two ways of
106 // referencing are the same.
107 float* floatPtr = qf.GetData();
108 for (int i = 0; i < 3; ++i)
109 {
110 if (qf[i] != floatPtr[i] || qf(i) != qf[i])
111 {
112 std::cerr << "Error: qf[i] != floatPtr[i]" << std::endl
113 << "qf[i] = " << qf[i] << std::endl
114 << "floatPtr[i] = " << floatPtr[i] << std::endl;
115 ++retVal;
116 }
117 }
118
119 // To and from float[4]
120 float setArray[4] = { 1.0f, -38.0f, 42.0f, 0.0001f };
121 qf.Set(setArray);
122 if (!qf.Compare(vtkQuaternionf(1.0, -38.0, 42.0, 0.0001), 0.0001))
123 {
124 std::cerr << "Error vtkQuaterniond::Set(float[4]) failed: " << qf << std::endl;
125 ++retVal;
126 }
127
128 float arrayToCompare[4];
129 qf.Get(arrayToCompare);
130 for (int i = 0; i < 4; ++i)
131 {
132 if (!vtkMathUtilities::FuzzyCompare(setArray[i], arrayToCompare[i]))
133 {
134 std::cerr << "Error vtkQuaterniond::Get(float[4]) failed: " << setArray[i]
135 << "!= " << arrayToCompare[i] << std::endl;
136 ++retVal;
137 }
138 }
139
140 return retVal;
141 }
142
143 // Test the normalize and normalized functions.
144 //------------------------------------------------------------------------------
TestQuaternionNormalization()145 int TestQuaternionNormalization() // This test use vtkQuaterniond
146 {
147 int retVal = 0;
148
149 vtkQuaterniond normy(1.0, 2.0, 3.0, 4.0);
150 vtkQuaterniond normed = normy.Normalized();
151 if (!normed.Compare(vtkQuaterniond(0.182574, 0.365148, 0.547723, 0.730297), 0.0001))
152 {
153 std::cerr << "Error vtkQuaterniond::Normalized() failed: " << normed << std::endl;
154 ++retVal;
155 }
156 normy.Normalize();
157 if (!normy.Compare(normed, 0.0001))
158 {
159 std::cerr << "Error vtkQuaterniond::Normalize() failed: " << normy << std::endl;
160 }
161 if (!vtkMathUtilities::FuzzyCompare(normy.Norm(), 1.0, 0.0001))
162 {
163 std::cerr << "Normalized length should always be ~= 1.0, value is " << normy.Norm()
164 << std::endl;
165 ++retVal;
166 }
167
168 return retVal;
169 }
170
171 // This tests the conjugation and inversion at the same time.
172 // Since inversion depends on normalization, this will probably fail
173 // if TestQuaternionNormalisation() fails.
174 //------------------------------------------------------------------------------
TestQuaternionConjugationAndInversion()175 int TestQuaternionConjugationAndInversion() // this test uses vtkQuaternionf
176 {
177 int retVal = 0;
178
179 //
180 // Test conjugate and inverse at the same time.
181 // [inv(q) = conj(q)/norm2(q)]
182 vtkQuaternionf toConjugate(2.0f);
183 vtkQuaternionf conjugate = toConjugate.Conjugated();
184 if (!conjugate.Compare(vtkQuaternionf(2.0f, -2.0f, -2.0f, -2.0f), 0.0001))
185 {
186 std::cerr << "Error vtkQuaternionf::Conjugated() failed: " << conjugate << std::endl;
187 ++retVal;
188 }
189 float squaredNorm = conjugate.SquaredNorm();
190 vtkQuaternionf invToConjugate = conjugate / squaredNorm;
191 if (!invToConjugate.Compare(vtkQuaternionf(0.125f, -0.125f, -0.125f, -0.125f), 0.0001))
192 {
193 std::cerr << "Error vtkQuaternionf Divide by Scalar() failed: " << invToConjugate << std::endl;
194 ++retVal;
195 }
196
197 vtkQuaternionf shouldBeIdentity = invToConjugate * toConjugate;
198 vtkQuaternionf identity;
199 identity.ToIdentity();
200 if (!shouldBeIdentity.Compare(identity, 0.0001))
201 {
202 std::cerr << "Error vtkQuaternionf multiplication failed: " << shouldBeIdentity << std::endl;
203 ++retVal;
204 }
205 toConjugate.Invert();
206 if (!invToConjugate.Compare(toConjugate, 0.0001))
207 {
208 std::cerr << "Error vtkQuaternionf::Inverse failed: " << toConjugate << std::endl;
209 ++retVal;
210 }
211 shouldBeIdentity.Invert();
212 if (!shouldBeIdentity.Compare(identity, 0.0001))
213 {
214 std::cerr << "Error vtkQuaternionf::Inverse failed: " << shouldBeIdentity << std::endl;
215 ++retVal;
216 }
217
218 return retVal;
219 }
220
221 // Test the rotations
222 //------------------------------------------------------------------------------
TestQuaternionRotation()223 int TestQuaternionRotation() // this test uses vtkQuaterniond
224 {
225 int retVal = 0;
226
227 //
228 // Test rotations
229 vtkQuaterniond rotation;
230 rotation.SetRotationAngleAndAxis(vtkMath::RadiansFromDegrees(10.0), 1.0, 1.0, 1.0);
231
232 if (!rotation.Compare(vtkQuaterniond(0.996195, 0.0290519, 0.0290519, 0.0290519), 0.0001))
233 {
234 std::cerr << "Error vtkQuaterniond::SetRotation Angle()"
235 << " and Axis() failed: " << rotation << std::endl;
236 ++retVal;
237 }
238
239 vtkQuaterniond secondRotation;
240 secondRotation.SetRotationAngleAndAxis(vtkMath::RadiansFromDegrees(-20.0), 1.0, -1.0, 1.0);
241 if (!secondRotation.Compare(vtkQuaterniond(0.984808, -0.0578827, 0.0578827, -0.0578827), 0.0001))
242 {
243 std::cerr << "Error vtkQuaterniond::SetRotation Angle()"
244 << " and Axis() failed: " << secondRotation << std::endl;
245 ++retVal;
246 }
247
248 vtkQuaterniond resultRotation = rotation * secondRotation;
249 double axis[3];
250 double supposedAxis[3] = { -0.338805, 0.901731, -0.2685 };
251 double angle = resultRotation.GetRotationAngleAndAxis(axis);
252
253 if (!vtkMathUtilities::FuzzyCompare(axis[0], supposedAxis[0], 0.0001) ||
254 !vtkMathUtilities::FuzzyCompare(axis[1], supposedAxis[1], 0.0001) ||
255 !vtkMathUtilities::FuzzyCompare(axis[2], supposedAxis[2], 0.0001))
256 {
257 std::cerr << "Error vtkQuaterniond::GetRotationAxis() failed: " << axis[0] << " " << axis[1]
258 << " " << axis[2] << std::endl;
259 ++retVal;
260 }
261 if (!vtkMathUtilities::FuzzyCompare(vtkMath::DegreesFromRadians(angle), 11.121, 0.0001))
262 {
263 std::cerr << "Error vtkQuaterniond::GetRotationAngle() failed: "
264 << vtkMath::DegreesFromRadians(angle) << std::endl;
265 ++retVal;
266 }
267
268 return retVal;
269 }
270
271 // Test the matrix conversions
272 //------------------------------------------------------------------------------
TestQuaternionMatrixConversions()273 int TestQuaternionMatrixConversions() // this test uses vtkQuaternionf
274 {
275 int retVal = 0;
276
277 vtkQuaternionf quat;
278 float M[3][3];
279 M[0][0] = 0.98420;
280 M[0][1] = 0.17354;
281 M[0][2] = 0.03489;
282 M[1][0] = -0.17327;
283 M[1][1] = 0.90415;
284 M[1][2] = 0.39049;
285 M[2][0] = 0.03621;
286 M[2][1] = -0.39037;
287 M[2][2] = 0.91994;
288 quat.FromMatrix3x3(M);
289
290 if (!(quat.Compare(vtkQuaternionf(-0.975744, 0.200069, 0.000338168, 0.0888578), 0.001)))
291 {
292 std::cerr << "Error vtkQuaternionf FromMatrix3x3 failed: " << quat << std::endl;
293 ++retVal;
294 }
295
296 // an easy one, just to make sure !
297 float newM[3][3];
298 quat.ToMatrix3x3(newM);
299 for (int i = 0; i < 3; ++i)
300 {
301 for (int j = 0; j < 3; ++j)
302 {
303 if (!vtkMathUtilities::FuzzyCompare(M[i][j], newM[i][j], 0.001f))
304 {
305 std::cerr << "Error vtkQuaternionf ToMatrix3x3 failed: " << M[i][j] << " != " << newM[i][j]
306 << std::endl;
307 ++retVal;
308 }
309 }
310 }
311
312 // Rotate -23 degrees around X
313 M[0][0] = 1.0;
314 M[0][1] = 0.0;
315 M[0][2] = 0.0;
316 M[1][0] = 0.0;
317 M[1][1] = 0.92050;
318 M[1][2] = 0.39073;
319 M[2][0] = 0.0;
320 M[2][1] = -0.39073;
321 M[2][2] = 0.92050;
322 // Let's also make the quaternion
323 quat.SetRotationAngleAndAxis(vtkMath::RadiansFromDegrees(-23.0), 1.0, 0.0, 0.0);
324
325 // just in case, it makes another test
326 vtkQuaternionf newQuat;
327 newQuat.FromMatrix3x3(M);
328 if (!(newQuat.Compare(quat, 0.00001)))
329 {
330 std::cerr << "Error vtkQuaternionf FromMatrix3x3 failed: " << newQuat << " != " << quat
331 << std::endl;
332 ++retVal;
333 }
334
335 // And compare again !
336 quat.ToMatrix3x3(newM);
337 for (int i = 0; i < 3; ++i)
338 {
339 for (int j = 0; j < 3; ++j)
340 {
341 if (!vtkMathUtilities::FuzzyCompare(M[i][j], newM[i][j], 0.001f))
342 {
343 std::cerr << "Error vtkQuaternionf ToMatrix3x3 failed: " << M[i][j] << " != " << newM[i][j]
344 << std::endl;
345 ++retVal;
346 }
347 }
348 }
349
350 return retVal;
351 }
352
353 // Test the quaternion's conversions
354 //------------------------------------------------------------------------------
TestQuaternionConversions()355 int TestQuaternionConversions() // this test uses vtkQuaterniond
356 {
357 int retVal = 0;
358 vtkQuaterniond quat(15.0, -3.0, 2.0, 0.001);
359
360 // Logarithm
361 vtkQuaterniond logQuat;
362 logQuat = quat.UnitLog();
363 if (!(logQuat.Compare(vtkQuaterniond(0, -0.19628, 0.13085, 0.00007), 0.00001)))
364 {
365 std::cerr << "Error vtkQuaterniond UnitLogQuaternion() failed: " << logQuat << std::endl;
366 ++retVal;
367 }
368
369 // Exponential
370 vtkQuaterniond expQuat = quat.UnitExp();
371 if (!(expQuat.Compare(vtkQuaterniond(-0.89429, 0.37234, -0.24822, -0.00012), 0.00001)))
372 {
373 std::cerr << "Error vtkQuaterniond UnitExpQuaternion() failed: " << expQuat << std::endl;
374 ++retVal;
375 }
376
377 // UnitExp(UnitLog(q)) on a normalized quaternion is an identity operation
378 vtkQuaterniond normQuat = quat.Normalized();
379 if (!(normQuat.Compare(logQuat.UnitExp(), 0.00001)))
380 {
381 std::cerr << "Error vtkQuaterniond UnitExp(UnitLog(q)) is not identity: " << logQuat.UnitExp()
382 << " vs. " << normQuat << std::endl;
383 ++retVal;
384 }
385
386 // To VTK
387 vtkQuaterniond vtkQuat = quat.NormalizedWithAngleInDegrees();
388 if (!(vtkQuat.Compare(vtkQuaterniond(55.709, -0.194461, 0.129641, 6.48204e-005), 0.00001)))
389 {
390 std::cerr << "Error vtkQuaterniond UnitForVTKQuaternion() failed: " << vtkQuat << std::endl;
391 ++retVal;
392 }
393
394 return retVal;
395 }
396
397 // Test the quaternion's slerp
398 //------------------------------------------------------------------------------
TestQuaternionSlerp()399 int TestQuaternionSlerp() // this test uses vtkQuaterniond
400 {
401 // return value
402 int retVal = 0;
403
404 // first quaternion
405 vtkQuaternion<double> q1;
406 // quaternion which represents a small rotation
407 vtkQuaternion<double> dq;
408 // q2 is obtained by doing dq*q1
409 vtkQuaternion<double> q2;
410 // dqt is the rotation to multiply with q1
411 // to obtained the SLERP interpolation of q1 and q2
412 vtkQuaternion<double> dqt;
413 // qTruth is the result of dqt*q1
414 vtkQuaternion<double> qTruth;
415 // qSlerp is the result of the SLERP interpolation
416 // it should be equal to qTruth
417 vtkQuaternion<double> qSlerp;
418
419 // exhaustive test : 250000 operations
420 // Control the sampling of rotation's axis
421 const int M = 5;
422 // Control the sampling of the rotation's angle
423 const int L = 10;
424 // Control the sampling of the interpolation
425 const int N = 20;
426
427 // axis coordinates step
428 double dAxis = 1.0 / static_cast<double>(M);
429 // angle step
430 double dAngle = 360.0 / static_cast<double>(L);
431 // interpolation step
432 double dt = 1.0 / static_cast<double>(N);
433
434 double x, y, z, angle, t, distance, angleShort;
435 double axis[3];
436 double axisNorme;
437
438 // loop over x-coordinates
439 for (int i = 1; i <= M; ++i)
440 {
441 x = static_cast<double>(i) * dAxis;
442 // loop over y-coordinates
443 for (int j = 1; j <= M; ++j)
444 {
445 y = static_cast<double>(j) * dAxis;
446 // loop over z-coordinates
447 for (int k = 1; k <= M; ++k)
448 {
449 z = static_cast<double>(k) * dAxis;
450 axisNorme = sqrt(x * x + y * y + z * z);
451 axis[0] = x / axisNorme;
452 axis[1] = y / axisNorme;
453 axis[2] = z / axisNorme;
454 // loop over the angle of q1
455 for (int u = 1; u <= L; ++u)
456 {
457 angle = static_cast<double>(u) * dAngle;
458 q1.SetRotationAngleAndAxis(vtkMath::RadiansFromDegrees(angle), axis[0], axis[1], axis[2]);
459 // loop over the angle of dq
460 for (int v = 1; v < L; ++v)
461 {
462 angleShort = (static_cast<double>(v) * dAngle) / 2;
463 dq.SetRotationAngleAndAxis(
464 vtkMath::RadiansFromDegrees(angleShort), axis[0], axis[1], axis[2]);
465 q2 = dq * q1;
466 // loop over the interpolation step
467 for (int w = 0; w <= N; ++w)
468 {
469 t = static_cast<double>(w) * dt;
470 dqt.SetRotationAngleAndAxis(
471 vtkMath::RadiansFromDegrees(t * angleShort), axis[0], axis[1], axis[2]);
472 qTruth = dqt * q1;
473 qSlerp = q1.Slerp(t, q2);
474 distance = (qSlerp - qTruth).Norm();
475 if (distance > 1e-12)
476 {
477 ++retVal;
478 }
479 }
480 }
481 }
482 }
483 }
484 }
485
486 // Particular test : we test that the SLERP take the
487 // short path
488
489 double u[3] = { -0.54, -0.0321, 1 };
490 double normU = sqrt(u[0] * u[0] + u[1] * u[1] + u[2] * u[2]);
491 u[0] /= normU;
492 u[1] /= normU;
493 u[2] /= normU;
494
495 // interpolation step
496 const int N2 = 1000;
497 double dtheta = 3.0;
498 // Set q1 close to the angle boundary
499 q1.SetRotationAngleAndAxis(vtkMath::RadiansFromDegrees(359.5), u[0], u[1], u[2]);
500 // dq represents a small rotation
501 dq.SetRotationAngleAndAxis(vtkMath::RadiansFromDegrees(dtheta), u[0], u[1], u[2]);
502 // q2 is a rotation close to q1 but the quaternion representant is far
503 q2 = dq * q1;
504
505 dt = 1.0 / static_cast<double>(N2);
506
507 for (int i = 0; i <= N2; ++i)
508 {
509 t = static_cast<double>(i) * dt;
510 dqt.SetRotationAngleAndAxis(vtkMath::RadiansFromDegrees(t * dtheta), u[0], u[1], u[2]);
511 qTruth = dqt * q1;
512 qSlerp = q1.Slerp(t, q2);
513 distance = (qSlerp - qTruth).Norm();
514 if (distance > 1e-12)
515 {
516 ++retVal;
517 }
518 }
519
520 if (retVal != 0)
521 {
522 std::cerr << "Error TestQuaternionSlerp() failed" << std::endl;
523 }
524
525 return retVal;
526 }
527