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 1390 by gezelter, Wed Nov 25 20:02:06 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. Redistributions of source code must retain the above copyright
10 + *    notice, this list of conditions and the following disclaimer.
11 + *
12 + * 2. Redistributions in binary form must reproduce the above copyright
13 + *    notice, this list of conditions and the following disclaimer in the
14 + *    documentation and/or other materials provided with the
15 + *    distribution.
16 + *
17 + * This software is provided "AS IS," without a warranty of any
18 + * kind. All express or implied conditions, representations and
19 + * warranties, including any implied warranty of merchantability,
20 + * fitness for a particular purpose or non-infringement, are hereby
21 + * excluded.  The University of Notre Dame and its licensors shall not
22 + * be liable for any damages suffered by licensee as a result of
23 + * using, modifying or distributing the software or its
24 + * derivatives. In no event will the University of Notre Dame or its
25 + * licensors be liable for any lost revenue, profit or data, or for
26 + * direct, indirect, special, consequential, incidental or punitive
27 + * damages, however caused and regardless of the theory of liability,
28 + * arising out of the use of or inability to use software, even if the
29 + * University of Notre Dame has been advised of the possibility of
30 + * such damages.
31 + *
32 + * SUPPORT OPEN SCIENCE!  If you use OpenMD or its source code in your
33 + * research, please cite the appropriate papers when you publish your
34 + * work.  Good starting points are:
35 + *                                                                      
36 + * [1]  Meineke, et al., J. Comp. Chem. 26, 252-271 (2005).            
37 + * [2]  Fennell & Gezelter, J. Chem. Phys. 124, 234104 (2006).          
38 + * [3]  Sun, Lin & Gezelter, J. Chem. Phys. 128, 24107 (2008).          
39 + * [4]  Vardeman & Gezelter, in progress (2009).                        
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 < namespace oopse{
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 OpenMD{
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 +      return *this;
277 +    }
278 +    
279 +    /**
280 +       convert a quaternion to axis angle representation,
281 +       preserve the axis direction and angle from -PI to +PI
282 +    */
283 +    void toAxisAngle(Vector3<Real>& axis, Real& angle)const {
284 +      Real vl = sqrt( x()*x() + y()*y() + z()*z() );
285 +      if( vl > tiny ) {
286 +        Real ivl = 1.0/vl;
287 +        axis.x() = x() * ivl;
288 +        axis.y() = y() * ivl;
289 +        axis.z() = z() * ivl;
290 +
291 +        if( w() < 0 )
292 +          angle = 2.0*atan2(-vl, -w()); //-PI,0
293 +        else
294 +          angle = 2.0*atan2( vl,  w()); //0,PI
295 +      } else {
296 +        axis = Vector3<Real>(0.0,0.0,0.0);
297 +        angle = 0.0;
298 +      }
299 +    }
300 +
301 +    /**
302 +       shortest arc quaternion rotate one vector to another by shortest path.
303 +       create rotation from -> to, for any length vectors.
304 +    */
305 +    Quaternion<Real> fromShortestArc(const Vector3d& from,
306 +                                     const Vector3d& to ) {
307 +      
308 +      Vector3d c( cross(from,to) );
309 +      *this = Quaternion<Real>(dot(from,to),
310 +                               c.x(),
311 +                               c.y(),
312 +                               c.z());
313 +
314 +      this->normalize();    // if "from" or "to" not unit, normalize quat
315 +      w += 1.0f;            // reducing angle to halfangle
316 +      if( w <= 1e-6 ) {     // angle close to PI
317 +        if( ( from.z()*from.z() ) > ( from.x()*from.x() ) ) {
318 +          this->data_[0] =  w;    
319 +          this->data_[1] =  0.0;       //cross(from , Vector3d(1,0,0))
320 +          this->data_[2] =  from.z();
321 +          this->data_[3] = -from.y();
322 +        } else {
323 +          this->data_[0] =  w;
324 +          this->data_[1] =  from.y();  //cross(from, Vector3d(0,0,1))
325 +          this->data_[2] = -from.x();
326 +          this->data_[3] =  0.0;
327 +        }
328 +      }
329 +      this->normalize();
330 +    }
331 +
332 +    Real ComputeTwist(const Quaternion& q) {
333 +      return  (Real)2.0 * atan2(q.z(), q.w());
334 +    }
335 +
336 +    void RemoveTwist(Quaternion& q) {
337 +      Real t = ComputeTwist(q);
338 +      Quaternion rt = fromAxisAngle(V3Z, t);
339 +      
340 +      q *= rt.inverse();
341 +    }
342 +
343 +    void getTwistSwingAxisAngle(Real& twistAngle, Real& swingAngle,
344 +                                Vector3<Real>& swingAxis) {
345 +      
346 +      twistAngle = (Real)2.0 * atan2(z(), w());
347 +      Quaternion rt, rs;
348 +      rt.fromAxisAngle(V3Z, twistAngle);
349 +      rs = *this * rt.inverse();
350 +      
351 +      Real vl = sqrt( rs.x()*rs.x() + rs.y()*rs.y() + rs.z()*rs.z() );
352 +      if( vl > tiny ) {
353 +        Real ivl = 1.0 / vl;
354 +        swingAxis.x() = rs.x() * ivl;
355 +        swingAxis.y() = rs.y() * ivl;
356 +        swingAxis.z() = rs.z() * ivl;
357 +
358 +        if( rs.w() < 0.0 )
359 +          swingAngle = 2.0*atan2(-vl, -rs.w()); //-PI,0
360 +        else
361 +          swingAngle = 2.0*atan2( vl,  rs.w()); //0,PI
362 +      } else {
363 +        swingAxis = Vector3<Real>(1.0,0.0,0.0);
364 +        swingAngle = 0.0;
365 +      }          
366 +    }
367 +
368 +
369 +    Vector3<Real> rotate(const Vector3<Real>& v) {
370 +
371 +      Quaternion<Real> q(v.x() * w() + v.z() * y() - v.y() * z(),
372 +                         v.y() * w() + v.x() * z() - v.z() * x(),
373 +                         v.z() * w() + v.y() * x() - v.x() * y(),
374 +                         v.x() * x() + v.y() * y() + v.z() * z());
375 +
376 +      return Vector3<Real>(w()*q.x() + x()*q.w() + y()*q.z() - z()*q.y(),
377 +                           w()*q.y() + y()*q.w() + z()*q.x() - x()*q.z(),
378 +                           w()*q.z() + z()*q.w() + x()*q.y() - y()*q.x())*
379 +        ( 1.0/this->lengthSquare() );      
380 +    }  
381 +
382 +    Quaternion<Real>& align (const Vector3<Real>& V1,
383 +                             const Vector3<Real>& V2) {
384 +
385 +      // If V1 and V2 are not parallel, the axis of rotation is the unit-length
386 +      // vector U = Cross(V1,V2)/Length(Cross(V1,V2)).  The angle of rotation,
387 +      // A, is the angle between V1 and V2.  The quaternion for the rotation is
388 +      // q = cos(A/2) + sin(A/2)*(ux*i+uy*j+uz*k) where U = (ux,uy,uz).
389 +      //
390 +      // (1) Rather than extract A = acos(Dot(V1,V2)), multiply by 1/2, then
391 +      //     compute sin(A/2) and cos(A/2), we reduce the computational costs
392 +      //     by computing the bisector B = (V1+V2)/Length(V1+V2), so cos(A/2) =
393 +      //     Dot(V1,B).
394 +      //
395 +      // (2) The rotation axis is U = Cross(V1,B)/Length(Cross(V1,B)), but
396 +      //     Length(Cross(V1,B)) = Length(V1)*Length(B)*sin(A/2) = sin(A/2), in
397 +      //     which case sin(A/2)*(ux*i+uy*j+uz*k) = (cx*i+cy*j+cz*k) where
398 +      //     C = Cross(V1,B).
399 +      //
400 +      // If V1 = V2, then B = V1, cos(A/2) = 1, and U = (0,0,0).  If V1 = -V2,
401 +      // then B = 0.  This can happen even if V1 is approximately -V2 using
402 +      // floating point arithmetic, since Vector3::Normalize checks for
403 +      // closeness to zero and returns the zero vector accordingly.  The test
404 +      // for exactly zero is usually not recommend for floating point
405 +      // arithmetic, but the implementation of Vector3::Normalize guarantees
406 +      // the comparison is robust.  In this case, the A = pi and any axis
407 +      // perpendicular to V1 may be used as the rotation axis.
408 +
409 +      Vector3<Real> Bisector = V1 + V2;
410 +      Bisector.normalize();
411 +
412 +      Real CosHalfAngle = dot(V1,Bisector);
413 +
414 +      this->data_[0] = CosHalfAngle;
415 +
416 +      if (CosHalfAngle != (Real)0.0) {
417 +        Vector3<Real> Cross = cross(V1, Bisector);
418 +        this->data_[1] = Cross.x();
419 +        this->data_[2] = Cross.y();
420 +        this->data_[3] = Cross.z();
421 +      } else {
422 +        Real InvLength;
423 +        if (fabs(V1[0]) >= fabs(V1[1])) {
424 +          // V1.x or V1.z is the largest magnitude component
425 +          InvLength = (Real)1.0/sqrt(V1[0]*V1[0] + V1[2]*V1[2]);
426 +
427 +          this->data_[1] = -V1[2]*InvLength;
428 +          this->data_[2] = (Real)0.0;
429 +          this->data_[3] = +V1[0]*InvLength;
430 +        } else {
431 +          // V1.y or V1.z is the largest magnitude component
432 +          InvLength = (Real)1.0 / sqrt(V1[1]*V1[1] + V1[2]*V1[2]);
433 +          
434 +          this->data_[1] = (Real)0.0;
435 +          this->data_[2] = +V1[2]*InvLength;
436 +          this->data_[3] = -V1[1]*InvLength;
437 +        }
438 +      }
439 +      return *this;
440 +    }
441 +
442 +    void toTwistSwing ( Real& tw, Real& sx, Real& sy ) {
443 +      
444 +      // First test if the swing is in the singularity:
445 +
446 +      if ( ISZERO(z(),tiny) && ISZERO(w(),tiny) ) { sx=sy=M_PI; tw=0; return; }
447 +
448 +      // Decompose into twist-swing by solving the equation:
449 +      //
450 +      //                       Qtwist(t*2) * Qswing(s*2) = q
451 +      //
452 +      // note: (x,y) is the normalized swing axis (x*x+y*y=1)
453 +      //
454 +      //          ( Ct 0 0 St ) * ( Cs xSs ySs 0 ) = ( qw qx qy qz )
455 +      //  ( CtCs  xSsCt-yStSs  xStSs+ySsCt  StCs ) = ( qw qx qy qz )      (1)
456 +      // From (1): CtCs / StCs = qw/qz => Ct/St = qw/qz => tan(t) = qz/qw (2)
457 +      //
458 +      // The swing rotation/2 s comes from:
459 +      //
460 +      // From (1): (CtCs)^2 + (StCs)^2 = qw^2 + qz^2 =>  
461 +      //                                       Cs = sqrt ( qw^2 + qz^2 ) (3)
462 +      //
463 +      // From (1): (xSsCt-yStSs)^2 + (xStSs+ySsCt)^2 = qx^2 + qy^2 =>
464 +      //                                       Ss = sqrt ( qx^2 + qy^2 ) (4)
465 +      // From (1):  |SsCt -StSs| |x| = |qx|
466 +      //            |StSs +SsCt| |y|   |qy|                              (5)
467 +
468 +      Real qw, qx, qy, qz;
469 +      
470 +      if ( w()<0 ) {
471 +        qw=-w();
472 +        qx=-x();
473 +        qy=-y();
474 +        qz=-z();
475 +      } else {
476 +        qw=w();
477 +        qx=x();
478 +        qy=y();
479 +        qz=z();
480 +      }
481 +      
482 +      Real t = atan2 ( qz, qw ); // from (2)
483 +      Real s = atan2( sqrt(qx*qx+qy*qy), sqrt(qz*qz+qw*qw) ); // from (3)
484 +                                                              // and (4)
485 +
486 +      Real x=0.0, y=0.0, sins=sin(s);
487 +
488 +      if ( !ISZERO(sins,tiny) ) {
489 +        Real sint = sin(t);
490 +        Real cost = cos(t);
491 +        
492 +        // by solving the linear system in (5):
493 +        y = (-qx*sint + qy*cost)/sins;
494 +        x = ( qx*cost + qy*sint)/sins;
495 +      }
496 +
497 +      tw = (Real)2.0*t;
498 +      sx = (Real)2.0*x*s;
499 +      sy = (Real)2.0*y*s;
500 +    }
501 +
502 +    void toSwingTwist(Real& sx, Real& sy, Real& tw ) {
503 +
504 +      // Decompose q into swing-twist using a similar development as
505 +      // in function toTwistSwing
506 +
507 +      if ( ISZERO(z(),tiny) && ISZERO(w(),tiny) ) { sx=sy=M_PI; tw=0; }
508 +      
509 +      Real qw, qx, qy, qz;
510 +      if ( w() < 0 ){
511 +        qw=-w();
512 +        qx=-x();
513 +        qy=-y();
514 +        qz=-z();
515 +      } else {
516 +        qw=w();
517 +        qx=x();
518 +        qy=y();
519 +        qz=z();
520 +      }
521 +
522 +      // Get the twist t:
523 +      Real t = 2.0 * atan2(qz,qw);
524 +      
525 +      Real bet = atan2( sqrt(qx*qx+qy*qy), sqrt(qz*qz+qw*qw) );
526 +      Real gam = t/2.0;
527 +      Real sinc = ISZERO(bet,tiny)? 1.0 : sin(bet)/bet;
528 +      Real singam = sin(gam);
529 +      Real cosgam = cos(gam);
530 +
531 +      sx = Real( (2.0/sinc) * (cosgam*qx - singam*qy) );
532 +      sy = Real( (2.0/sinc) * (singam*qx + cosgam*qy) );
533 +      tw = Real( t );
534 +    }
535 +      
536 +    
537 +    
538 +    /**
539 +     * Returns the corresponding rotation matrix (3x3)
540 +     * @return a 3x3 rotation matrix
541 +     */
542 +    SquareMatrix<Real, 3> toRotationMatrix3() {
543 +      SquareMatrix<Real, 3> rotMat3;
544 +      
545 +      Real w2;
546 +      Real x2;
547 +      Real y2;
548 +      Real z2;
549 +
550 +      if (!this->isNormalized())
551 +        this->normalize();
552 +                
553 +      w2 = w() * w();
554 +      x2 = x() * x();
555 +      y2 = y() * y();
556 +      z2 = z() * z();
557 +
558 +      rotMat3(0, 0) = w2 + x2 - y2 - z2;
559 +      rotMat3(0, 1) = 2.0 * ( x() * y() + w() * z() );
560 +      rotMat3(0, 2) = 2.0 * ( x() * z() - w() * y() );
561 +
562 +      rotMat3(1, 0) = 2.0 * ( x() * y() - w() * z() );
563 +      rotMat3(1, 1) = w2 - x2 + y2 - z2;
564 +      rotMat3(1, 2) = 2.0 * ( y() * z() + w() * x() );
565 +
566 +      rotMat3(2, 0) = 2.0 * ( x() * z() + w() * y() );
567 +      rotMat3(2, 1) = 2.0 * ( y() * z() - w() * x() );
568 +      rotMat3(2, 2) = w2 - x2 -y2 +z2;
569 +
570 +      return rotMat3;
571 +    }
572 +
573 +  };//end Quaternion
574 +
575 +
576 +    /**
577 +     * Returns the vaule of scalar multiplication of this quaterion q (q * s).
578 +     * @return  the vaule of scalar multiplication of this vector
579 +     * @param q the source quaternion
580 +     * @param s the scalar value
581 +     */
582 +  template<typename Real, unsigned int Dim>                
583 +  Quaternion<Real> operator * ( const Quaternion<Real>& q, Real s) {      
584 +    Quaternion<Real> result(q);
585 +    result.mul(s);
586 +    return result;          
587 +  }
588 +    
589 +  /**
590 +   * Returns the vaule of scalar multiplication of this quaterion q (q * s).
591 +   * @return  the vaule of scalar multiplication of this vector
592 +   * @param s the scalar value
593 +   * @param q the source quaternion
594 +   */  
595 +  template<typename Real, unsigned int Dim>
596 +  Quaternion<Real> operator * ( const Real& s, const Quaternion<Real>& q ) {
597 +    Quaternion<Real> result(q);
598 +    result.mul(s);
599 +    return result;          
600 +  }    
601 +
602 +  /**
603 +   * Returns the multiplication of two quaternion
604 +   * @return the multiplication of two quaternion
605 +   * @param q1 the first quaternion
606 +   * @param q2 the second quaternion
607 +   */
608 +  template<typename Real>
609 +  inline Quaternion<Real> operator *(const Quaternion<Real>& q1, const Quaternion<Real>& q2) {
610 +    Quaternion<Real> result(q1);
611 +    result *= q2;
612 +    return result;
613 +  }
614 +
615 +  /**
616 +   * Returns the division of two quaternion
617 +   * @param q1 divisor
618 +   * @param q2 dividen
619 +   */
620 +
621 +  template<typename Real>
622 +  inline Quaternion<Real> operator /( Quaternion<Real>& q1,  Quaternion<Real>& q2) {
623 +    return q1 * q2.inverse();
624 +  }
625 +
626 +  /**
627 +   * Returns the value of the division of a scalar by a quaternion
628 +   * @return the value of the division of a scalar by a quaternion
629 +   * @param s scalar
630 +   * @param q quaternion
631 +   * @note for a quaternion q, 1/q = q.inverse()
632 +   */
633 +  template<typename Real>
634 +  Quaternion<Real> operator /(const Real& s,  Quaternion<Real>& q) {
635 +
636 +    Quaternion<Real> x;
637 +    x = q.inverse();
638 +    x *= s;
639 +    return x;
640 +  }
641 +    
642 +  template <class T>
643 +  inline bool operator==(const Quaternion<T>& lhs, const Quaternion<T>& rhs) {
644 +    return equal(lhs[0] ,rhs[0]) && equal(lhs[1] , rhs[1]) && equal(lhs[2], rhs[2]) && equal(lhs[3], rhs[3]);
645 +  }
646 +    
647 +  typedef Quaternion<RealType> Quat4d;
648   }
649   #endif //MATH_QUATERNION_HPP

Diff Legend

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