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.0f, -38.0f, 42.0f, 0.0001f};
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.19628, 0.13085, 0.00007),
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.89429, 0.37234, -0.24822, -0.00012),
395 0.00001)))
396 {
397 std::cerr << "Error vtkQuaterniond UnitExpQuaternion() failed: "
398 << expQuat << std::endl;
399 ++retVal;
400 }
401
402 // UnitExp(UnitLog(q)) on a normalized quaternion is an identity operation
403 vtkQuaterniond normQuat = quat.Normalized();
404 if (!(normQuat.Compare(logQuat.UnitExp(), 0.00001)))
405 {
406 std::cerr << "Error vtkQuaterniond UnitExp(UnitLog(q)) is not identity: "
407 << logQuat.UnitExp() << " vs. " << normQuat << std::endl;
408 ++retVal;
409 }
410
411 //To VTK
412 vtkQuaterniond vtkQuat = quat.NormalizedWithAngleInDegrees();
413 if (!(vtkQuat.Compare(
414 vtkQuaterniond(55.709, -0.194461, 0.129641, 6.48204e-005),
415 0.00001)))
416 {
417 std::cerr << "Error vtkQuaterniond UnitForVTKQuaternion() failed: "
418 << vtkQuat << std::endl;
419 ++retVal;
420 }
421
422 return retVal;
423 }
424
425 // Test the quaternion's slerp
426 //----------------------------------------------------------------------------
TestQuaternionSlerp()427 int TestQuaternionSlerp() //this test uses vtkQuaterniond
428 {
429 // return value
430 int retVal = 0;
431
432 // first quaternion
433 vtkQuaternion<double> q1;
434 // quaternion which represents a small rotation
435 vtkQuaternion<double> dq;
436 // q2 is obtained by doing dq*q1
437 vtkQuaternion<double> q2;
438 // dqt is the rotation to multiply with q1
439 // to obtained the SLERP interpolation of q1 and q2
440 vtkQuaternion<double> dqt;
441 // qTruth is the result of dqt*q1
442 vtkQuaternion<double> qTruth;
443 // qSlerp is the result of the SLERP interpolation
444 // it should be equal to qTruth
445 vtkQuaternion<double> qSlerp;
446
447 // exhaustive test : 250000 operations
448 // Control the sampling of rotation's axis
449 const int M = 5;
450 // Control the sampling of the rotation's angle
451 const int L = 10;
452 // Control the sampling of the interpolation
453 const int N = 20;
454
455 // axis coordinates step
456 double dAxis = 1.0 / static_cast<double>(M);
457 // angle step
458 double dAngle = 360.0 / static_cast<double>(L);
459 // interpolation step
460 double dt = 1.0 / static_cast<double>(N);
461
462 double x, y, z, angle, t, distance, angleShort;
463 double axis[3];
464 double axisNorme;
465
466 // loop over x-coordinates
467 for (int i = 1; i <= M; ++i)
468 {
469 x = static_cast<double>(i) * dAxis;
470 // loop over y-coordinates
471 for (int j = 1; j <= M; ++j)
472 {
473 y = static_cast<double>(j) * dAxis;
474 // loop over z-coordinates
475 for (int k = 1; k <= M; ++k)
476 {
477 z = static_cast<double>(k) * dAxis;
478 axisNorme = sqrt(x * x + y * y + z * z);
479 axis[0] = x / axisNorme;
480 axis[1] = y / axisNorme;
481 axis[2] = z / axisNorme;
482 // loop over the angle of q1
483 for (int u = 1; u <= L; ++u)
484 {
485 angle = static_cast<double>(u) * dAngle;
486 q1.SetRotationAngleAndAxis(vtkMath::RadiansFromDegrees(angle), axis[0], axis[1], axis[2]);
487 // loop over the angle of dq
488 for (int v = 1; v < L; ++v)
489 {
490 angleShort = (static_cast<double>(v) * dAngle) / 2;
491 dq.SetRotationAngleAndAxis(vtkMath::RadiansFromDegrees(angleShort), axis[0], axis[1], axis[2]);
492 q2 = dq * q1;
493 // loop over the interpolation step
494 for (int w = 0; w <= N; ++w)
495 {
496 t = static_cast<double>(w) * dt;
497 dqt.SetRotationAngleAndAxis(vtkMath::RadiansFromDegrees(t * angleShort), axis[0], axis[1], axis[2]);
498 qTruth = dqt * q1;
499 qSlerp = q1.Slerp(t, q2);
500 distance = (qSlerp - qTruth).Norm();
501 if (distance > 1e-12)
502 {
503 ++retVal;
504 }
505 }
506 }
507 }
508 }
509 }
510 }
511
512 // Particular test : we test that the SLERP take the
513 // short path
514
515 double u[3] = {-0.54, -0.0321, 1};
516 double normU = sqrt(u[0] * u[0] + u[1] * u[1] + u[2] * u[2]);
517 u[0] /= normU;
518 u[1] /= normU;
519 u[2] /= normU;
520
521 // interpolation step
522 const int N2 = 1000;
523 double dtheta = 3.0;
524 // Set q1 close to the angle boundary
525 q1.SetRotationAngleAndAxis(vtkMath::RadiansFromDegrees(359.5), u[0], u[1], u[2]);
526 // dq represents a small rotation
527 dq.SetRotationAngleAndAxis(vtkMath::RadiansFromDegrees(dtheta), u[0], u[1], u[2]);
528 // q2 is a rotation close to q1 but the quaternion representant is far
529 q2 = dq * q1;
530
531 dt = 1.0 / static_cast<double>(N2);
532
533 for (int i = 0; i <= N2; ++i)
534 {
535 t = static_cast<double>(i) * dt;
536 dqt.SetRotationAngleAndAxis(vtkMath::RadiansFromDegrees(t * dtheta), u[0], u[1], u[2]);
537 qTruth = dqt * q1;
538 qSlerp = q1.Slerp(t, q2);
539 distance = (qSlerp - qTruth).Norm();
540 if (distance > 1e-12)
541 {
542 ++retVal;
543 }
544 }
545
546 if (retVal != 0 )
547 {
548 std::cerr << "Error TestQuaternionSlerp() failed" << std::endl;
549 }
550
551 return retVal;
552 }
553