1
///////////////////////////////////////////////////////////////////////////
3
// Copyright (c) 2002, Industrial Light & Magic, a division of Lucas
6
// All rights reserved.
8
// Redistribution and use in source and binary forms, with or without
9
// modification, are permitted provided that the following conditions are
11
// * Redistributions of source code must retain the above copyright
12
// notice, this list of conditions and the following disclaimer.
13
// * Redistributions in binary form must reproduce the above
14
// copyright notice, this list of conditions and the following disclaimer
15
// in the documentation and/or other materials provided with the
17
// * Neither the name of Industrial Light & Magic nor the names of
18
// its contributors may be used to endorse or promote products derived
19
// from this software without specific prior written permission.
21
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
24
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
25
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
26
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
27
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
28
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
29
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
30
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
31
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
33
///////////////////////////////////////////////////////////////////////////
37
#ifndef INCLUDED_IMATHQUAT_H
38
#define INCLUDED_IMATHQUAT_H
40
//----------------------------------------------------------------------
42
// template class Quat<T>
44
// "Quaternions came from Hamilton ... and have been an unmixed
45
// evil to those who have touched them in any way. Vector is a
46
// useless survival ... and has never been of the slightest use
51
// This class implements the quaternion numerical type -- you
52
// will probably want to use this class to represent orientations
53
// in R3 and to convert between various euler angle reps. You
54
// should probably use Imath::Euler<> for that.
56
//----------------------------------------------------------------------
59
#include "ImathMatrix.h"
65
#if (defined _WIN32 || defined _WIN64) && defined _MSC_VER
66
// Disable MS VC++ warnings about conversion from double to float
67
#pragma warning(disable:4244)
74
Quat<T> slerp (const Quat<T> &q1,const Quat<T> &q2, T t);
77
Quat<T> squad (const Quat<T> &q1,const Quat<T> &q2,
78
const Quat<T> &qa,const Quat<T> &qb, T t);
81
void intermediate (const Quat<T> &q0, const Quat<T> &q1,
82
const Quat<T> &q2, const Quat<T> &q3,
83
Quat<T> &qa, Quat<T> &qb);
91
Vec3<T> v; // imaginary vector
93
//-----------------------------------------------------
94
// Constructors - default constructor is identity quat
95
//-----------------------------------------------------
97
Quat() : r(1), v(0,0,0) {}
100
Quat( const Quat<S>& q) : r(q.r), v(q.v) {}
102
Quat( T s, T i, T j, T k ) : r(s), v(i,j,k) {}
104
Quat( T s, Vec3<T> d ) : r(s), v(d) {}
106
static Quat<T> identity() { return Quat<T>(); }
108
//------------------------------------------------
109
// Basic Algebra - Operators and Methods
110
// The operator return values are *NOT* normalized
112
// operator^ is 4D dot product
113
// operator/ uses the inverse() quaternion
114
// operator~ is conjugate -- if (S+V) is quat then
115
// the conjugate (S+V)* == (S-V)
117
// some operators (*,/,*=,/=) treat the quat as
118
// a 4D vector when one of the operands is scalar
119
//------------------------------------------------
121
const Quat<T>& operator= (const Quat<T>&);
122
const Quat<T>& operator*= (const Quat<T>&);
123
const Quat<T>& operator*= (T);
124
const Quat<T>& operator/= (const Quat<T>&);
125
const Quat<T>& operator/= (T);
126
const Quat<T>& operator+= (const Quat<T>&);
127
const Quat<T>& operator-= (const Quat<T>&);
128
T& operator[] (int index); // as 4D vector
129
T operator[] (int index) const;
131
template <class S> bool operator == (const Quat<S> &q) const;
132
template <class S> bool operator != (const Quat<S> &q) const;
134
Quat<T>& invert(); // this -> 1 / this
135
Quat<T> inverse() const;
136
Quat<T>& normalize(); // returns this
137
Quat<T> normalized() const;
138
T length() const; // in R4
140
//-----------------------
141
// Rotation conversion
142
//-----------------------
144
Quat<T>& setAxisAngle(const Vec3<T>& axis, T radians);
145
Quat<T>& setRotation(const Vec3<T>& fromDirection,
146
const Vec3<T>& toDirection);
149
Vec3<T> axis() const;
151
Matrix33<T> toMatrix33() const;
152
Matrix44<T> toMatrix44() const;
159
void setRotationInternal (const Vec3<T>& f0,
165
//--------------------
166
// Convenient typedefs
167
//--------------------
169
typedef Quat<float> Quatf;
170
typedef Quat<double> Quatd;
178
inline const Quat<T>& Quat<T>::operator= (const Quat<T>& q)
186
inline const Quat<T>& Quat<T>::operator*= (const Quat<T>& q)
188
T rtmp = r * q.r - (v ^ q.v);
189
v = r * q.v + v * q.r + v % q.v;
195
inline const Quat<T>& Quat<T>::operator*= (T t)
203
inline const Quat<T>& Quat<T>::operator/= (const Quat<T>& q)
205
*this = *this * q.inverse();
210
inline const Quat<T>& Quat<T>::operator/= (T t)
218
inline const Quat<T>& Quat<T>::operator+= (const Quat<T>& q)
226
inline const Quat<T>& Quat<T>::operator-= (const Quat<T>& q)
233
inline T& Quat<T>::operator[] (int index)
235
return index ? v[index-1] : r;
239
inline T Quat<T>::operator[] (int index) const
241
return index ? v[index-1] : r;
247
Quat<T>::operator == (const Quat<S> &q) const
249
return r == q.r && v == q.v;
255
Quat<T>::operator != (const Quat<S> &q) const
257
return r != q.r || v != q.v;
261
inline T operator^ (const Quat<T>& q1,const Quat<T>& q2)
263
return q1.r * q2.r + (q1.v ^ q2.v);
267
inline T Quat<T>::length() const
269
return Math<T>::sqrt( r * r + (v ^ v) );
273
inline Quat<T>& Quat<T>::normalize()
275
if ( T l = length() ) { r /= l; v /= l; }
276
else { r = 1; v = Vec3<T>(0); }
281
inline Quat<T> Quat<T>::normalized() const
283
if ( T l = length() ) return Quat( r / l, v / l );
288
inline Quat<T> Quat<T>::inverse() const
291
// - = ---- where Q* is conjugate (operator~)
292
// Q Q* Q and (Q* Q) == Q ^ Q (4D dot)
294
T qdot = *this ^ *this;
295
return Quat( r / qdot, -v / qdot );
299
inline Quat<T>& Quat<T>::invert()
301
T qdot = (*this) ^ (*this);
310
angle4D (const Quat<T> &q1, const Quat<T> &q2)
313
// Compute the angle between two quaternions,
314
// interpreting the quaternions as 4D vectors.
318
T lengthD = Math<T>::sqrt (d ^ d);
321
T lengthS = Math<T>::sqrt (s ^ s);
323
return 2 * Math<T>::atan2 (lengthD, lengthS);
329
slerp(const Quat<T> &q1,const Quat<T> &q2, T t)
332
// Spherical linear interpolation.
333
// Assumes q1 and q2 are normalized and that q1 != -q2.
335
// This method does *not* interpolate along the shortest
336
// arc between q1 and q2. If you desire interpolation
337
// along the shortest arc, and q1^q2 is negative, then
338
// consider flipping the second quaternion explicitly.
340
// The implementation of squad() depends on a slerp()
341
// that interpolates as is, without the automatic
344
// Don Hatch explains the method we use here on his
345
// web page, The Right Way to Calculate Stuff, at
346
// http://www.plunk.org/~hatch/rightway.php
349
T a = angle4D (q1, q2);
352
Quat<T> q = sinx_over_x (s * a) / sinx_over_x (a) * s * q1 +
353
sinx_over_x (t * a) / sinx_over_x (a) * t * q2;
355
return q.normalized();
360
Quat<T> spline(const Quat<T> &q0, const Quat<T> &q1,
361
const Quat<T> &q2, const Quat<T> &q3,
364
// Spherical Cubic Spline Interpolation -
365
// from Advanced Animation and Rendering
366
// Techniques by Watt and Watt, Page 366:
367
// A spherical curve is constructed using three
368
// spherical linear interpolations of a quadrangle
369
// of unit quaternions: q1, qa, qb, q2.
370
// Given a set of quaternion keys: q0, q1, q2, q3,
371
// this routine does the interpolation between
372
// q1 and q2 by constructing two intermediate
373
// quaternions: qa and qb. The qa and qb are
374
// computed by the intermediate function to
375
// guarantee the continuity of tangents across
376
// adjacent cubic segments. The qa represents in-tangent
377
// for q1 and the qb represents the out-tangent for q2.
379
// The q1 q2 is the cubic segment being interpolated.
380
// The q0 is from the previous adjacent segment and q3 is
381
// from the next adjacent segment. The q0 and q3 are used
382
// in computing qa and qb.
385
Quat<T> qa = intermediate (q0, q1, q2);
386
Quat<T> qb = intermediate (q1, q2, q3);
387
Quat<T> result = squad(q1, qa, qb, q2, t);
393
Quat<T> squad(const Quat<T> &q1, const Quat<T> &qa,
394
const Quat<T> &qb, const Quat<T> &q2,
397
// Spherical Quadrangle Interpolation -
398
// from Advanced Animation and Rendering
399
// Techniques by Watt and Watt, Page 366:
400
// It constructs a spherical cubic interpolation as
401
// a series of three spherical linear interpolations
402
// of a quadrangle of unit quaternions.
405
Quat<T> r1 = slerp(q1, q2, t);
406
Quat<T> r2 = slerp(qa, qb, t);
407
Quat<T> result = slerp(r1, r2, 2*t*(1-t));
413
Quat<T> intermediate(const Quat<T> &q0, const Quat<T> &q1, const Quat<T> &q2)
415
// From advanced Animation and Rendering
416
// Techniques by Watt and Watt, Page 366:
417
// computing the inner quadrangle
418
// points (qa and qb) to guarantee tangent
421
Quat<T> q1inv = q1.inverse();
422
Quat<T> c1 = q1inv*q2;
423
Quat<T> c2 = q1inv*q0;
424
Quat<T> c3 = (T) (-0.25) * (c2.log() + c1.log());
425
Quat<T> qa = q1 * c3.exp();
431
inline Quat<T> Quat<T>::log() const
434
// For unit quaternion, from Advanced Animation and
435
// Rendering Techniques by Watt and Watt, Page 366:
438
T theta = Math<T>::acos (std::min (r, (T) 1.0));
441
return Quat<T> (0, v);
443
T sintheta = Math<T>::sin (theta);
446
if (abs (sintheta) < 1 && abs (theta) >= limits<T>::max() * abs (sintheta))
449
k = theta / sintheta;
451
return Quat<T> ((T) 0, v.x * k, v.y * k, v.z * k);
455
inline Quat<T> Quat<T>::exp() const
458
// For pure quaternion (zero scalar part):
459
// from Advanced Animation and Rendering
460
// Techniques by Watt and Watt, Page 366:
463
T theta = v.length();
464
T sintheta = Math<T>::sin (theta);
467
if (abs (theta) < 1 && abs (sintheta) >= limits<T>::max() * abs (theta))
470
k = sintheta / theta;
472
T costheta = Math<T>::cos (theta);
474
return Quat<T> (costheta, v.x * k, v.y * k, v.z * k);
478
inline T Quat<T>::angle() const
480
return 2.0*Math<T>::acos(r);
484
inline Vec3<T> Quat<T>::axis() const
486
return v.normalized();
490
inline Quat<T>& Quat<T>::setAxisAngle(const Vec3<T>& axis, T radians)
492
r = Math<T>::cos(radians/2);
493
v = axis.normalized() * Math<T>::sin(radians/2);
500
Quat<T>::setRotation(const Vec3<T>& from, const Vec3<T>& to)
503
// Create a quaternion that rotates vector from into vector to,
504
// such that the rotation is around an axis that is the cross
505
// product of from and to.
507
// This function calls function setRotationInternal(), which is
508
// numerically accurate only for rotation angles that are not much
509
// greater than pi/2. In order to achieve good accuracy for angles
510
// greater than pi/2, we split large angles in half, and rotate in
515
// Normalize from and to, yielding f0 and t0.
518
Vec3<T> f0 = from.normalized();
519
Vec3<T> t0 = to.normalized();
524
// The rotation angle is less than or equal to pi/2.
527
setRotationInternal (f0, t0, *this);
532
// The angle is greater than pi/2. After computing h0,
533
// which is halfway between f0 and t0, we rotate first
534
// from f0 to h0, then from h0 to t0.
537
Vec3<T> h0 = (f0 + t0).normalized();
541
setRotationInternal (f0, h0, *this);
544
setRotationInternal (h0, t0, q);
551
// f0 and t0 point in exactly opposite directions.
552
// Pick an arbitrary axis that is orthogonal to f0,
558
Vec3<T> f02 = f0 * f0;
560
if (f02.x <= f02.y && f02.x <= f02.z)
561
v = (f0 % Vec3<T> (1, 0, 0)).normalized();
562
else if (f02.y <= f02.z)
563
v = (f0 % Vec3<T> (0, 1, 0)).normalized();
565
v = (f0 % Vec3<T> (0, 0, 1)).normalized();
575
Quat<T>::setRotationInternal (const Vec3<T>& f0, const Vec3<T>& t0, Quat<T> &q)
578
// The following is equivalent to setAxisAngle(n,2*phi),
579
// where the rotation axis, is orthogonal to the f0 and
580
// t0 vectors, and 2*phi is the angle between f0 and t0.
582
// This function is called by setRotation(), above; it assumes
583
// that f0 and t0 are normalized and that the angle between
584
// them is not much greater than pi/2. This function becomes
585
// numerically inaccurate if f0 and t0 point into nearly
586
// opposite directions.
590
// Find a normalized vector, h0, that is half way between f0 and t0.
591
// The angle between f0 and h0 is phi.
594
Vec3<T> h0 = (f0 + t0).normalized();
597
// Store the rotation axis and rotation angle.
600
q.r = f0 ^ h0; // f0 ^ h0 == cos (phi)
601
q.v = f0 % h0; // (f0 % h0).length() == sin (phi)
606
Matrix33<T> Quat<T>::toMatrix33() const
608
return Matrix33<T>(1. - 2.0 * (v.y * v.y + v.z * v.z),
609
2.0 * (v.x * v.y + v.z * r),
610
2.0 * (v.z * v.x - v.y * r),
612
2.0 * (v.x * v.y - v.z * r),
613
1. - 2.0 * (v.z * v.z + v.x * v.x),
614
2.0 * (v.y * v.z + v.x * r),
616
2.0 * (v.z * v.x + v.y * r),
617
2.0 * (v.y * v.z - v.x * r),
618
1. - 2.0 * (v.y * v.y + v.x * v.x));
622
Matrix44<T> Quat<T>::toMatrix44() const
624
return Matrix44<T>(1. - 2.0 * (v.y * v.y + v.z * v.z),
625
2.0 * (v.x * v.y + v.z * r),
626
2.0 * (v.z * v.x - v.y * r),
628
2.0 * (v.x * v.y - v.z * r),
629
1. - 2.0 * (v.z * v.z + v.x * v.x),
630
2.0 * (v.y * v.z + v.x * r),
632
2.0 * (v.z * v.x + v.y * r),
633
2.0 * (v.y * v.z - v.x * r),
634
1. - 2.0 * (v.y * v.y + v.x * v.x),
644
inline Matrix33<T> operator* (const Matrix33<T> &M, const Quat<T> &q)
646
return M * q.toMatrix33();
650
inline Matrix33<T> operator* (const Quat<T> &q, const Matrix33<T> &M)
652
return q.toMatrix33() * M;
656
std::ostream& operator<< (std::ostream &o, const Quat<T> &q)
658
return o << "(" << q.r
667
inline Quat<T> operator* (const Quat<T>& q1, const Quat<T>& q2)
669
// (S1+V1) (S2+V2) = S1 S2 - V1.V2 + S1 V2 + V1 S2 + V1 x V2
670
return Quat<T>( q1.r * q2.r - (q1.v ^ q2.v),
671
q1.r * q2.v + q1.v * q2.r + q1.v % q2.v );
675
inline Quat<T> operator/ (const Quat<T>& q1, const Quat<T>& q2)
677
return q1 * q2.inverse();
681
inline Quat<T> operator/ (const Quat<T>& q,T t)
683
return Quat<T>(q.r/t,q.v/t);
687
inline Quat<T> operator* (const Quat<T>& q,T t)
689
return Quat<T>(q.r*t,q.v*t);
693
inline Quat<T> operator* (T t, const Quat<T>& q)
695
return Quat<T>(q.r*t,q.v*t);
699
inline Quat<T> operator+ (const Quat<T>& q1, const Quat<T>& q2)
701
return Quat<T>( q1.r + q2.r, q1.v + q2.v );
705
inline Quat<T> operator- (const Quat<T>& q1, const Quat<T>& q2)
707
return Quat<T>( q1.r - q2.r, q1.v - q2.v );
711
inline Quat<T> operator~ (const Quat<T>& q)
713
return Quat<T>( q.r, -q.v ); // conjugate: (S+V)* = S-V
717
inline Quat<T> operator- (const Quat<T>& q)
719
return Quat<T>( -q.r, -q.v );
723
inline Vec3<T> operator* (const Vec3<T>& v, const Quat<T>& q)
727
return v + T (2) * (q.r * a + b);
730
#if (defined _WIN32 || defined _WIN64) && defined _MSC_VER
731
#pragma warning(default:4244)
1
///////////////////////////////////////////////////////////////////////////
3
// Copyright (c) 2002, Industrial Light & Magic, a division of Lucas
6
// All rights reserved.
8
// Redistribution and use in source and binary forms, with or without
9
// modification, are permitted provided that the following conditions are
11
// * Redistributions of source code must retain the above copyright
12
// notice, this list of conditions and the following disclaimer.
13
// * Redistributions in binary form must reproduce the above
14
// copyright notice, this list of conditions and the following disclaimer
15
// in the documentation and/or other materials provided with the
17
// * Neither the name of Industrial Light & Magic nor the names of
18
// its contributors may be used to endorse or promote products derived
19
// from this software without specific prior written permission.
21
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
24
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
25
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
26
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
27
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
28
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
29
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
30
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
31
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
33
///////////////////////////////////////////////////////////////////////////
37
#ifndef INCLUDED_IMATHQUAT_H
38
#define INCLUDED_IMATHQUAT_H
40
//----------------------------------------------------------------------
42
// template class Quat<T>
44
// "Quaternions came from Hamilton ... and have been an unmixed
45
// evil to those who have touched them in any way. Vector is a
46
// useless survival ... and has never been of the slightest use
51
// This class implements the quaternion numerical type -- you
52
// will probably want to use this class to represent orientations
53
// in R3 and to convert between various euler angle reps. You
54
// should probably use Imath::Euler<> for that.
56
//----------------------------------------------------------------------
59
#include "ImathMatrix.h"
65
#if (defined _WIN32 || defined _WIN64) && defined _MSC_VER
66
// Disable MS VC++ warnings about conversion from double to float
67
#pragma warning(disable:4244)
74
Quat<T> slerp (const Quat<T> &q1,const Quat<T> &q2, T t);
77
Quat<T> squad (const Quat<T> &q1,const Quat<T> &q2,
78
const Quat<T> &qa,const Quat<T> &qb, T t);
81
void intermediate (const Quat<T> &q0, const Quat<T> &q1,
82
const Quat<T> &q2, const Quat<T> &q3,
83
Quat<T> &qa, Quat<T> &qb);
91
Vec3<T> v; // imaginary vector
93
//-----------------------------------------------------
94
// Constructors - default constructor is identity quat
95
//-----------------------------------------------------
97
Quat() : r(1), v(0,0,0) {}
100
Quat( const Quat<S>& q) : r(q.r), v(q.v) {}
102
Quat( T s, T i, T j, T k ) : r(s), v(i,j,k) {}
104
Quat( T s, Vec3<T> d ) : r(s), v(d) {}
106
static Quat<T> identity() { return Quat<T>(); }
108
//------------------------------------------------
109
// Basic Algebra - Operators and Methods
110
// The operator return values are *NOT* normalized
112
// operator^ is 4D dot product
113
// operator/ uses the inverse() quaternion
114
// operator~ is conjugate -- if (S+V) is quat then
115
// the conjugate (S+V)* == (S-V)
117
// some operators (*,/,*=,/=) treat the quat as
118
// a 4D vector when one of the operands is scalar
119
//------------------------------------------------
121
const Quat<T>& operator= (const Quat<T>&);
122
const Quat<T>& operator*= (const Quat<T>&);
123
const Quat<T>& operator*= (T);
124
const Quat<T>& operator/= (const Quat<T>&);
125
const Quat<T>& operator/= (T);
126
const Quat<T>& operator+= (const Quat<T>&);
127
const Quat<T>& operator-= (const Quat<T>&);
128
T& operator[] (int index); // as 4D vector
129
T operator[] (int index) const;
131
template <class S> bool operator == (const Quat<S> &q) const;
132
template <class S> bool operator != (const Quat<S> &q) const;
134
Quat<T>& invert(); // this -> 1 / this
135
Quat<T> inverse() const;
136
Quat<T>& normalize(); // returns this
137
Quat<T> normalized() const;
138
T length() const; // in R4
140
//-----------------------
141
// Rotation conversion
142
//-----------------------
144
Quat<T>& setAxisAngle(const Vec3<T>& axis, T radians);
145
Quat<T>& setRotation(const Vec3<T>& fromDirection,
146
const Vec3<T>& toDirection);
149
Vec3<T> axis() const;
151
Matrix33<T> toMatrix33() const;
152
Matrix44<T> toMatrix44() const;
159
void setRotationInternal (const Vec3<T>& f0,
165
//--------------------
166
// Convenient typedefs
167
//--------------------
169
typedef Quat<float> Quatf;
170
typedef Quat<double> Quatd;
178
inline const Quat<T>& Quat<T>::operator= (const Quat<T>& q)
186
inline const Quat<T>& Quat<T>::operator*= (const Quat<T>& q)
188
T rtmp = r * q.r - (v ^ q.v);
189
v = r * q.v + v * q.r + v % q.v;
195
inline const Quat<T>& Quat<T>::operator*= (T t)
203
inline const Quat<T>& Quat<T>::operator/= (const Quat<T>& q)
205
*this = *this * q.inverse();
210
inline const Quat<T>& Quat<T>::operator/= (T t)
218
inline const Quat<T>& Quat<T>::operator+= (const Quat<T>& q)
226
inline const Quat<T>& Quat<T>::operator-= (const Quat<T>& q)
233
inline T& Quat<T>::operator[] (int index)
235
return index ? v[index-1] : r;
239
inline T Quat<T>::operator[] (int index) const
241
return index ? v[index-1] : r;
247
Quat<T>::operator == (const Quat<S> &q) const
249
return r == q.r && v == q.v;
255
Quat<T>::operator != (const Quat<S> &q) const
257
return r != q.r || v != q.v;
261
inline T operator^ (const Quat<T>& q1,const Quat<T>& q2)
263
return q1.r * q2.r + (q1.v ^ q2.v);
267
inline T Quat<T>::length() const
269
return Math<T>::sqrt( r * r + (v ^ v) );
273
inline Quat<T>& Quat<T>::normalize()
275
if ( T l = length() ) { r /= l; v /= l; }
276
else { r = 1; v = Vec3<T>(0); }
281
inline Quat<T> Quat<T>::normalized() const
283
if ( T l = length() ) return Quat( r / l, v / l );
288
inline Quat<T> Quat<T>::inverse() const
291
// - = ---- where Q* is conjugate (operator~)
292
// Q Q* Q and (Q* Q) == Q ^ Q (4D dot)
294
T qdot = *this ^ *this;
295
return Quat( r / qdot, -v / qdot );
299
inline Quat<T>& Quat<T>::invert()
301
T qdot = (*this) ^ (*this);
310
angle4D (const Quat<T> &q1, const Quat<T> &q2)
313
// Compute the angle between two quaternions,
314
// interpreting the quaternions as 4D vectors.
318
T lengthD = Math<T>::sqrt (d ^ d);
321
T lengthS = Math<T>::sqrt (s ^ s);
323
return 2 * Math<T>::atan2 (lengthD, lengthS);
329
slerp(const Quat<T> &q1,const Quat<T> &q2, T t)
332
// Spherical linear interpolation.
333
// Assumes q1 and q2 are normalized and that q1 != -q2.
335
// This method does *not* interpolate along the shortest
336
// arc between q1 and q2. If you desire interpolation
337
// along the shortest arc, and q1^q2 is negative, then
338
// consider flipping the second quaternion explicitly.
340
// The implementation of squad() depends on a slerp()
341
// that interpolates as is, without the automatic
344
// Don Hatch explains the method we use here on his
345
// web page, The Right Way to Calculate Stuff, at
346
// http://www.plunk.org/~hatch/rightway.php
349
T a = angle4D (q1, q2);
352
Quat<T> q = sinx_over_x (s * a) / sinx_over_x (a) * s * q1 +
353
sinx_over_x (t * a) / sinx_over_x (a) * t * q2;
355
return q.normalized();
360
Quat<T> spline(const Quat<T> &q0, const Quat<T> &q1,
361
const Quat<T> &q2, const Quat<T> &q3,
364
// Spherical Cubic Spline Interpolation -
365
// from Advanced Animation and Rendering
366
// Techniques by Watt and Watt, Page 366:
367
// A spherical curve is constructed using three
368
// spherical linear interpolations of a quadrangle
369
// of unit quaternions: q1, qa, qb, q2.
370
// Given a set of quaternion keys: q0, q1, q2, q3,
371
// this routine does the interpolation between
372
// q1 and q2 by constructing two intermediate
373
// quaternions: qa and qb. The qa and qb are
374
// computed by the intermediate function to
375
// guarantee the continuity of tangents across
376
// adjacent cubic segments. The qa represents in-tangent
377
// for q1 and the qb represents the out-tangent for q2.
379
// The q1 q2 is the cubic segment being interpolated.
380
// The q0 is from the previous adjacent segment and q3 is
381
// from the next adjacent segment. The q0 and q3 are used
382
// in computing qa and qb.
385
Quat<T> qa = intermediate (q0, q1, q2);
386
Quat<T> qb = intermediate (q1, q2, q3);
387
Quat<T> result = squad(q1, qa, qb, q2, t);
393
Quat<T> squad(const Quat<T> &q1, const Quat<T> &qa,
394
const Quat<T> &qb, const Quat<T> &q2,
397
// Spherical Quadrangle Interpolation -
398
// from Advanced Animation and Rendering
399
// Techniques by Watt and Watt, Page 366:
400
// It constructs a spherical cubic interpolation as
401
// a series of three spherical linear interpolations
402
// of a quadrangle of unit quaternions.
405
Quat<T> r1 = slerp(q1, q2, t);
406
Quat<T> r2 = slerp(qa, qb, t);
407
Quat<T> result = slerp(r1, r2, 2*t*(1-t));
413
Quat<T> intermediate(const Quat<T> &q0, const Quat<T> &q1, const Quat<T> &q2)
415
// From advanced Animation and Rendering
416
// Techniques by Watt and Watt, Page 366:
417
// computing the inner quadrangle
418
// points (qa and qb) to guarantee tangent
421
Quat<T> q1inv = q1.inverse();
422
Quat<T> c1 = q1inv*q2;
423
Quat<T> c2 = q1inv*q0;
424
Quat<T> c3 = (T) (-0.25) * (c2.log() + c1.log());
425
Quat<T> qa = q1 * c3.exp();
431
inline Quat<T> Quat<T>::log() const
434
// For unit quaternion, from Advanced Animation and
435
// Rendering Techniques by Watt and Watt, Page 366:
438
T theta = Math<T>::acos (std::min (r, (T) 1.0));
441
return Quat<T> (0, v);
443
T sintheta = Math<T>::sin (theta);
446
if (abs (sintheta) < 1 && abs (theta) >= limits<T>::max() * abs (sintheta))
449
k = theta / sintheta;
451
return Quat<T> ((T) 0, v.x * k, v.y * k, v.z * k);
455
inline Quat<T> Quat<T>::exp() const
458
// For pure quaternion (zero scalar part):
459
// from Advanced Animation and Rendering
460
// Techniques by Watt and Watt, Page 366:
463
T theta = v.length();
464
T sintheta = Math<T>::sin (theta);
467
if (abs (theta) < 1 && abs (sintheta) >= limits<T>::max() * abs (theta))
470
k = sintheta / theta;
472
T costheta = Math<T>::cos (theta);
474
return Quat<T> (costheta, v.x * k, v.y * k, v.z * k);
478
inline T Quat<T>::angle() const
480
return 2.0*Math<T>::acos(r);
484
inline Vec3<T> Quat<T>::axis() const
486
return v.normalized();
490
inline Quat<T>& Quat<T>::setAxisAngle(const Vec3<T>& axis, T radians)
492
r = Math<T>::cos(radians/2);
493
v = axis.normalized() * Math<T>::sin(radians/2);
500
Quat<T>::setRotation(const Vec3<T>& from, const Vec3<T>& to)
503
// Create a quaternion that rotates vector from into vector to,
504
// such that the rotation is around an axis that is the cross
505
// product of from and to.
507
// This function calls function setRotationInternal(), which is
508
// numerically accurate only for rotation angles that are not much
509
// greater than pi/2. In order to achieve good accuracy for angles
510
// greater than pi/2, we split large angles in half, and rotate in
515
// Normalize from and to, yielding f0 and t0.
518
Vec3<T> f0 = from.normalized();
519
Vec3<T> t0 = to.normalized();
524
// The rotation angle is less than or equal to pi/2.
527
setRotationInternal (f0, t0, *this);
532
// The angle is greater than pi/2. After computing h0,
533
// which is halfway between f0 and t0, we rotate first
534
// from f0 to h0, then from h0 to t0.
537
Vec3<T> h0 = (f0 + t0).normalized();
541
setRotationInternal (f0, h0, *this);
544
setRotationInternal (h0, t0, q);
551
// f0 and t0 point in exactly opposite directions.
552
// Pick an arbitrary axis that is orthogonal to f0,
558
Vec3<T> f02 = f0 * f0;
560
if (f02.x <= f02.y && f02.x <= f02.z)
561
v = (f0 % Vec3<T> (1, 0, 0)).normalized();
562
else if (f02.y <= f02.z)
563
v = (f0 % Vec3<T> (0, 1, 0)).normalized();
565
v = (f0 % Vec3<T> (0, 0, 1)).normalized();
575
Quat<T>::setRotationInternal (const Vec3<T>& f0, const Vec3<T>& t0, Quat<T> &q)
578
// The following is equivalent to setAxisAngle(n,2*phi),
579
// where the rotation axis, is orthogonal to the f0 and
580
// t0 vectors, and 2*phi is the angle between f0 and t0.
582
// This function is called by setRotation(), above; it assumes
583
// that f0 and t0 are normalized and that the angle between
584
// them is not much greater than pi/2. This function becomes
585
// numerically inaccurate if f0 and t0 point into nearly
586
// opposite directions.
590
// Find a normalized vector, h0, that is half way between f0 and t0.
591
// The angle between f0 and h0 is phi.
594
Vec3<T> h0 = (f0 + t0).normalized();
597
// Store the rotation axis and rotation angle.
600
q.r = f0 ^ h0; // f0 ^ h0 == cos (phi)
601
q.v = f0 % h0; // (f0 % h0).length() == sin (phi)
606
Matrix33<T> Quat<T>::toMatrix33() const
608
return Matrix33<T>(1. - 2.0 * (v.y * v.y + v.z * v.z),
609
2.0 * (v.x * v.y + v.z * r),
610
2.0 * (v.z * v.x - v.y * r),
612
2.0 * (v.x * v.y - v.z * r),
613
1. - 2.0 * (v.z * v.z + v.x * v.x),
614
2.0 * (v.y * v.z + v.x * r),
616
2.0 * (v.z * v.x + v.y * r),
617
2.0 * (v.y * v.z - v.x * r),
618
1. - 2.0 * (v.y * v.y + v.x * v.x));
622
Matrix44<T> Quat<T>::toMatrix44() const
624
return Matrix44<T>(1. - 2.0 * (v.y * v.y + v.z * v.z),
625
2.0 * (v.x * v.y + v.z * r),
626
2.0 * (v.z * v.x - v.y * r),
628
2.0 * (v.x * v.y - v.z * r),
629
1. - 2.0 * (v.z * v.z + v.x * v.x),
630
2.0 * (v.y * v.z + v.x * r),
632
2.0 * (v.z * v.x + v.y * r),
633
2.0 * (v.y * v.z - v.x * r),
634
1. - 2.0 * (v.y * v.y + v.x * v.x),
644
inline Matrix33<T> operator* (const Matrix33<T> &M, const Quat<T> &q)
646
return M * q.toMatrix33();
650
inline Matrix33<T> operator* (const Quat<T> &q, const Matrix33<T> &M)
652
return q.toMatrix33() * M;
656
std::ostream& operator<< (std::ostream &o, const Quat<T> &q)
658
return o << "(" << q.r
667
inline Quat<T> operator* (const Quat<T>& q1, const Quat<T>& q2)
669
// (S1+V1) (S2+V2) = S1 S2 - V1.V2 + S1 V2 + V1 S2 + V1 x V2
670
return Quat<T>( q1.r * q2.r - (q1.v ^ q2.v),
671
q1.r * q2.v + q1.v * q2.r + q1.v % q2.v );
675
inline Quat<T> operator/ (const Quat<T>& q1, const Quat<T>& q2)
677
return q1 * q2.inverse();
681
inline Quat<T> operator/ (const Quat<T>& q,T t)
683
return Quat<T>(q.r/t,q.v/t);
687
inline Quat<T> operator* (const Quat<T>& q,T t)
689
return Quat<T>(q.r*t,q.v*t);
693
inline Quat<T> operator* (T t, const Quat<T>& q)
695
return Quat<T>(q.r*t,q.v*t);
699
inline Quat<T> operator+ (const Quat<T>& q1, const Quat<T>& q2)
701
return Quat<T>( q1.r + q2.r, q1.v + q2.v );
705
inline Quat<T> operator- (const Quat<T>& q1, const Quat<T>& q2)
707
return Quat<T>( q1.r - q2.r, q1.v - q2.v );
711
inline Quat<T> operator~ (const Quat<T>& q)
713
return Quat<T>( q.r, -q.v ); // conjugate: (S+V)* = S-V
717
inline Quat<T> operator- (const Quat<T>& q)
719
return Quat<T>( -q.r, -q.v );
723
inline Vec3<T> operator* (const Vec3<T>& v, const Quat<T>& q)
727
return v + T (2) * (q.r * a + b);
730
#if (defined _WIN32 || defined _WIN64) && defined _MSC_VER
731
#pragma warning(default:4244)
added
removed
Source/OpenEXR/Imath/ImathQuat.h
