source: projectionDesigner/trunk/projdesigner/include/gmtl/QuatOps.h @ 397

Last change on this file since 397 was 4, checked in by Torben Dannhauer, 15 years ago
File size: 21.0 KB
Line 
1/************************************************************** ggt-head beg
2 *
3 * GGT: Generic Graphics Toolkit
4 *
5 * Original Authors:
6 *   Allen Bierbaum
7 *
8 * -----------------------------------------------------------------
9 * File:          QuatOps.h,v
10 * Date modified: 2004/05/25 16:36:28
11 * Version:       1.26
12 * -----------------------------------------------------------------
13 *
14 *********************************************************** ggt-head end */
15/*************************************************************** ggt-cpr beg
16*
17* GGT: The Generic Graphics Toolkit
18* Copyright (C) 2001,2002 Allen Bierbaum
19*
20* This library is free software; you can redistribute it and/or
21* modify it under the terms of the GNU Lesser General Public
22* License as published by the Free Software Foundation; either
23* version 2.1 of the License, or (at your option) any later version.
24*
25* This library is distributed in the hope that it will be useful,
26* but WITHOUT ANY WARRANTY; without even the implied warranty of
27* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
28* Lesser General Public License for more details.
29*
30* You should have received a copy of the GNU Lesser General Public
31* License along with this library; if not, write to the Free Software
32* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
33*
34 ************************************************************ ggt-cpr end */
35#ifndef _GMTL_QUAT_OPS_H_
36#define _GMTL_QUAT_OPS_H_
37
38#include <gmtl/Math.h>
39#include <gmtl/Quat.h>
40
41namespace gmtl
42{
43/** @ingroup Ops Quat
44 * @name Quat Operations
45 * @{
46 */
47       
48   /** product of two quaternions (quaternion product)
49    *  multiplication of quats is much like multiplication of typical complex numbers.
50    *  @post q1q2 = (s1 + v1)(s2 + v2)
51    *  @post result = q1 * q2 (where q2 would be applied first to any xformed geometry)
52    *  @see Quat
53    */
54   template <typename DATA_TYPE>
55   Quat<DATA_TYPE>& mult( Quat<DATA_TYPE>& result, const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2 )
56   {
57      // Here is the easy to understand equation: (grassman product)
58      // scalar_component = q1.s * q2.s - dot(q1.v, q2.v)
59      // vector_component = q2.v * q1.s + q1.v * q2.s + cross(q1.v, q2.v)
60
61      // Here is another version (euclidean product, just FYI)...
62      // scalar_component = q1.s * q2.s + dot(q1.v, q2.v)
63      // vector_component = q2.v * q1.s - q1.v * q2.s - cross(q1.v, q2.v)
64
65      // Here it is, using vector algebra (grassman product)
66      /*
67      const float& w1( q1[Welt] ), w2( q2[Welt] );
68      Vec3 v1( q1[Xelt], q1[Yelt], q1[Zelt] ), v2( q2[Xelt], q2[Yelt], q2[Zelt] );
69
70      float w = w1 * w2 - v1.dot( v2 );
71      Vec3 v = (w1 * v2) + (w2 * v1) + v1.cross( v2 );
72
73      vec[Welt] = w;
74      vec[Xelt] = v[0];
75      vec[Yelt] = v[1];
76      vec[Zelt] = v[2];
77      */
78
79      // Here is the same, only expanded... (grassman product)
80      Quat<DATA_TYPE> temporary; // avoid aliasing problems...
81      temporary[Xelt] = q1[Welt]*q2[Xelt] + q1[Xelt]*q2[Welt] + q1[Yelt]*q2[Zelt] - q1[Zelt]*q2[Yelt];
82      temporary[Yelt] = q1[Welt]*q2[Yelt] + q1[Yelt]*q2[Welt] + q1[Zelt]*q2[Xelt] - q1[Xelt]*q2[Zelt];
83      temporary[Zelt] = q1[Welt]*q2[Zelt] + q1[Zelt]*q2[Welt] + q1[Xelt]*q2[Yelt] - q1[Yelt]*q2[Xelt];
84      temporary[Welt] = q1[Welt]*q2[Welt] - q1[Xelt]*q2[Xelt] - q1[Yelt]*q2[Yelt] - q1[Zelt]*q2[Zelt];
85
86      // use a temporary, in case q1 or q2 is the same as self.
87      result[Xelt] = temporary[Xelt];
88      result[Yelt] = temporary[Yelt];
89      result[Zelt] = temporary[Zelt];
90      result[Welt] = temporary[Welt];
91
92      // don't normalize, because it might not be rotation arithmetic we're doing
93      // (only rotation quats have unit length)
94      return result;
95   }
96
97   /** product of two quaternions (quaternion product)
98    *  Does quaternion multiplication.
99    *  @post temp' = q1 * q2 (where q2 would be applied first to any xformed geometry)
100    *  @see Quat
101    *  @todo metaprogramming on quat operator*()
102    */
103   template <typename DATA_TYPE>
104   Quat<DATA_TYPE> operator*( const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2 )
105   {
106      // (grassman product - see mult() for discussion)
107      // don't normalize, because it might not be rotation arithmetic we're doing
108      // (only rotation quats have unit length)
109      return Quat<DATA_TYPE>( q1[Welt]*q2[Xelt] + q1[Xelt]*q2[Welt] + q1[Yelt]*q2[Zelt] - q1[Zelt]*q2[Yelt],
110                              q1[Welt]*q2[Yelt] + q1[Yelt]*q2[Welt] + q1[Zelt]*q2[Xelt] - q1[Xelt]*q2[Zelt],
111                              q1[Welt]*q2[Zelt] + q1[Zelt]*q2[Welt] + q1[Xelt]*q2[Yelt] - q1[Yelt]*q2[Xelt],
112                              q1[Welt]*q2[Welt] - q1[Xelt]*q2[Xelt] - q1[Yelt]*q2[Yelt] - q1[Zelt]*q2[Zelt] );
113   }
114
115   /** quaternion postmult
116    * @post result' = result * q2 (where q2 is applied first to any xformed geometry)
117    * @see Quat
118    */
119   template <typename DATA_TYPE>
120   Quat<DATA_TYPE>& operator*=( Quat<DATA_TYPE>& result, const Quat<DATA_TYPE>& q2 )
121   {
122      return mult( result, result, q2 );
123   }
124
125   /** Vector negation - negate each element in the quaternion vector.
126    * the negative of a rotation quaternion is geometrically equivelent
127    * to the original. there exist 2 quats for every possible rotation.
128    * @return returns the negation of the given quat.
129    */
130   template <typename DATA_TYPE>
131   Quat<DATA_TYPE>& negate( Quat<DATA_TYPE>& result )
132   {
133      result[0] = -result[0];
134      result[1] = -result[1];
135      result[2] = -result[2];
136      result[3] = -result[3];
137      return result;
138   }
139
140   /** Vector negation - (operator-) return a temporary that is the negative of the given quat.
141    * the negative of a rotation quaternion is geometrically equivelent
142    * to the original. there exist 2 quats for every possible rotation.
143    * @return returns the negation of the given quat
144    */
145   template <typename DATA_TYPE>
146   Quat<DATA_TYPE> operator-( const Quat<DATA_TYPE>& quat )
147   {
148      return Quat<DATA_TYPE>( -quat[0], -quat[1], -quat[2], -quat[3] );
149   }
150
151   /** vector scalar multiplication
152    * @post result' = [qx*s, qy*s, qz*s, qw*s]
153    * @see Quat
154    */
155   template <typename DATA_TYPE>
156   Quat<DATA_TYPE>& mult( Quat<DATA_TYPE>& result, const Quat<DATA_TYPE>& q, DATA_TYPE s )
157   {
158      result[0] = q[0] * s;
159      result[1] = q[1] * s;
160      result[2] = q[2] * s;
161      result[3] = q[3] * s;
162      return result;
163   }
164
165   /** vector scalar multiplication
166    * @post result' = [qx*s, qy*s, qz*s, qw*s]
167    * @see Quat
168    */
169   template <typename DATA_TYPE>
170   Quat<DATA_TYPE> operator*( const Quat<DATA_TYPE>& q, DATA_TYPE s )
171   {
172      Quat<DATA_TYPE> temporary;
173      return mult( temporary, q, s );
174   }
175
176   /** vector scalar multiplication
177    * @post result' = [resultx*s, resulty*s, resultz*s, resultw*s]
178    * @see Quat
179    */
180   template <typename DATA_TYPE>
181   Quat<DATA_TYPE>& operator*=( Quat<DATA_TYPE>& q, DATA_TYPE s )
182   {
183      return mult( q, q, s );
184   }
185
186   /** quotient of two quaternions
187    * @post result = q1 * (1/q2)  (where 1/q2 is applied first to any xform'd geometry)
188    * @see Quat
189    */
190   template <typename DATA_TYPE>
191   Quat<DATA_TYPE>& div( Quat<DATA_TYPE>& result, const Quat<DATA_TYPE>& q1, Quat<DATA_TYPE> q2 )
192   {
193      // multiply q1 by the multiplicative inverse of the quaternion
194      return mult( result, q1, invert( q2 ) );
195   }
196
197   /** quotient of two quaternions
198    * @post result = q1 * (1/q2) (where 1/q2 is applied first to any xform'd geometry)
199    *  @see Quat
200    */
201   template <typename DATA_TYPE>
202   Quat<DATA_TYPE> operator/( const Quat<DATA_TYPE>& q1, Quat<DATA_TYPE> q2 )
203   {
204      return q1 * invert( q2 );
205   }
206
207   /** quotient of two quaternions
208    * @post result = result * (1/q2)  (where 1/q2 is applied first to any xform'd geometry)
209    * @see Quat
210    */
211   template <typename DATA_TYPE>
212   Quat<DATA_TYPE>& operator/=( Quat<DATA_TYPE>& result, const Quat<DATA_TYPE>& q2 )
213   {
214      return div( result, result, q2 );
215   }
216
217   /** quaternion vector scale
218    * @post result = q / s
219    * @see Quat
220    */
221   template <typename DATA_TYPE>
222   Quat<DATA_TYPE>& div( Quat<DATA_TYPE>& result, const Quat<DATA_TYPE>& q, DATA_TYPE s )
223   {
224      result[0] = q[0] / s;
225      result[1] = q[1] / s;
226      result[2] = q[2] / s;
227      result[3] = q[3] / s;
228      return result;
229   }
230
231   /** vector scalar division
232    * @post result' = [qx/s, qy/s, qz/s, qw/s]
233    * @see Quat
234    */
235   template <typename DATA_TYPE>
236   Quat<DATA_TYPE> operator/( const Quat<DATA_TYPE>& q, DATA_TYPE s )
237   {
238      Quat<DATA_TYPE> temporary;
239      return div( temporary, q, s );
240   }
241
242   /** vector scalar division
243    * @post result' = [resultx/s, resulty/s, resultz/s, resultw/s]
244    * @see Quat
245    */
246   template <typename DATA_TYPE>
247   Quat<DATA_TYPE>& operator/=( const Quat<DATA_TYPE>& q, DATA_TYPE s )
248   {
249      return div( q, q, s );
250   }
251
252   /** vector addition
253    * @see Quat
254    */
255   template <typename DATA_TYPE>
256   Quat<DATA_TYPE>& add( Quat<DATA_TYPE>& result, const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2 )
257   {
258      result[0] = q1[0] + q2[0];
259      result[1] = q1[1] + q2[1];
260      result[2] = q1[2] + q2[2];
261      result[3] = q1[3] + q2[3];
262      return result;
263   }
264
265   /** vector addition
266    * @post result' = [qx+s, qy+s, qz+s, qw+s]
267    * @see Quat
268    */
269   template <typename DATA_TYPE>
270   Quat<DATA_TYPE> operator+( const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2 )
271   {
272      Quat<DATA_TYPE> temporary;
273      return add( temporary, q1, q2 );
274   }
275
276   /** vector addition
277    * @post result' = [resultx+s, resulty+s, resultz+s, resultw+s]
278    * @see Quat
279    */
280   template <typename DATA_TYPE>
281   Quat<DATA_TYPE>& operator+=( Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2 )
282   {
283      return add( q1, q1, q2 );
284   }
285
286   /** vector subtraction
287    *  @see Quat
288    */
289   template <typename DATA_TYPE>
290   Quat<DATA_TYPE>& sub( Quat<DATA_TYPE>& result, const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2 )
291   {
292      result[0] = q1[0] - q2[0];
293      result[1] = q1[1] - q2[1];
294      result[2] = q1[2] - q2[2];
295      result[3] = q1[3] - q2[3];
296      return result;
297   }
298
299   /** vector subtraction
300    * @post result' = [qx-s, qy-s, qz-s, qw-s]
301    * @see Quat
302    */
303   template <typename DATA_TYPE>
304   Quat<DATA_TYPE> operator-( const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2 )
305   {
306      Quat<DATA_TYPE> temporary;
307      return sub( temporary, q1, q2 );
308   }
309
310   /** vector subtraction
311    * @post result' = [resultx-s, resulty-s, resultz-s, resultw-s]
312    * @see Quat
313    */
314   template <typename DATA_TYPE>
315   Quat<DATA_TYPE>& operator-=( Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2 )
316   {
317      return sub( q1, q1, q2 );
318   }
319
320   /** vector dot product between two quaternions.
321    *  get the lengthSquared between two quat vectors...
322    *  @post N(q) = x1*x2 + y1*y2 + z1*z2 + w1*w2
323    *  @return dot product of q1 and q2
324    *  @see Quat
325    */
326   template <typename DATA_TYPE>
327   DATA_TYPE dot( const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2 )
328   {
329      return DATA_TYPE( (q1[0] * q2[0]) +
330                        (q1[1] * q2[1]) +
331                        (q1[2] * q2[2]) +
332                        (q1[3] * q2[3])  );
333   }
334
335   /** quaternion "norm" (also known as vector length squared)
336    *  using this can be faster than using length for some operations...
337    *  @post returns the vector length squared
338    *  @post N(q) = x^2 + y^2 + z^2 + w^2
339    *  @post result = x*x + y*y + z*z + w*w
340    *  @see Quat
341    */
342   template <typename DATA_TYPE>
343   DATA_TYPE lengthSquared( const Quat<DATA_TYPE>& q )
344   {
345      return dot( q, q );
346   }
347
348   /** quaternion "absolute" (also known as vector length or magnitude)
349    *  using this can be faster than using length for some operations...
350    *  @post returns the magnitude of the 4D vector.
351    *  @post result = sqrt( lengthSquared( q ) )
352    *  @see Quat
353    */
354   template <typename DATA_TYPE>
355   DATA_TYPE length( const Quat<DATA_TYPE>& q )
356   {
357      return Math::sqrt( lengthSquared( q ) );
358   }
359
360   /** set self to the normalized quaternion of self.
361    *  @pre magnitude should be > 0, otherwise no calculation is done.
362    *  @post result' = normalize( result ), where normalize makes length( result ) == 1
363    *  @see Quat
364    */
365   template <typename DATA_TYPE>
366   Quat<DATA_TYPE>& normalize( Quat<DATA_TYPE>& result )
367   {
368      DATA_TYPE l = length( result );
369
370      // return if no magnitude (already as normalized as possible)
371      if (l < (DATA_TYPE)0.0001)
372         return result;
373
374      DATA_TYPE l_inv = ((DATA_TYPE)1.0) / l;
375      result[Xelt] *= l_inv;
376      result[Yelt] *= l_inv;
377      result[Zelt] *= l_inv;
378      result[Welt] *= l_inv;
379
380      return result;
381   }
382
383   /**
384    * Determines if the given quaternion is normalized within the given tolerance. The
385    * quaternion is normalized if its lengthSquared is 1.
386    *
387    * @param q1      the quaternion to test
388    * @param eps     the epsilon tolerance
389    *
390    * @return  true if the quaternion is normalized, false otherwise
391    */
392   template< typename DATA_TYPE >
393   bool isNormalized( const Quat<DATA_TYPE>& q1, const DATA_TYPE eps = 0.0001f )
394   {
395      return Math::isEqual( lengthSquared( q1 ), DATA_TYPE(1), eps );
396   }
397
398   /** quaternion complex conjugate.
399    *  @post set result to the complex conjugate of result.
400    *  @post q* = [s,-v]
401    *  @post result'[x,y,z,w] == result[-x,-y,-z,w]
402    *  @see Quat
403    */
404   template <typename DATA_TYPE>
405   Quat<DATA_TYPE>& conj( Quat<DATA_TYPE>& result )
406   {
407      result[Xelt] = -result[Xelt];
408      result[Yelt] = -result[Yelt];
409      result[Zelt] = -result[Zelt];
410      return result;
411   }
412
413   /** quaternion multiplicative inverse.
414    *  @post self becomes the multiplicative inverse of self
415    *  @post 1/q = q* / N(q)
416    *  @see Quat
417    */
418   template <typename DATA_TYPE>
419   Quat<DATA_TYPE>& invert( Quat<DATA_TYPE>& result )
420   {
421      // from game programming gems p198
422      // do result = conj( q ) / norm( q )
423      conj( result );
424
425      // return if norm() is near 0 (divide by 0 would result in NaN)
426      DATA_TYPE l = lengthSquared( result );
427      if (l < (DATA_TYPE)0.0001)
428         return result;
429
430      DATA_TYPE l_inv = ((DATA_TYPE)1.0) / l;
431      result[Xelt] *= l_inv;
432      result[Yelt] *= l_inv;
433      result[Zelt] *= l_inv;
434      result[Welt] *= l_inv;
435      return result;
436   }
437
438   /** complex exponentiation.
439    *  @pre safe to pass self as argument
440    *  @post sets self to the exponentiation of quat
441    *  @see Quat
442    */
443   template <typename DATA_TYPE>
444   Quat<DATA_TYPE>& exp( Quat<DATA_TYPE>& result )
445   {
446      DATA_TYPE len1, len2;
447
448      len1 = Math::sqrt( result[Xelt] * result[Xelt] +
449                         result[Yelt] * result[Yelt] +
450                         result[Zelt] * result[Zelt] );
451      if (len1 > (DATA_TYPE)0.0)
452         len2 = Math::sin( len1 ) / len1;
453      else
454         len2 = (DATA_TYPE)1.0;
455
456      result[Xelt] = result[Xelt] * len2;
457      result[Yelt] = result[Yelt] * len2;
458      result[Zelt] = result[Zelt] * len2;
459      result[Welt] = Math::cos( len1 );
460
461      return result;
462   }
463
464   /** complex logarithm
465    *  @post sets self to the log of quat
466    *  @see Quat
467    */
468   template <typename DATA_TYPE>
469   Quat<DATA_TYPE>& log( Quat<DATA_TYPE>& result )
470   {
471      DATA_TYPE length;
472
473      length = Math::sqrt( result[Xelt] * result[Xelt] +
474                           result[Yelt] * result[Yelt] +
475                           result[Zelt] * result[Zelt] );
476
477      // avoid divide by 0
478      if (Math::isEqual( result[Welt], (DATA_TYPE)0.0, (DATA_TYPE)0.00001 ) == false)
479         length = Math::aTan( length / result[Welt] );
480      else
481         length = Math::PI_OVER_2;
482
483      result[Welt] = (DATA_TYPE)0.0;
484      result[Xelt] = result[Xelt] * length;
485      result[Yelt] = result[Yelt] * length;
486      result[Zelt] = result[Zelt] * length;
487      return result;
488   }
489   
490   /** WARNING: not implemented (do not use) */
491   template <typename DATA_TYPE>
492   void squad( Quat<DATA_TYPE>& result, DATA_TYPE t, const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2, const Quat<DATA_TYPE>& a, const Quat<DATA_TYPE>& b )
493   {
494      gmtlASSERT( false );
495   }
496
497   /** WARNING: not implemented (do not use) */
498   template <typename DATA_TYPE>
499   void meanTangent( Quat<DATA_TYPE>& result, const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2, const Quat<DATA_TYPE>& q3 )
500   {
501       gmtlASSERT( false );
502   }
503
504
505   
506/** @} */
507   
508/** @ingroup Interp Quat
509 * @name Quaternion Interpolation
510 * @{
511 */
512
513
514   /** spherical linear interpolation between two rotation quaternions.
515    *  t is a value between 0 and 1 that interpolates between from and to.
516    * @pre no aliasing problems to worry about ("result" can be "from" or "to" param).
517    * @param adjustSign - If true, then slerp will operate by adjusting the sign of the slerp to take shortest path
518    *
519    * References:
520    * <ul>
521    * <li> From Adv Anim and Rendering Tech. Pg 364
522    * </ul>
523    * @see Quat
524    */
525   template <typename DATA_TYPE>
526   Quat<DATA_TYPE>& slerp( Quat<DATA_TYPE>& result, const DATA_TYPE t, const Quat<DATA_TYPE>& from, const Quat<DATA_TYPE>& to, bool adjustSign=true)
527   {
528      const Quat<DATA_TYPE>& p = from; // just an alias to match q
529
530      // calc cosine theta
531      DATA_TYPE cosom = dot( from, to );
532
533      // adjust signs (if necessary)
534      Quat<DATA_TYPE> q;
535      if (adjustSign && (cosom < (DATA_TYPE)0.0))
536      {
537         cosom = -cosom;
538         q[0] = -to[0];   // Reverse all signs
539         q[1] = -to[1];
540         q[2] = -to[2];
541         q[3] = -to[3];
542      }
543      else
544      {
545         q = to;
546      }
547
548      // Calculate coefficients
549      DATA_TYPE sclp, sclq;
550      if (((DATA_TYPE)1.0 - cosom) > (DATA_TYPE)0.0001) // 0.0001 -> some epsillon
551      {
552         // Standard case (slerp)
553         DATA_TYPE omega, sinom;
554         omega = gmtl::Math::aCos( cosom ); // extract theta from dot product's cos theta
555         sinom = gmtl::Math::sin( omega );
556         sclp  = gmtl::Math::sin( ((DATA_TYPE)1.0 - t) * omega ) / sinom;
557         sclq  = gmtl::Math::sin( t * omega ) / sinom;
558      }
559      else
560      {
561         // Very close, do linear interp (because it's faster)
562         sclp = (DATA_TYPE)1.0 - t;
563         sclq = t;
564      }
565
566      result[Xelt] = sclp * p[Xelt] + sclq * q[Xelt];
567      result[Yelt] = sclp * p[Yelt] + sclq * q[Yelt];
568      result[Zelt] = sclp * p[Zelt] + sclq * q[Zelt];
569      result[Welt] = sclp * p[Welt] + sclq * q[Welt];
570      return result;
571   }
572
573   /** linear interpolation between two quaternions.
574    *  t is a value between 0 and 1 that interpolates between from and to.
575    * @pre no aliasing problems to worry about ("result" can be "from" or "to" param).
576    * References:
577    * <ul>
578    * <li> From Adv Anim and Rendering Tech. Pg 364
579    * </ul>
580    *  @see Quat
581    */
582   template <typename DATA_TYPE>
583   Quat<DATA_TYPE>& lerp( Quat<DATA_TYPE>& result, const DATA_TYPE t, const Quat<DATA_TYPE>& from, const Quat<DATA_TYPE>& to)
584   {
585      // just an alias to match q
586      const Quat<DATA_TYPE>& p = from;
587
588      // calc cosine theta
589      DATA_TYPE cosom = dot( from, to );
590
591      // adjust signs (if necessary)
592      Quat<DATA_TYPE> q;
593      if (cosom < (DATA_TYPE)0.0)
594      {
595         q[0] = -to[0];   // Reverse all signs
596         q[1] = -to[1];
597         q[2] = -to[2];
598         q[3] = -to[3];
599      }
600      else
601      {
602         q = to;
603      }
604
605      // do linear interp
606      DATA_TYPE sclp, sclq;
607      sclp = (DATA_TYPE)1.0 - t;
608      sclq = t;
609
610      result[Xelt] = sclp * p[Xelt] + sclq * q[Xelt];
611      result[Yelt] = sclp * p[Yelt] + sclq * q[Yelt];
612      result[Zelt] = sclp * p[Zelt] + sclq * q[Zelt];
613      result[Welt] = sclp * p[Welt] + sclq * q[Welt];
614      return result;
615   }
616
617/** @} */
618
619/** @ingroup Compare Quat
620 *  @name Quat Comparisons
621 * @{
622 */
623
624   /** Compare two quaternions for equality.
625    * @see isEqual( Quat, Quat )
626    */
627   template <typename DATA_TYPE>
628   inline bool operator==( const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2 )
629   {
630      return bool( q1[0] == q2[0] &&
631                   q1[1] == q2[1] &&
632                   q1[2] == q2[2] &&
633                   q1[3] == q2[3]  );
634   }
635
636   /** Compare two quaternions for not-equality.
637    * @see isEqual( Quat, Quat )
638    */
639   template <typename DATA_TYPE>
640   inline bool operator!=( const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2 )
641   {
642      return !operator==( q1, q2 );
643   }
644
645   /** Compare two quaternions for equality with tolerance.
646    */
647   template <typename DATA_TYPE>
648   bool isEqual( const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2, DATA_TYPE tol = 0.0 )
649   {
650      return bool( Math::isEqual( q1[0], q2[0], tol ) &&
651                   Math::isEqual( q1[1], q2[1], tol ) &&
652                   Math::isEqual( q1[2], q2[2], tol ) &&
653                   Math::isEqual( q1[3], q2[3], tol )    );
654   }
655
656   /** Compare two quaternions for geometric equivelence (with tolerance).
657    * there exist 2 quats for every possible rotation: the original,
658    * and its negative.  the negative of a rotation quaternion is geometrically
659    * equivelent to the original.
660    */
661   template <typename DATA_TYPE>
662   bool isEquiv( const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2, DATA_TYPE tol = 0.0 )
663   {
664      return bool( isEqual( q1, q2, tol ) || isEqual( q1, -q2, tol ) );
665   }
666   
667   /** @} */
668}
669
670#endif
Note: See TracBrowser for help on using the repository browser.