1 //------------------------------------------------------------------------------
3 //------------------------------------------------------------------------------
4 // GLVU : Copyright 1997 - 2002
5 // The University of North Carolina at Chapel Hill
6 //------------------------------------------------------------------------------
7 // Permission to use, copy, modify, distribute and sell this software and its
8 // documentation for any purpose is hereby granted without fee, provided that
9 // the above copyright notice appear in all copies and that both that copyright
10 // notice and this permission notice appear in supporting documentation.
11 // Binaries may be compiled with this software without any royalties or
14 // The University of North Carolina at Chapel Hill makes no representations
15 // about the suitability of this software for any purpose. It is provided
16 // "as is" without express or implied warranty.
18 /***********************************************************************
22 A quaternion template class
24 -------------------------------------------------------------------
26 Feb 1998, Paul Rademacher (rademach@cs.unc.edu)
30 - See main header file, quat.hpp
32 ************************************************************************/
39 //============================================================================
41 //============================================================================
44 Quat<Type>::Quat( void )
46 // do nothing so default construction is fast
50 Quat<Type>::Quat( Type _s, Type x, Type y, Type z )
56 Quat<Type>::Quat( Type x, Type y, Type z )
63 Quat<Type>::Quat( const qvec& _v, Type _s )
69 Quat<Type>::Quat( Type _s, const qvec& _v )
76 Quat<Type>::Quat( const Type *d )
83 Quat<Type>::Quat( const Quat &q )
89 //============================================================================
91 //============================================================================
94 void Quat<Type>::Set( Type _s, Type x, Type y, Type z )
100 void Quat<Type>::Set( Type x, Type y, Type z )
106 template <class Type>
107 void Quat<Type>::Set( const qvec& _v, Type _s )
112 template <class Type>
113 void Quat<Type>::Set( Type _s, const qvec& _v )
120 //============================================================================
122 //============================================================================
124 template <class Type>
125 Quat<Type>& Quat<Type>::operator = (const Quat& q)
127 v = q.v; s = q.s; return *this;
130 template <class Type>
131 Quat<Type>& Quat<Type>::operator += ( const Quat &q )
133 v += q.v; s += q.s; return *this;
136 template <class Type>
137 Quat<Type>& Quat<Type>::operator -= ( const Quat &q )
139 v -= q.v; s -= q.s; return *this;
142 template <class Type>
143 Quat<Type> &Quat<Type>::operator *= ( const Type d )
145 v *= d; s *= d; return *this;
148 template <class Type>
149 Quat<Type> &Quat<Type>::operator *= ( const Quat& q )
152 // Quaternion multiplication with
153 // temporary object construction minimized (hopefully)
154 Type ns = s*q.s - v*q.v;
163 // optimized (12 mults, and no compiler-generated temp objects)
164 Type A, B, C, D, E, F, G, H;
166 A = (s + v.x)*(q.s + q.v.x);
167 B = (v.z - v.y)*(q.v.y - q.v.z);
168 C = (s - v.x)*(q.v.y + q.v.z);
169 D = (v.y + v.z)*(q.s - q.v.x);
170 E = (v.x + v.z)*(q.v.x + q.v.y);
171 F = (v.x - v.z)*(q.v.x - q.v.y);
172 G = (s + v.y)*(q.s - q.v.z);
173 H = (s - v.y)*(q.s + q.v.z);
175 v.x = A - (E + F + G + H) * Type(0.5);
176 v.y = C + (E - F + G - H) * Type(0.5);
177 v.z = D + (E - F - G + H) * Type(0.5);
178 s = B + (-E - F + G + H) * Type(0.5);
184 template <class Type>
185 Quat<Type> &Quat<Type>::operator /= ( const Type d )
187 Type r = Type(1.0)/d;
193 template <class Type>
194 Type &Quat<Type>::operator [] ( int i)
206 //============================================================================
208 //============================================================================
209 template <class Type>
210 inline Type Quat<Type>::Length( void ) const
212 return Type( sqrt( v*v + s*s ) );
215 template <class Type>
216 inline Type Quat<Type>::LengthSqr( void ) const
221 template <class Type>
222 inline Type Quat<Type>::Norm( void ) const
227 template <class Type>
228 inline Quat<Type>& Quat<Type>::Normalize( void )
230 *this *= Type(1.0) / Type(sqrt(v*v + s*s));
234 template <class Type>
235 inline Quat<Type>& Quat<Type>::Invert( void )
237 Type scale = Type(1.0)/Norm();
243 template <class Type>
244 inline Quat<Type>& Quat<Type>::Conjugate( void )
253 //----------------------------------------------------------------------------
255 //----------------------------------------------------------------------------
256 // Transform a vector by this quaternion using q * v * q^-1
257 //----------------------------------------------------------------------------
258 template <class Type>
259 Quat<Type>::qvec Quat<Type>::Xform( const qvec &vec ) const
261 /* copy vector into temp quaternion for multiply */
263 /* invert multiplier */
267 /* do q * vec * q(inv) */
268 Quat tempVecQuat(*this * vecQuat);
269 tempVecQuat *= inverse;
271 /* return vector part */
272 return tempVecQuat.v;
275 //----------------------------------------------------------------------------
277 //----------------------------------------------------------------------------
278 // Natural log of quat
279 //----------------------------------------------------------------------------
280 template <class Type>
281 Quat<Type> &Quat<Type>::Log(void)
286 theta = ATan2(scale, s);
296 //----------------------------------------------------------------------------
298 //----------------------------------------------------------------------------
299 // e to the quat: e^quat
300 // -- assuming scalar part 0
301 //----------------------------------------------------------------------------
302 template <class Type>
303 Quat<Type> &Quat<Type>::Exp(void)
306 Type theta = v.Length();
308 if (theta > FUDGE()) {
309 scale = Sin(theta)/theta ;
318 //----------------------------------------------------------------------------
319 // SetAngle (radians)
320 //----------------------------------------------------------------------------
321 template <class Type>
322 void Quat<Type>::SetAngle( Type f )
324 qvec axis(GetAxis());
330 //----------------------------------------------------------------------------
332 //----------------------------------------------------------------------------
333 template <class Type>
334 inline void Quat<Type>::ScaleAngle( Type f )
336 SetAngle( f * GetAngle() );
339 //----------------------------------------------------------------------------
340 // GetAngle (radians)
341 //----------------------------------------------------------------------------
342 // get rot angle in radians. Assumes s is between -1 and 1, which will always
343 // be the case for unit quaternions.
344 //----------------------------------------------------------------------------
345 template <class Type>
346 inline Type Quat<Type>::GetAngle( void ) const
348 return ( Type(2.0) * ACos( s ) );
351 //----------------------------------------------------------------------------
353 //----------------------------------------------------------------------------
354 template <class Type>
355 Quat<Type>::qvec Quat<Type>::GetAxis( void ) const
359 scale = Sin( acos( s ) ) ;
360 if ( scale < FUDGE() && scale > -FUDGE() )
361 return qvec( 0.0, 0.0, 0.0 );
366 //----------------------------------------------------------------------------
368 //----------------------------------------------------------------------------
369 template <class Type>
370 inline void Quat<Type>::Print( ) const
372 printf( "(%3.2f, <%3.2f %3.2f %3.2f>)\n", s, v.x, v.y, v.z );
376 //============================================================================
378 //============================================================================
380 template <class Type>
381 Mat44<Type>& Quat<Type>::ToMat( Mat44<Type>& dest ) const
383 Type t, xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
386 t = Type(2.0) / (v*v + s*s);
389 xs = v.x*t; ys = v.y*t; zs = v.z*t;
390 wx = s*xs; wy = s*ys; wz = s*zs;
391 xx = v.x*xs; xy = v.x*ys; xz = v.x*zs;
392 yy = v.y*ys; yz = v.y*zs; zz = v.z*zs;
394 dest.Set( ONE-(yy+zz), xy-wz, xz+wy, 0.0,
395 xy+wz, ONE-(xx+zz), yz-wx, 0.0,
396 xz-wy, yz+wx, ONE-(xx+yy), 0.0,
397 0.0, 0.0, 0.0, ONE );
402 template <class Type>
403 Mat33<Type>& Quat<Type>::ToMat( Mat33<Type>& dest ) const
405 Type t, xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
408 t = Type(2.0) / Norm();
411 xs = v.x*t; ys = v.y*t; zs = v.z*t;
412 wx = s*xs; wy = s*ys; wz = s*zs;
413 xx = v.x*xs; xy = v.x*ys; xz = v.x*zs;
414 yy = v.y*ys; yz = v.y*zs; zz = v.z*zs;
416 dest.Set( ONE-(yy+zz), xy-wz, xz+wy,
417 xy+wz, ONE-(xx+zz), yz-wx,
418 xz-wy, yz+wx, ONE-(xx+yy) );
423 //----------------------------------------------------------------------------
425 //----------------------------------------------------------------------------
426 // Convert rotation matrix to quaternion
427 // Results will be bad if matrix is not (very close to) orthonormal
428 // Modified from gamasutra.com article:
429 // http://www.gamasutra.com/features/programming/19980703/quaternions_07.htm
430 //----------------------------------------------------------------------------
431 template <class Type>
432 Quat<Type>& Quat<Type>::FromMat( const Mat44<Type>& m )
436 // check the diagonal
438 Type scale = Type( sqrt (tr) );
439 s = scale * Type(0.5);
440 scale = Type(0.5) / scale;
441 v.x = (m(1,2) - m(2,1)) * scale;
442 v.y = (m(2,0) - m(0,2)) * scale;
443 v.z = (m(0,1) - m(1,0)) * scale;
445 // diagonal is negative or zero
448 if (m(1,1) > m(0,0)) i = 1;
449 if (m(2,2) > m(i,i)) i = 2;
450 int nxt[3] = {1, 2, 0};
454 Type scale = Type( sqrt (Type(1.0) + m(i,i) - (m(j,j) + m(k,k)) ) );
456 v[i] = scale * Type(0.5);
458 if (scale != 0.0) scale = Type(0.5) / scale;
460 s = (m(j,k) - m(k,j)) * scale;
461 v[j] = (m(i,j) + m(j,i)) * scale;
462 v[k] = (m(i,k) + m(k,i)) * scale;
467 template <class Type>
468 Quat<Type>& Quat<Type>::FromMat( const Mat33<Type>& m )
472 // check the diagonal
474 Type scale = Type( sqrt (tr + Type(1.0)) );
475 s = scale * Type(0.5);
476 scale = Type(0.5) / scale;
477 v.x = (m(1,2) - m(2,1)) * scale;
478 v.y = (m(2,0) - m(0,2)) * scale;
479 v.z = (m(0,1) - m(1,0)) * scale;
481 // diagonal is negative or zero
484 if (m(1,1) > m(0,0)) i = 1;
485 if (m(2,2) > m(i,i)) i = 2;
486 int nxt[3] = {1, 2, 0};
490 Type scale = Type( sqrt (Type(1.0) + m(i,i) - (m(j,j) + m(k,k)) ) );
492 v[i] = scale * Type(0.5);
494 if (scale != 0.0) scale = Type(0.5) / scale;
496 s = (m(j,k) - m(k,j)) * scale;
497 v[j] = (m(i,j) + m(j,i)) * scale;
498 v[k] = (m(i,k) + m(k,i)) * scale;
503 //----------------------------------------------------------------------------
504 // ToAngleAxis (radians)
505 //----------------------------------------------------------------------------
506 // Convert to angle & axis representation
507 //----------------------------------------------------------------------------
508 template <class Type>
509 void Quat<Type>::ToAngleAxis( Type &angle, qvec &axis ) const
511 Type cinv = ACos( s );
512 angle = Type(2.0) * cinv;
517 if ( scale < FUDGE() && scale > -FUDGE() )
525 //----------------------------------------------------------------------------
526 // FromAngleAxis (radians)
527 //----------------------------------------------------------------------------
528 // Convert to quat from angle & axis representation
529 //----------------------------------------------------------------------------
530 template <class Type>
531 Quat<Type>& Quat<Type>::FromAngleAxis( Type angle, const qvec &axis )
533 /* normalize vector */
534 Type length = axis.Length();
536 /* if zero vector passed in, just set to identity quaternion */
537 if ( length < FUDGE() )
542 length = Type(1.0)/length;
552 //----------------------------------------------------------------------------
554 //----------------------------------------------------------------------------
555 // Return the quat that rotates vector a into vector b
556 //----------------------------------------------------------------------------
557 template <class Type>
558 Quat<Type>& Quat<Type>::FromTwoVecs(const qvec &a, const qvec& b)
562 double theta ; /* angle of rotation about axis */
563 double theta_complement ;
564 double crossProductMagnitude ;
567 // Normalize both vectors and take cross product to get rotation axis.
570 qvec axis( u1 ^ u2 );
573 // | u1 X u2 | = |u1||u2|sin(theta)
575 // Since u1 and u2 are normalized,
577 // theta = arcsin(|axis|)
578 crossProductMagnitude = axis.Length();
580 // Occasionally, even though the vectors are normalized, the
581 // magnitude will be calculated to be slightly greater than one. If
582 // this happens, just set it to 1 or asin() will barf.
583 if( crossProductMagnitude > Type(1.0) )
584 crossProductMagnitude = Type(1.0) ;
586 // Take arcsin of magnitude of rotation axis to compute rotation
587 // angle. Since crossProductMagnitude=[0,1], we will have
589 theta = ASin( crossProductMagnitude ) ;
590 theta_complement = Type(3.14159265358979323846) - theta ;
592 // If cos(theta) < 0, use complement of theta as rotation angle.
596 theta = theta_complement ;
597 theta_complement = tmp;
600 // if angle is 0, just return identity quaternion
601 if( theta < FUDGE() )
607 if( theta_complement < FUDGE() )
609 // The two vectors are opposed. Find some arbitrary axis vector.
610 // First try cross product with x-axis if u1 not parallel to x-axis.
611 if( (u1.y*u1.y + u1.z*u1.z) >= FUDGE() )
613 axis.Set( 0.0, u1.z, -u1.y ) ;
617 // u1 is parallel to to x-axis. Use z-axis as axis of rotation.
618 axis.Set(0.0, 0.0, 1.0);
623 FromAngleAxis(Type(theta), axis);
629 //----------------------------------------------------------------------------
631 //----------------------------------------------------------------------------
632 // converts 3 euler angles (in radians) to a quaternion
634 // angles are in radians; Assumes roll is rotation about X, pitch is
635 // rotation about Y, yaw is about Z. (So thinking of
636 // Z as up) Assumes order of yaw, pitch, roll applied as follows:
638 // p' = roll( pitch( yaw(p) ) )
640 // Where yaw, pitch, and roll are defined in the BODY coordinate sys.
641 // In other words these are ZYX-relative (or XYZ-fixed) Euler Angles.
643 // For a complete Euler angle implementation that handles all 24 angle
644 // sets, see "Euler Angle Conversion" by Ken Shoemake, in "Graphics
645 // Gems IV", Academic Press, 1994
646 //----------------------------------------------------------------------------
647 template <class Type>
648 Quat<Type>& Quat<Type>::FromEuler(Type yaw, Type pitch, Type roll)
650 Type cosYaw, sinYaw, cosPitch, sinPitch, cosRoll, sinRoll;
651 Type half_roll, half_pitch, half_yaw;
653 /* put angles into radians and divide by two, since all angles in formula
656 const Type HALF(0.5);
657 half_yaw = yaw * HALF;
658 half_pitch = pitch * HALF;
659 half_roll = roll * HALF;
661 cosYaw = Cos(half_yaw);
662 sinYaw = Sin(half_yaw);
664 cosPitch = Cos(half_pitch);
665 sinPitch = Sin(half_pitch);
667 cosRoll = Cos(half_roll);
668 sinRoll = Sin(half_roll);
670 Type cpcy = cosPitch * cosYaw;
671 Type spsy = sinPitch * sinYaw;
673 v.x = sinRoll * cpcy - cosRoll * spsy;
674 v.y = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
675 v.z = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
676 s = cosRoll * cpcy + sinRoll * spsy;
681 //----------------------------------------------------------------------------
683 //----------------------------------------------------------------------------
684 // converts a quaternion to 3 euler angles (in radians)
686 // See FromEuler for details of which set of Euler Angles are returned
687 //----------------------------------------------------------------------------
688 template <class Type>
689 void Quat<Type>::ToEuler(Type& yaw, Type& pitch, Type& roll) const
691 // This is probably wrong
694 const int i = 0, j = 1, k = 2;
695 double cy = sqrt(M(i,i)*M(i,i) + M(i,j)*M(i,j));
697 roll = ATan2(M(j,k), M(k,k));
698 pitch = ATan2(-M(i,k), cy);
699 yaw = ATan2(M(i,j), M(i,i));
701 roll = ATan2(-M(k,j), M(j,j));
702 pitch = ATan2(-M(i,k), cy);
707 //============================================================================
709 //============================================================================
712 template <class Type>
713 Quat<Type> operator + (const Quat<Type> &a, const Quat<Type> &b)
715 return Quat<Type>( a.s+b.s, a.v+b.v );
718 template <class Type>
719 Quat<Type> operator - (const Quat<Type> &a, const Quat<Type> &b)
721 return Quat<Type>( a.s-b.s, a.v-b.v );
724 template <class Type>
725 Quat<Type> operator - (const Quat<Type> &a )
727 return Quat<Type>( -a.s, -a.v );
730 template <class Type>
731 Quat<Type> operator * ( const Quat<Type> &a, const Quat<Type> &b)
735 return Quat<Type>( a.s*b.s - a.v*b.v, a.s*b.v + b.s*a.v + a.v^b.v );
737 // optimized (12 mults, and no compiler-generated temp objects)
738 Type A, B, C, D, E, F, G, H;
740 A = (a.s + a.v.x)*(b.s + b.v.x);
741 B = (a.v.z - a.v.y)*(b.v.y - b.v.z);
742 C = (a.s - a.v.x)*(b.v.y + b.v.z);
743 D = (a.v.y + a.v.z)*(b.s - b.v.x);
744 E = (a.v.x + a.v.z)*(b.v.x + b.v.y);
745 F = (a.v.x - a.v.z)*(b.v.x - b.v.y);
746 G = (a.s + a.v.y)*(b.s - b.v.z);
747 H = (a.s - a.v.y)*(b.s + b.v.z);
750 B + (-E - F + G + H) * Type(0.5),
751 A - (E + F + G + H) * Type(0.5),
752 C + (E - F + G - H) * Type(0.5),
753 D + (E - F - G + H) * Type(0.5));
757 template <class Type>
758 Quat<Type> operator * ( const Quat<Type> &a, const Type t)
760 return Quat<Type>( a.v * t, a.s * t );
763 template <class Type>
764 Quat<Type> operator * ( const Type t, const Quat<Type> &a )
766 return Quat<Type>( a.v * t, a.s * t );
768 template <class Type>
769 Quat<Type> operator / ( const Quat<Type> &a, const Type t )
771 return Quat<Type>( a.v / t, a.s / t );
774 template <class Type>
775 bool operator == (const Quat<Type> &a, const Quat<Type> &b)
777 return (a.s == b.s && a.v == b.v);
779 template <class Type>
780 bool operator != (const Quat<Type> &a, const Quat<Type> &b)
782 return (a.s != b.s || a.v != b.v);
786 //============================================================================
788 //============================================================================
789 template <class Type>
790 inline Type Quat<Type>::DEG2RAD(Type d)
792 return d * Type(0.0174532925199432957692369076848861);
794 template <class Type>
795 inline Type Quat<Type>::RAD2DEG(Type d)
797 return d * Type(57.2957795130823208767981548141052);
800 template <class Type>
801 inline Type Quat<Type>::Sin(double d) { return Type(sin(d)); }
802 template <class Type>
803 inline Type Quat<Type>::Cos(double d) { return Type(cos(d)); }
804 template <class Type>
805 inline Type Quat<Type>::ACos(double d) { return Type(acos(d)); }
806 template <class Type>
807 inline Type Quat<Type>::ASin(double d) { return Type(asin(d)); }
808 template <class Type>
809 inline Type Quat<Type>::ATan(double d) { return Type(atan(d)); }
810 template <class Type>
811 inline Type Quat<Type>::ATan2(double n, double d) {return Type(atan2(n,d));}
813 template <class Type>
814 inline Quat<Type> Quat<Type>::ZERO() {return Quat(0,0,0,0); }
815 template <class Type>
816 inline Quat<Type> Quat<Type>::IDENTITY() {return Quat(1,0,0,0); }
819 inline Type Quat<Type>::FUDGE() { return 1e-6; }
821 inline double Quat<double>::FUDGE() { return 1e-10; }
823 //----------------------------------------------------------------------------
825 //----------------------------------------------------------------------------
826 template <class Type>
827 Quat<Type>& QuatSlerp(
829 const Quat<Type> &from, const Quat<Type> &to, Type t )
832 // compact mathematical version
833 // exp(t*log(to*from^-1))*from
834 Quat<Type> fminv(from);
835 Quat<Type> tofrom(to*fminv.Invert());
836 Quat<Type> slerp = t*tofrom.Log();
842 double omega, cosom, sinom, scale0, scale1;
844 /* calculate cosine */
845 cosom = from.v * to.v + from.s + to.s;
847 /* Adjust signs (if necessary) so we take shorter path */
857 /* Calculate coefficients */
858 if ((1.0 - cosom) > Quat<Type>::FUDGE ) {
859 /* standard case (slerp) */
860 omega = acos( cosom );
861 sinom = sin( omega );
862 scale0 = sin((1.0 - t) * omega) / sinom;
863 scale1 = sin(t * omega) / sinom;
866 /* 'from' and 'to' are very close - just do linear interpolation */
872 dest *= Type(scale0);
873 dest += Type(scale1) * to1;
877 // This version creates more temporary objects
878 template <class Type>
879 inline Quat<Type> QuatSlerp(
880 const Quat<Type>& from, const Quat<Type>& to, Type t )
883 return QuatSlerp(q, from, to, t);