ViewVC Help
View File | Revision Log | Show Annotations | View Changeset | Root Listing
root/OpenMD/branches/development/src/math/Quaternion.hpp
(Generate patch)

Comparing trunk/src/math/Quaternion.hpp (file contents):
Revision 92 by tim, Sat Oct 16 01:31:28 2004 UTC vs.
Revision 1360 by cli2, Mon Sep 7 16:31:51 2009 UTC

# Line 1 | Line 1
1   /*
2 < * Copyright (C) 2000-2004  Object Oriented Parallel Simulation Engine (OOPSE) project
3 < *
4 < * Contact: oopse@oopse.org
5 < *
6 < * This program is free software; you can redistribute it and/or
7 < * modify it under the terms of the GNU Lesser General Public License
8 < * as published by the Free Software Foundation; either version 2.1
9 < * of the License, or (at your option) any later version.
10 < * All we ask is that proper credit is given for our work, which includes
11 < * - but is not limited to - adding the above copyright notice to the beginning
12 < * of your source code files, and to any copyright notice that you may distribute
13 < * with programs based on this work.
14 < *
15 < * This program is distributed in the hope that it will be useful,
16 < * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 < * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
18 < * GNU Lesser General Public License for more details.
19 < *
20 < * You should have received a copy of the GNU Lesser General Public License
21 < * along with this program; if not, write to the Free Software
22 < * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
2 > * Copyright (c) 2005 The University of Notre Dame. All Rights Reserved.
3   *
4 + * The University of Notre Dame grants you ("Licensee") a
5 + * non-exclusive, royalty free, license to use, modify and
6 + * redistribute this software in source and binary code form, provided
7 + * that the following conditions are met:
8 + *
9 + * 1. Acknowledgement of the program authors must be made in any
10 + *    publication of scientific results based in part on use of the
11 + *    program.  An acceptable form of acknowledgement is citation of
12 + *    the article in which the program was described (Matthew
13 + *    A. Meineke, Charles F. Vardeman II, Teng Lin, Christopher
14 + *    J. Fennell and J. Daniel Gezelter, "OOPSE: An Object-Oriented
15 + *    Parallel Simulation Engine for Molecular Dynamics,"
16 + *    J. Comput. Chem. 26, pp. 252-271 (2005))
17 + *
18 + * 2. Redistributions of source code must retain the above copyright
19 + *    notice, this list of conditions and the following disclaimer.
20 + *
21 + * 3. Redistributions in binary form must reproduce the above copyright
22 + *    notice, this list of conditions and the following disclaimer in the
23 + *    documentation and/or other materials provided with the
24 + *    distribution.
25 + *
26 + * This software is provided "AS IS," without a warranty of any
27 + * kind. All express or implied conditions, representations and
28 + * warranties, including any implied warranty of merchantability,
29 + * fitness for a particular purpose or non-infringement, are hereby
30 + * excluded.  The University of Notre Dame and its licensors shall not
31 + * be liable for any damages suffered by licensee as a result of
32 + * using, modifying or distributing the software or its
33 + * derivatives. In no event will the University of Notre Dame or its
34 + * licensors be liable for any lost revenue, profit or data, or for
35 + * direct, indirect, special, consequential, incidental or punitive
36 + * damages, however caused and regardless of the theory of liability,
37 + * arising out of the use of or inability to use software, even if the
38 + * University of Notre Dame has been advised of the possibility of
39 + * such damages.
40   */
41 <
41 >
42   /**
43   * @file Quaternion.hpp
44   * @author Teng Lin
# Line 33 | Line 49
49   #ifndef MATH_QUATERNION_HPP
50   #define MATH_QUATERNION_HPP
51  
52 + #include "math/Vector3.hpp"
53 + #include "math/SquareMatrix.hpp"
54 + #define ISZERO(a,eps) ( (a)>-(eps) && (a)<(eps) )
55 + const RealType tiny=1.0e-6;    
56 +
57   namespace oopse{
58  
59 +  /**
60 +   * @class Quaternion Quaternion.hpp "math/Quaternion.hpp"
61 +   * Quaternion is a sort of a higher-level complex number.
62 +   * It is defined as Q = w + x*i + y*j + z*k,
63 +   * where w, x, y, and z are numbers of type T (e.g. RealType), and
64 +   * i*i = -1; j*j = -1; k*k = -1;
65 +   * i*j = k; j*k = i; k*i = j;
66 +   */
67 +  template<typename Real>
68 +  class Quaternion : public Vector<Real, 4> {
69 +
70 +  public:
71 +    Quaternion() : Vector<Real, 4>() {}
72 +
73 +    /** Constructs and initializes a Quaternion from w, x, y, z values */    
74 +    Quaternion(Real w, Real x, Real y, Real z) {
75 +      this->data_[0] = w;
76 +      this->data_[1] = x;
77 +      this->data_[2] = y;
78 +      this->data_[3] = z;                
79 +    }
80 +            
81 +    /** Constructs and initializes a Quaternion from a  Vector<Real,4> */    
82 +    Quaternion(const Vector<Real,4>& v)
83 +      : Vector<Real, 4>(v){
84 +    }
85 +
86 +    /** copy assignment */
87 +    Quaternion& operator =(const Vector<Real, 4>& v){
88 +      if (this == & v)
89 +        return *this;
90 +      
91 +      Vector<Real, 4>::operator=(v);
92 +      
93 +      return *this;
94 +    }
95 +    
96      /**
97 <     * @class Quaternion Quaternion.hpp "math/Quaternion.hpp"
98 <     * @brief
97 >     * Returns the value of the first element of this quaternion.
98 >     * @return the value of the first element of this quaternion
99       */
100 <    template<typename Real>
101 <    class Quaternion : public Vector<Real, 4> {
100 >    Real w() const {
101 >      return this->data_[0];
102 >    }
103  
104 <    };
104 >    /**
105 >     * Returns the reference of the first element of this quaternion.
106 >     * @return the reference of the first element of this quaternion
107 >     */
108 >    Real& w() {
109 >      return this->data_[0];    
110 >    }
111 >
112 >    /**
113 >     * Returns the value of the first element of this quaternion.
114 >     * @return the value of the first element of this quaternion
115 >     */
116 >    Real x() const {
117 >      return this->data_[1];
118 >    }
119 >
120 >    /**
121 >     * Returns the reference of the second element of this quaternion.
122 >     * @return the reference of the second element of this quaternion
123 >     */
124 >    Real& x() {
125 >      return this->data_[1];    
126 >    }
127 >
128 >    /**
129 >     * Returns the value of the thirf element of this quaternion.
130 >     * @return the value of the third element of this quaternion
131 >     */
132 >    Real y() const {
133 >      return this->data_[2];
134 >    }
135 >
136 >    /**
137 >     * Returns the reference of the third element of this quaternion.
138 >     * @return the reference of the third element of this quaternion
139 >     */          
140 >    Real& y() {
141 >      return this->data_[2];    
142 >    }
143 >
144 >    /**
145 >     * Returns the value of the fourth element of this quaternion.
146 >     * @return the value of the fourth element of this quaternion
147 >     */
148 >    Real z() const {
149 >      return this->data_[3];
150 >    }
151 >    /**
152 >     * Returns the reference of the fourth element of this quaternion.
153 >     * @return the reference of the fourth element of this quaternion
154 >     */
155 >    Real& z() {
156 >      return this->data_[3];    
157 >    }
158 >
159 >    /**
160 >     * Tests if this quaternion is equal to other quaternion
161 >     * @return true if equal, otherwise return false
162 >     * @param q quaternion to be compared
163 >     */
164 >    inline bool operator ==(const Quaternion<Real>& q) {
165 >
166 >      for (unsigned int i = 0; i < 4; i ++) {
167 >        if (!equal(this->data_[i], q[i])) {
168 >          return false;
169 >        }
170 >      }
171 >                
172 >      return true;
173 >    }
174 >            
175 >    /**
176 >     * Returns the inverse of this quaternion
177 >     * @return inverse
178 >     * @note since quaternion is a complex number, the inverse of quaternion
179 >     * q = w + xi + yj+ zk is inv_q = (w -xi - yj - zk)/(|q|^2)
180 >     */
181 >    Quaternion<Real> inverse() {
182 >      Quaternion<Real> q;
183 >      Real d = this->lengthSquare();
184 >                
185 >      q.w() = w() / d;
186 >      q.x() = -x() / d;
187 >      q.y() = -y() / d;
188 >      q.z() = -z() / d;
189 >                
190 >      return q;
191 >    }
192 >
193 >    /**
194 >     * Sets the value to the multiplication of itself and another quaternion
195 >     * @param q the other quaternion
196 >     */
197 >    void mul(const Quaternion<Real>& q) {
198 >      Quaternion<Real> tmp(*this);
199 >
200 >      this->data_[0] = (tmp[0]*q[0]) -(tmp[1]*q[1]) - (tmp[2]*q[2]) - (tmp[3]*q[3]);
201 >      this->data_[1] = (tmp[0]*q[1]) + (tmp[1]*q[0]) + (tmp[2]*q[3]) - (tmp[3]*q[2]);
202 >      this->data_[2] = (tmp[0]*q[2]) + (tmp[2]*q[0]) + (tmp[3]*q[1]) - (tmp[1]*q[3]);
203 >      this->data_[3] = (tmp[0]*q[3]) + (tmp[3]*q[0]) + (tmp[1]*q[2]) - (tmp[2]*q[1]);                
204 >    }
205 >
206 >    void mul(const Real& s) {
207 >      this->data_[0] *= s;
208 >      this->data_[1] *= s;
209 >      this->data_[2] *= s;
210 >      this->data_[3] *= s;
211 >    }
212 >
213 >    /** Set the value of this quaternion to the division of itself by another quaternion */
214 >    void div(Quaternion<Real>& q) {
215 >      mul(q.inverse());
216 >    }
217 >
218 >    void div(const Real& s) {
219 >      this->data_[0] /= s;
220 >      this->data_[1] /= s;
221 >      this->data_[2] /= s;
222 >      this->data_[3] /= s;
223 >    }
224 >            
225 >    Quaternion<Real>& operator *=(const Quaternion<Real>& q) {
226 >      mul(q);
227 >      return *this;
228 >    }
229 >
230 >    Quaternion<Real>& operator *=(const Real& s) {
231 >      mul(s);
232 >      return *this;
233 >    }
234 >            
235 >    Quaternion<Real>& operator /=(Quaternion<Real>& q) {                
236 >      *this *= q.inverse();
237 >      return *this;
238 >    }
239 >
240 >    Quaternion<Real>& operator /=(const Real& s) {
241 >      div(s);
242 >      return *this;
243 >    }            
244 >    /**
245 >     * Returns the conjugate quaternion of this quaternion
246 >     * @return the conjugate quaternion of this quaternion
247 >     */
248 >    Quaternion<Real> conjugate() const {
249 >      return Quaternion<Real>(w(), -x(), -y(), -z());            
250 >    }
251 >
252 >
253 >    /**
254 >       return rotation angle from -PI to PI
255 >    */
256 >    inline Real get_rotation_angle() const{
257 >      if( w < (Real)0.0 )
258 >        return 2.0*atan2(-sqrt( x()*x() + y()*y() + z()*z() ), -w() );
259 >      else
260 >        return 2.0*atan2( sqrt( x()*x() + y()*y() + z()*z() ),  w() );
261 >    }
262 >
263 >    /**
264 >       create a unit quaternion from axis angle representation
265 >    */
266 >    Quaternion<Real> fromAxisAngle(const Vector3<Real>& axis,
267 >                                   const Real& angle){
268 >      Vector3<Real> v(axis);
269 >      v.normalize();
270 >      Real half_angle = angle*0.5;
271 >      Real sin_a = sin(half_angle);
272 >      *this = Quaternion<Real>(cos(half_angle),
273 >                               v.x()*sin_a,
274 >                               v.y()*sin_a,
275 >                               v.z()*sin_a);
276 >    }
277 >    
278 >    /**
279 >       convert a quaternion to axis angle representation,
280 >       preserve the axis direction and angle from -PI to +PI
281 >    */
282 >    void toAxisAngle(Vector3<Real>& axis, Real& angle)const {
283 >      Real vl = sqrt( x()*x() + y()*y() + z()*z() );
284 >      if( vl > tiny ) {
285 >        Real ivl = 1.0/vl;
286 >        axis.x() = x() * ivl;
287 >        axis.y() = y() * ivl;
288 >        axis.z() = z() * ivl;
289 >
290 >        if( w() < 0 )
291 >          angle = 2.0*atan2(-vl, -w()); //-PI,0
292 >        else
293 >          angle = 2.0*atan2( vl,  w()); //0,PI
294 >      } else {
295 >        axis = Vector3<Real>(0.0,0.0,0.0);
296 >        angle = 0.0;
297 >      }
298 >    }
299 >
300 >    /**
301 >       shortest arc quaternion rotate one vector to another by shortest path.
302 >       create rotation from -> to, for any length vectors.
303 >    */
304 >    Quaternion<Real> fromShortestArc(const Vector3d& from,
305 >                                     const Vector3d& to ) {
306 >      
307 >      Vector3d c( cross(from,to) );
308 >      *this = Quaternion<Real>(dot(from,to),
309 >                               c.x(),
310 >                               c.y(),
311 >                               c.z());
312 >
313 >      this->normalize();    // if "from" or "to" not unit, normalize quat
314 >      w += 1.0f;            // reducing angle to halfangle
315 >      if( w <= 1e-6 ) {     // angle close to PI
316 >        if( ( from.z()*from.z() ) > ( from.x()*from.x() ) ) {
317 >          this->data_[0] =  w;    
318 >          this->data_[1] =  0.0;       //cross(from , Vector3d(1,0,0))
319 >          this->data_[2] =  from.z();
320 >          this->data_[3] = -from.y();
321 >        } else {
322 >          this->data_[0] =  w;
323 >          this->data_[1] =  from.y();  //cross(from, Vector3d(0,0,1))
324 >          this->data_[2] = -from.x();
325 >          this->data_[3] =  0.0;
326 >        }
327 >      }
328 >      this->normalize();
329 >    }
330  
331 +    Real ComputeTwist(const Quaternion& q) {
332 +      return  (Real)2.0 * atan2(q.z(), q.w());
333 +    }
334 +
335 +    void RemoveTwist(Quaternion& q) {
336 +      Real t = ComputeTwist(q);
337 +      Quaternion rt = fromAxisAngle(V3Z, t);
338 +      
339 +      q *= rt.inverse();
340 +    }
341 +
342 +    void getTwistSwingAxisAngle(Real& twistAngle, Real& swingAngle,
343 +                                Vector3<Real>& swingAxis) {
344 +      
345 +      twistAngle = (Real)2.0 * atan2(z(), w());
346 +      Quaternion rt, rs;
347 +      rt.fromAxisAngle(V3Z, twistAngle);
348 +      rs = *this * rt.inverse();
349 +      
350 +      Real vl = sqrt( rs.x()*rs.x() + rs.y()*rs.y() + rs.z()*rs.z() );
351 +      if( vl > tiny ) {
352 +        Real ivl = 1.0 / vl;
353 +        swingAxis.x() = rs.x() * ivl;
354 +        swingAxis.y() = rs.y() * ivl;
355 +        swingAxis.z() = rs.z() * ivl;
356 +
357 +        if( rs.w() < 0.0 )
358 +          swingAngle = 2.0*atan2(-vl, -rs.w()); //-PI,0
359 +        else
360 +          swingAngle = 2.0*atan2( vl,  rs.w()); //0,PI
361 +      } else {
362 +        swingAxis = Vector3<Real>(1.0,0.0,0.0);
363 +        swingAngle = 0.0;
364 +      }          
365 +    }
366 +
367 +
368 +    Vector3<Real> rotate(const Vector3<Real>& v) {
369 +
370 +      Quaternion<Real> q(v.x() * w() + v.z() * y() - v.y() * z(),
371 +                         v.y() * w() + v.x() * z() - v.z() * x(),
372 +                         v.z() * w() + v.y() * x() - v.x() * y(),
373 +                         v.x() * x() + v.y() * y() + v.z() * z());
374 +
375 +      return Vector3<Real>(w()*q.x() + x()*q.w() + y()*q.z() - z()*q.y(),
376 +                           w()*q.y() + y()*q.w() + z()*q.x() - x()*q.z(),
377 +                           w()*q.z() + z()*q.w() + x()*q.y() - y()*q.x())*
378 +        ( 1.0/this->lengthSquare() );      
379 +    }  
380 +
381 +    Quaternion<Real>& align (const Vector3<Real>& V1,
382 +                             const Vector3<Real>& V2) {
383 +
384 +      // If V1 and V2 are not parallel, the axis of rotation is the unit-length
385 +      // vector U = Cross(V1,V2)/Length(Cross(V1,V2)).  The angle of rotation,
386 +      // A, is the angle between V1 and V2.  The quaternion for the rotation is
387 +      // q = cos(A/2) + sin(A/2)*(ux*i+uy*j+uz*k) where U = (ux,uy,uz).
388 +      //
389 +      // (1) Rather than extract A = acos(Dot(V1,V2)), multiply by 1/2, then
390 +      //     compute sin(A/2) and cos(A/2), we reduce the computational costs
391 +      //     by computing the bisector B = (V1+V2)/Length(V1+V2), so cos(A/2) =
392 +      //     Dot(V1,B).
393 +      //
394 +      // (2) The rotation axis is U = Cross(V1,B)/Length(Cross(V1,B)), but
395 +      //     Length(Cross(V1,B)) = Length(V1)*Length(B)*sin(A/2) = sin(A/2), in
396 +      //     which case sin(A/2)*(ux*i+uy*j+uz*k) = (cx*i+cy*j+cz*k) where
397 +      //     C = Cross(V1,B).
398 +      //
399 +      // If V1 = V2, then B = V1, cos(A/2) = 1, and U = (0,0,0).  If V1 = -V2,
400 +      // then B = 0.  This can happen even if V1 is approximately -V2 using
401 +      // floating point arithmetic, since Vector3::Normalize checks for
402 +      // closeness to zero and returns the zero vector accordingly.  The test
403 +      // for exactly zero is usually not recommend for floating point
404 +      // arithmetic, but the implementation of Vector3::Normalize guarantees
405 +      // the comparison is robust.  In this case, the A = pi and any axis
406 +      // perpendicular to V1 may be used as the rotation axis.
407 +
408 +      Vector3<Real> Bisector = V1 + V2;
409 +      Bisector.normalize();
410 +
411 +      Real CosHalfAngle = dot(V1,Bisector);
412 +
413 +      this->data_[0] = CosHalfAngle;
414 +
415 +      if (CosHalfAngle != (Real)0.0) {
416 +        Vector3<Real> Cross = cross(V1, Bisector);
417 +        this->data_[1] = Cross.x();
418 +        this->data_[2] = Cross.y();
419 +        this->data_[3] = Cross.z();
420 +      } else {
421 +        Real InvLength;
422 +        if (fabs(V1[0]) >= fabs(V1[1])) {
423 +          // V1.x or V1.z is the largest magnitude component
424 +          InvLength = (Real)1.0/sqrt(V1[0]*V1[0] + V1[2]*V1[2]);
425 +
426 +          this->data_[1] = -V1[2]*InvLength;
427 +          this->data_[2] = (Real)0.0;
428 +          this->data_[3] = +V1[0]*InvLength;
429 +        } else {
430 +          // V1.y or V1.z is the largest magnitude component
431 +          InvLength = (Real)1.0 / sqrt(V1[1]*V1[1] + V1[2]*V1[2]);
432 +          
433 +          this->data_[1] = (Real)0.0;
434 +          this->data_[2] = +V1[2]*InvLength;
435 +          this->data_[3] = -V1[1]*InvLength;
436 +        }
437 +      }
438 +      return *this;
439 +    }
440 +
441 +    void toTwistSwing ( Real& tw, Real& sx, Real& sy ) {
442 +      
443 +      // First test if the swing is in the singularity:
444 +
445 +      if ( ISZERO(z(),tiny) && ISZERO(w(),tiny) ) { sx=sy=M_PI; tw=0; return; }
446 +
447 +      // Decompose into twist-swing by solving the equation:
448 +      //
449 +      //                       Qtwist(t*2) * Qswing(s*2) = q
450 +      //
451 +      // note: (x,y) is the normalized swing axis (x*x+y*y=1)
452 +      //
453 +      //          ( Ct 0 0 St ) * ( Cs xSs ySs 0 ) = ( qw qx qy qz )
454 +      //  ( CtCs  xSsCt-yStSs  xStSs+ySsCt  StCs ) = ( qw qx qy qz )      (1)
455 +      // From (1): CtCs / StCs = qw/qz => Ct/St = qw/qz => tan(t) = qz/qw (2)
456 +      //
457 +      // The swing rotation/2 s comes from:
458 +      //
459 +      // From (1): (CtCs)^2 + (StCs)^2 = qw^2 + qz^2 =>  
460 +      //                                       Cs = sqrt ( qw^2 + qz^2 ) (3)
461 +      //
462 +      // From (1): (xSsCt-yStSs)^2 + (xStSs+ySsCt)^2 = qx^2 + qy^2 =>
463 +      //                                       Ss = sqrt ( qx^2 + qy^2 ) (4)
464 +      // From (1):  |SsCt -StSs| |x| = |qx|
465 +      //            |StSs +SsCt| |y|   |qy|                              (5)
466 +
467 +      Real qw, qx, qy, qz;
468 +      
469 +      if ( w()<0 ) {
470 +        qw=-w();
471 +        qx=-x();
472 +        qy=-y();
473 +        qz=-z();
474 +      } else {
475 +        qw=w();
476 +        qx=x();
477 +        qy=y();
478 +        qz=z();
479 +      }
480 +      
481 +      Real t = atan2 ( qz, qw ); // from (2)
482 +      Real s = atan2( sqrt(qx*qx+qy*qy), sqrt(qz*qz+qw*qw) ); // from (3)
483 +                                                              // and (4)
484 +
485 +      Real x=0.0, y=0.0, sins=sin(s);
486 +
487 +      if ( !ISZERO(sins,tiny) ) {
488 +        Real sint = sin(t);
489 +        Real cost = cos(t);
490 +        
491 +        // by solving the linear system in (5):
492 +        y = (-qx*sint + qy*cost)/sins;
493 +        x = ( qx*cost + qy*sint)/sins;
494 +      }
495 +
496 +      tw = (Real)2.0*t;
497 +      sx = (Real)2.0*x*s;
498 +      sy = (Real)2.0*y*s;
499 +    }
500 +
501 +    void toSwingTwist(Real& sx, Real& sy, Real& tw ) {
502 +
503 +      // Decompose q into swing-twist using a similar development as
504 +      // in function toTwistSwing
505 +
506 +      if ( ISZERO(z(),tiny) && ISZERO(w(),tiny) ) { sx=sy=M_PI; tw=0; }
507 +      
508 +      Real qw, qx, qy, qz;
509 +      if ( w() < 0 ){
510 +        qw=-w();
511 +        qx=-x();
512 +        qy=-y();
513 +        qz=-z();
514 +      } else {
515 +        qw=w();
516 +        qx=x();
517 +        qy=y();
518 +        qz=z();
519 +      }
520 +
521 +      // Get the twist t:
522 +      Real t = 2.0 * atan2(qz,qw);
523 +      
524 +      Real bet = atan2( sqrt(qx*qx+qy*qy), sqrt(qz*qz+qw*qw) );
525 +      Real gam = t/2.0;
526 +      Real sinc = ISZERO(bet,tiny)? 1.0 : sin(bet)/bet;
527 +      Real singam = sin(gam);
528 +      Real cosgam = cos(gam);
529 +
530 +      sx = Real( (2.0/sinc) * (cosgam*qx - singam*qy) );
531 +      sy = Real( (2.0/sinc) * (singam*qx + cosgam*qy) );
532 +      tw = Real( t );
533 +    }
534 +      
535 +    
536 +    
537 +    /**
538 +     * Returns the corresponding rotation matrix (3x3)
539 +     * @return a 3x3 rotation matrix
540 +     */
541 +    SquareMatrix<Real, 3> toRotationMatrix3() {
542 +      SquareMatrix<Real, 3> rotMat3;
543 +      
544 +      Real w2;
545 +      Real x2;
546 +      Real y2;
547 +      Real z2;
548 +
549 +      if (!this->isNormalized())
550 +        this->normalize();
551 +                
552 +      w2 = w() * w();
553 +      x2 = x() * x();
554 +      y2 = y() * y();
555 +      z2 = z() * z();
556 +
557 +      rotMat3(0, 0) = w2 + x2 - y2 - z2;
558 +      rotMat3(0, 1) = 2.0 * ( x() * y() + w() * z() );
559 +      rotMat3(0, 2) = 2.0 * ( x() * z() - w() * y() );
560 +
561 +      rotMat3(1, 0) = 2.0 * ( x() * y() - w() * z() );
562 +      rotMat3(1, 1) = w2 - x2 + y2 - z2;
563 +      rotMat3(1, 2) = 2.0 * ( y() * z() + w() * x() );
564 +
565 +      rotMat3(2, 0) = 2.0 * ( x() * z() + w() * y() );
566 +      rotMat3(2, 1) = 2.0 * ( y() * z() - w() * x() );
567 +      rotMat3(2, 2) = w2 - x2 -y2 +z2;
568 +
569 +      return rotMat3;
570 +    }
571 +
572 +  };//end Quaternion
573 +
574 +
575 +    /**
576 +     * Returns the vaule of scalar multiplication of this quaterion q (q * s).
577 +     * @return  the vaule of scalar multiplication of this vector
578 +     * @param q the source quaternion
579 +     * @param s the scalar value
580 +     */
581 +  template<typename Real, unsigned int Dim>                
582 +  Quaternion<Real> operator * ( const Quaternion<Real>& q, Real s) {      
583 +    Quaternion<Real> result(q);
584 +    result.mul(s);
585 +    return result;          
586 +  }
587 +    
588 +  /**
589 +   * Returns the vaule of scalar multiplication of this quaterion q (q * s).
590 +   * @return  the vaule of scalar multiplication of this vector
591 +   * @param s the scalar value
592 +   * @param q the source quaternion
593 +   */  
594 +  template<typename Real, unsigned int Dim>
595 +  Quaternion<Real> operator * ( const Real& s, const Quaternion<Real>& q ) {
596 +    Quaternion<Real> result(q);
597 +    result.mul(s);
598 +    return result;          
599 +  }    
600 +
601 +  /**
602 +   * Returns the multiplication of two quaternion
603 +   * @return the multiplication of two quaternion
604 +   * @param q1 the first quaternion
605 +   * @param q2 the second quaternion
606 +   */
607 +  template<typename Real>
608 +  inline Quaternion<Real> operator *(const Quaternion<Real>& q1, const Quaternion<Real>& q2) {
609 +    Quaternion<Real> result(q1);
610 +    result *= q2;
611 +    return result;
612 +  }
613 +
614 +  /**
615 +   * Returns the division of two quaternion
616 +   * @param q1 divisor
617 +   * @param q2 dividen
618 +   */
619 +
620 +  template<typename Real>
621 +  inline Quaternion<Real> operator /( Quaternion<Real>& q1,  Quaternion<Real>& q2) {
622 +    return q1 * q2.inverse();
623 +  }
624 +
625 +  /**
626 +   * Returns the value of the division of a scalar by a quaternion
627 +   * @return the value of the division of a scalar by a quaternion
628 +   * @param s scalar
629 +   * @param q quaternion
630 +   * @note for a quaternion q, 1/q = q.inverse()
631 +   */
632 +  template<typename Real>
633 +  Quaternion<Real> operator /(const Real& s,  Quaternion<Real>& q) {
634 +
635 +    Quaternion<Real> x;
636 +    x = q.inverse();
637 +    x *= s;
638 +    return x;
639 +  }
640 +    
641 +  template <class T>
642 +  inline bool operator==(const Quaternion<T>& lhs, const Quaternion<T>& rhs) {
643 +    return equal(lhs[0] ,rhs[0]) && equal(lhs[1] , rhs[1]) && equal(lhs[2], rhs[2]) && equal(lhs[3], rhs[3]);
644 +  }
645 +    
646 +  typedef Quaternion<RealType> Quat4d;
647   }
648   #endif //MATH_QUATERNION_HPP

Diff Legend

– Removed lines
+ Added lines
< Changed lines
> Changed lines