1 /*=========================================================================
2
3 Program: Visualization Toolkit
4 Module: vtkQuaternion.txx
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 "vtkQuaternion.h"
17
18 #ifndef vtkQuaternion_txx
19 #define vtkQuaternion_txx
20
21 #include "vtkMath.h"
22
23 #include <cmath>
24
25 //----------------------------------------------------------------------------
26 template <typename T>
vtkQuaternion()27 vtkQuaternion<T>::vtkQuaternion()
28 {
29 this->ToIdentity();
30 }
31
32 //----------------------------------------------------------------------------
33 template <typename T>
vtkQuaternion(const T & w,const T & x,const T & y,const T & z)34 vtkQuaternion<T>::vtkQuaternion(const T& w, const T& x, const T& y, const T& z)
35 {
36 this->Data[0] = w;
37 this->Data[1] = x;
38 this->Data[2] = y;
39 this->Data[3] = z;
40 }
41
42 //----------------------------------------------------------------------------
43 template <typename T>
SquaredNorm() const44 T vtkQuaternion<T>::SquaredNorm() const
45 {
46 T result = 0.0;
47 for (int i = 0; i < 4; ++i)
48 {
49 result += this->Data[i] * this->Data[i];
50 }
51 return result;
52 }
53
54 //----------------------------------------------------------------------------
55 template <typename T>
Norm() const56 T vtkQuaternion<T>::Norm() const
57 {
58 return sqrt(this->SquaredNorm());
59 }
60
61 //----------------------------------------------------------------------------
62 template <typename T>
ToIdentity()63 void vtkQuaternion<T>::ToIdentity()
64 {
65 this->Set(1.0, 0.0, 0.0, 0.0);
66 }
67
68 //----------------------------------------------------------------------------
69 template <typename T>
Identity()70 vtkQuaternion<T> vtkQuaternion<T>::Identity()
71 {
72 vtkQuaternion<T> identity(1.0, 0.0, 0.0, 0.0);
73 return identity;
74 }
75
76 //----------------------------------------------------------------------------
77 template <typename T>
Normalize()78 T vtkQuaternion<T>::Normalize()
79 {
80 T norm = this->Norm();
81 if (norm != 0.0)
82 {
83 for (int i = 0; i < 4; ++i)
84 {
85 this->Data[i] /= norm;
86 }
87 }
88 return norm;
89 }
90
91 //----------------------------------------------------------------------------
92 template <typename T>
Normalized() const93 vtkQuaternion<T> vtkQuaternion<T>::Normalized() const
94 {
95 vtkQuaternion<T> temp(*this);
96 temp.Normalize();
97 return temp;
98 }
99
100 //----------------------------------------------------------------------------
101 template <typename T>
Conjugate()102 void vtkQuaternion<T>::Conjugate()
103 {
104 for (int i = 1; i < 4; ++i)
105 {
106 this->Data[i] *= -1.0;
107 }
108 }
109
110 //----------------------------------------------------------------------------
111 template <typename T>
Conjugated() const112 vtkQuaternion<T> vtkQuaternion<T>::Conjugated() const
113 {
114 vtkQuaternion<T> ret(*this);
115 ret.Conjugate();
116 return ret;
117 }
118
119 //----------------------------------------------------------------------------
120 template <typename T>
Invert()121 void vtkQuaternion<T>::Invert()
122 {
123 T squareNorm = this->SquaredNorm();
124 if (squareNorm != 0.0)
125 {
126 this->Conjugate();
127 for (int i = 0; i < 4; ++i)
128 {
129 this->Data[i] /= squareNorm;
130 }
131 }
132 }
133
134 //----------------------------------------------------------------------------
135 template <typename T>
Inverse() const136 vtkQuaternion<T> vtkQuaternion<T>::Inverse() const
137 {
138 vtkQuaternion<T> ret(*this);
139 ret.Invert();
140 return ret;
141 }
142
143 //----------------------------------------------------------------------------
144 template <typename T>
145 template <typename CastTo>
Cast() const146 vtkQuaternion<CastTo> vtkQuaternion<T>::Cast() const
147 {
148 vtkQuaternion<CastTo> result;
149 for (int i = 0; i < 4; ++i)
150 {
151 result[i] = static_cast<CastTo>(this->Data[i]);
152 }
153 return result;
154 }
155
156 //----------------------------------------------------------------------------
157 template <typename T>
Set(const T & w,const T & x,const T & y,const T & z)158 void vtkQuaternion<T>::Set(const T& w, const T& x, const T& y, const T& z)
159 {
160 this->Data[0] = w;
161 this->Data[1] = x;
162 this->Data[2] = y;
163 this->Data[3] = z;
164 }
165
166 //----------------------------------------------------------------------------
167 template <typename T>
Set(T quat[4])168 void vtkQuaternion<T>::Set(T quat[4])
169 {
170 for (int i = 0; i < 4; ++i)
171 {
172 this->Data[i] = quat[i];
173 }
174 }
175
176 //----------------------------------------------------------------------------
177 template <typename T>
Get(T quat[4]) const178 void vtkQuaternion<T>::Get(T quat[4]) const
179 {
180 for (int i = 0; i < 4; ++i)
181 {
182 quat[i] = this->Data[i];
183 }
184 }
185
186 //----------------------------------------------------------------------------
187 template <typename T>
SetW(const T & w)188 void vtkQuaternion<T>::SetW(const T& w)
189 {
190 this->Data[0] = w;
191 }
192
193 //----------------------------------------------------------------------------
194 template <typename T>
GetW() const195 const T& vtkQuaternion<T>::GetW() const
196 {
197 return this->Data[0];
198 }
199
200 //----------------------------------------------------------------------------
201 template <typename T>
SetX(const T & x)202 void vtkQuaternion<T>::SetX(const T& x)
203 {
204 this->Data[1] = x;
205 }
206
207 //----------------------------------------------------------------------------
208 template <typename T>
GetX() const209 const T& vtkQuaternion<T>::GetX() const
210 {
211 return this->Data[1];
212 }
213
214 //----------------------------------------------------------------------------
215 template <typename T>
SetY(const T & y)216 void vtkQuaternion<T>::SetY(const T& y)
217 {
218 this->Data[2] = y;
219 }
220
221 //----------------------------------------------------------------------------
222 template <typename T>
GetY() const223 const T& vtkQuaternion<T>::GetY() const
224 {
225 return this->Data[2];
226 }
227
228 //----------------------------------------------------------------------------
229 template <typename T>
SetZ(const T & z)230 void vtkQuaternion<T>::SetZ(const T& z)
231 {
232 this->Data[3] = z;
233 }
234
235 //----------------------------------------------------------------------------
236 template <typename T>
GetZ() const237 const T& vtkQuaternion<T>::GetZ() const
238 {
239 return this->Data[3];
240 }
241
242 //----------------------------------------------------------------------------
243 template <typename T>
GetRotationAngleAndAxis(T axis[3]) const244 T vtkQuaternion<T>::GetRotationAngleAndAxis(T axis[3]) const
245 {
246 T w = this->GetW();
247 T x = this->GetX();
248 T y = this->GetY();
249 T z = this->GetZ();
250 T f = sqrt(x * x + y * y + z * z);
251 if (f != 0.0)
252 {
253 axis[0] = x / f;
254 axis[1] = y / f;
255 axis[2] = z / f;
256 }
257 else
258 {
259 w = 1.0;
260 axis[0] = 0.0;
261 axis[1] = 0.0;
262 axis[2] = 0.0;
263 }
264
265 // atan2() provides a more accurate angle result than acos()
266 return 2.0 * atan2(f, w);
267 }
268
269 //----------------------------------------------------------------------------
270 template <typename T>
SetRotationAngleAndAxis(T angle,T axis[3])271 void vtkQuaternion<T>::SetRotationAngleAndAxis(T angle, T axis[3])
272 {
273 this->SetRotationAngleAndAxis(angle, axis[0], axis[1], axis[2]);
274 }
275
276 //----------------------------------------------------------------------------
277 template <typename T>
SetRotationAngleAndAxis(const T & angle,const T & x,const T & y,const T & z)278 void vtkQuaternion<T>::SetRotationAngleAndAxis(const T& angle, const T& x, const T& y, const T& z)
279 {
280 T axisNorm = x * x + y * y + z * z;
281 if (axisNorm != 0.0)
282 {
283 T f = sin(0.5 * angle);
284 this->SetW(cos(0.5 * angle));
285 this->SetX((x / axisNorm) * f);
286 this->SetY((y / axisNorm) * f);
287 this->SetZ((z / axisNorm) * f);
288 }
289 else
290 {
291 // set the quaternion for "no rotation"
292 this->Set(1.0, 0.0, 0.0, 0.0);
293 }
294 }
295
296 //----------------------------------------------------------------------------
297 template <typename T>
operator +(const vtkQuaternion<T> & q) const298 vtkQuaternion<T> vtkQuaternion<T>::operator+(const vtkQuaternion<T>& q) const
299 {
300 vtkQuaternion<T> ret;
301 for (int i = 0; i < 4; ++i)
302 {
303 ret[i] = this->Data[i] + q[i];
304 }
305 return ret;
306 }
307
308 //----------------------------------------------------------------------------
309 template <typename T>
operator -(const vtkQuaternion<T> & q) const310 vtkQuaternion<T> vtkQuaternion<T>::operator-(const vtkQuaternion<T>& q) const
311 {
312 vtkQuaternion<T> ret;
313 for (int i = 0; i < 4; ++i)
314 {
315 ret[i] = this->Data[i] - q[i];
316 }
317 return ret;
318 }
319
320 //----------------------------------------------------------------------------
321 template <typename T>
operator *(const vtkQuaternion<T> & q) const322 vtkQuaternion<T> vtkQuaternion<T>::operator*(const vtkQuaternion<T>& q) const
323 {
324 vtkQuaternion<T> ret;
325 T ww = this->Data[0] * q[0];
326 T wx = this->Data[0] * q[1];
327 T wy = this->Data[0] * q[2];
328 T wz = this->Data[0] * q[3];
329
330 T xw = this->Data[1] * q[0];
331 T xx = this->Data[1] * q[1];
332 T xy = this->Data[1] * q[2];
333 T xz = this->Data[1] * q[3];
334
335 T yw = this->Data[2] * q[0];
336 T yx = this->Data[2] * q[1];
337 T yy = this->Data[2] * q[2];
338 T yz = this->Data[2] * q[3];
339
340 T zw = this->Data[3] * q[0];
341 T zx = this->Data[3] * q[1];
342 T zy = this->Data[3] * q[2];
343 T zz = this->Data[3] * q[3];
344
345 ret[0] = ww - xx - yy - zz;
346 ret[1] = wx + xw + yz - zy;
347 ret[2] = wy - xz + yw + zx;
348 ret[3] = wz + xy - yx + zw;
349 return ret;
350 }
351
352 //----------------------------------------------------------------------------
353 template <typename T>
operator *(const T & scalar) const354 vtkQuaternion<T> vtkQuaternion<T>::operator*(const T& scalar) const
355 {
356 vtkQuaternion<T> ret;
357 for (int i = 0; i < 4; ++i)
358 {
359 ret[i] = this->Data[i] * scalar;
360 }
361 return ret;
362 }
363
364 //----------------------------------------------------------------------------
365 template <typename T>
operator *=(const T & scalar) const366 void vtkQuaternion<T>::operator*=(const T& scalar) const
367 {
368 for (int i = 0; i < 4; ++i)
369 {
370 this->Data[i] *= scalar;
371 }
372 }
373
374 //----------------------------------------------------------------------------
375 template <typename T>
operator /(const vtkQuaternion<T> & q) const376 vtkQuaternion<T> vtkQuaternion<T>::operator/(const vtkQuaternion<T>& q) const
377 {
378 vtkQuaternion<T> inverseQuaternion = q.Inverse();
379 return (*this) * inverseQuaternion;
380 }
381
382 //----------------------------------------------------------------------------
383 template <typename T>
operator /(const T & scalar) const384 vtkQuaternion<T> vtkQuaternion<T>::operator/(const T& scalar) const
385 {
386 vtkQuaternion<T> ret;
387 for (int i = 0; i < 4; ++i)
388 {
389 ret[i] = this->Data[i] / scalar;
390 }
391 return ret;
392 }
393
394 //----------------------------------------------------------------------------
395 template <typename T>
operator /=(const T & scalar)396 void vtkQuaternion<T>::operator/=(const T& scalar)
397 {
398 for (int i = 0; i < 4; ++i)
399 {
400 this->Data[i] /= scalar;
401 }
402 }
403
404 //----------------------------------------------------------------------------
405 template <typename T>
ToMatrix3x3(T A[3][3]) const406 void vtkQuaternion<T>::ToMatrix3x3(T A[3][3]) const
407 {
408 T ww = this->Data[0] * this->Data[0];
409 T wx = this->Data[0] * this->Data[1];
410 T wy = this->Data[0] * this->Data[2];
411 T wz = this->Data[0] * this->Data[3];
412
413 T xx = this->Data[1] * this->Data[1];
414 T yy = this->Data[2] * this->Data[2];
415 T zz = this->Data[3] * this->Data[3];
416
417 T xy = this->Data[1] * this->Data[2];
418 T xz = this->Data[1] * this->Data[3];
419 T yz = this->Data[2] * this->Data[3];
420
421 T rr = xx + yy + zz;
422 // normalization factor, just in case quaternion was not normalized
423 T f;
424 if (ww + rr == 0.0) // means the quaternion is (0, 0, 0, 0)
425 {
426 A[0][0] = 0.0;
427 A[1][0] = 0.0;
428 A[2][0] = 0.0;
429 A[0][1] = 0.0;
430 A[1][1] = 0.0;
431 A[2][1] = 0.0;
432 A[0][2] = 0.0;
433 A[1][2] = 0.0;
434 A[2][2] = 0.0;
435 return;
436 }
437 f = 1.0 / (ww + rr);
438
439 T s = (ww - rr) * f;
440 f *= 2.0;
441
442 A[0][0] = xx * f + s;
443 A[1][0] = (xy + wz) * f;
444 A[2][0] = (xz - wy) * f;
445
446 A[0][1] = (xy - wz) * f;
447 A[1][1] = yy * f + s;
448 A[2][1] = (yz + wx) * f;
449
450 A[0][2] = (xz + wy) * f;
451 A[1][2] = (yz - wx) * f;
452 A[2][2] = zz * f + s;
453 }
454
455 //----------------------------------------------------------------------------
456 // The solution is based on
457 // Berthold K. P. Horn (1987),
458 // "Closed-form solution of absolute orientation using unit quaternions,"
459 // Journal of the Optical Society of America A, 4:629-642
460 template <typename T>
FromMatrix3x3(const T A[3][3])461 void vtkQuaternion<T>::FromMatrix3x3(const T A[3][3])
462 {
463 T n[4][4];
464
465 // on-diagonal elements
466 n[0][0] = A[0][0] + A[1][1] + A[2][2];
467 n[1][1] = A[0][0] - A[1][1] - A[2][2];
468 n[2][2] = -A[0][0] + A[1][1] - A[2][2];
469 n[3][3] = -A[0][0] - A[1][1] + A[2][2];
470
471 // off-diagonal elements
472 n[0][1] = n[1][0] = A[2][1] - A[1][2];
473 n[0][2] = n[2][0] = A[0][2] - A[2][0];
474 n[0][3] = n[3][0] = A[1][0] - A[0][1];
475
476 n[1][2] = n[2][1] = A[1][0] + A[0][1];
477 n[1][3] = n[3][1] = A[0][2] + A[2][0];
478 n[2][3] = n[3][2] = A[2][1] + A[1][2];
479
480 T eigenvectors[4][4];
481 T eigenvalues[4];
482
483 // convert into format that JacobiN can use,
484 // then use Jacobi to find eigenvalues and eigenvectors
485 T* nTemp[4];
486 T* eigenvectorsTemp[4];
487 for (int i = 0; i < 4; ++i)
488 {
489 nTemp[i] = n[i];
490 eigenvectorsTemp[i] = eigenvectors[i];
491 }
492 vtkMath::JacobiN(nTemp, 4, eigenvalues, eigenvectorsTemp);
493
494 // the first eigenvector is the one we want
495 for (int i = 0; i < 4; ++i)
496 {
497 this->Data[i] = eigenvectors[i][0];
498 }
499 }
500
501 //----------------------------------------------------------------------------
502 // This returns the constant angular velocity interpolation of two quaternions
503 // on the unit quaternion sphere :
504 // http://web.mit.edu/2.998/www/QuaternionReport1.pdf
505 template <typename T>
Slerp(T t,const vtkQuaternion<T> & q1) const506 vtkQuaternion<T> vtkQuaternion<T>::Slerp(T t, const vtkQuaternion<T>& q1) const
507 {
508 // Canonical scalar product on quaternion
509 T cosTheta = this->GetW() * q1.GetW() + this->GetX() * q1.GetX() + this->GetY() * q1.GetY() +
510 this->GetZ() * q1.GetZ();
511
512 // To prevent the SLERP interpolation from taking the long path
513 // we first check the relative orientation of the two quaternions
514 // If the angle is superior to 90 degrees we take the opposite quaternion
515 // which is closer and represents the same rotation
516 vtkQuaternion<T> qClosest = q1;
517 if (cosTheta < 0)
518 {
519 cosTheta = -cosTheta;
520 qClosest = qClosest * -1;
521 }
522
523 // To avoid division by zero, perform a linear interpolation (LERP), if our
524 // quarternions are nearly in the same direction, otherwise resort
525 // to spherical linear interpolation. In the limiting case (for small
526 // angles), SLERP is equivalent to LERP.
527 T t1, t2;
528 if ((1.0 - fabs(cosTheta)) < 1e-6)
529 {
530 t1 = 1.0 - t;
531 t2 = t;
532 }
533 else
534 {
535 // Angle (defined by the canonical scalar product for quaternions)
536 // between the two quaternions
537 const T theta = acos(cosTheta);
538 t1 = sin((1.0 - t) * theta) / sin(theta);
539 t2 = sin(t * theta) / sin(theta);
540 }
541
542 return (*this) * t1 + qClosest * t2;
543 }
544
545 //----------------------------------------------------------------------------
546 template <typename T>
InnerPoint(const vtkQuaternion<T> & q1,const vtkQuaternion<T> & q2) const547 vtkQuaternion<T> vtkQuaternion<T>::InnerPoint(
548 const vtkQuaternion<T>& q1, const vtkQuaternion<T>& q2) const
549 {
550 vtkQuaternion<T> qInv = q1.Inverse();
551 vtkQuaternion<T> qL = qInv * q2;
552 vtkQuaternion<T> qR = qInv * (*this);
553
554 vtkQuaternion<T> qLLog = qL.UnitLog();
555 vtkQuaternion<T> qRLog = qR.UnitLog();
556 vtkQuaternion<T> qSum = qLLog + qRLog;
557 T w = qSum.GetW();
558 qSum /= -4.0;
559 qSum.SetW(w);
560
561 vtkQuaternion<T> qExp = qSum.UnitExp();
562 return q1 * qExp;
563 }
564
565 //----------------------------------------------------------------------------
566 template <typename T>
ToUnitLog()567 void vtkQuaternion<T>::ToUnitLog()
568 {
569 T axis[3];
570 T angle = 0.5 * this->GetRotationAngleAndAxis(axis);
571
572 this->Set(0.0, angle * axis[0], angle * axis[1], angle * axis[2]);
573 }
574
575 //----------------------------------------------------------------------------
576 template <typename T>
UnitLog() const577 vtkQuaternion<T> vtkQuaternion<T>::UnitLog() const
578 {
579 vtkQuaternion<T> unitLog(*this);
580 unitLog.ToUnitLog();
581 return unitLog;
582 }
583
584 //----------------------------------------------------------------------------
585 template <typename T>
ToUnitExp()586 void vtkQuaternion<T>::ToUnitExp()
587 {
588 T x = this->GetX();
589 T y = this->GetY();
590 T z = this->GetZ();
591 T angle = sqrt(x * x + y * y + z * z);
592 T sinAngle = sin(angle);
593 T cosAngle = cos(angle);
594 if (angle != 0.0)
595 {
596 x /= angle;
597 y /= angle;
598 z /= angle;
599 }
600
601 this->Set(cosAngle, sinAngle * x, sinAngle * y, sinAngle * z);
602 }
603
604 //----------------------------------------------------------------------------
605 template <typename T>
UnitExp() const606 vtkQuaternion<T> vtkQuaternion<T>::UnitExp() const
607 {
608 vtkQuaternion<T> unitExp(*this);
609 unitExp.ToUnitExp();
610 return unitExp;
611 }
612
613 //----------------------------------------------------------------------------
614 template <typename T>
NormalizeWithAngleInDegrees()615 void vtkQuaternion<T>::NormalizeWithAngleInDegrees()
616 {
617 this->Normalize();
618 this->SetW(vtkMath::DegreesFromRadians(this->GetW()));
619 }
620
621 //----------------------------------------------------------------------------
622 template <typename T>
NormalizedWithAngleInDegrees() const623 vtkQuaternion<T> vtkQuaternion<T>::NormalizedWithAngleInDegrees() const
624 {
625 vtkQuaternion<T> unitVTK(*this);
626 unitVTK.Normalize();
627 unitVTK.SetW(vtkMath::DegreesFromRadians(unitVTK.GetW()));
628 return unitVTK;
629 }
630
631 #endif
632