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 385 by tim, Tue Mar 1 20:10:14 2005 UTC vs.
Revision 1390 by gezelter, Wed Nov 25 20:02:06 2009 UTC

# Line 1 | Line 1
1 < /*
1 > /*
2   * Copyright (c) 2005 The University of Notre Dame. All Rights Reserved.
3   *
4   * The University of Notre Dame grants you ("Licensee") a
# Line 6 | Line 6
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
9 > * 1. Redistributions of source code must retain the above copyright
10   *    notice, this list of conditions and the following disclaimer.
11   *
12 < * 3. Redistributions in binary form must reproduce the above copyright
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.
# Line 37 | Line 28
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  
42   /**
# Line 49 | Line 49
49   #ifndef MATH_QUATERNION_HPP
50   #define MATH_QUATERNION_HPP
51  
52 < #include "math/Vector.hpp"
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{
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. double), 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> {
67 <        public:
68 <            Quaternion() : Vector<Real, 4>() {}
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 <            /** Constructs and initializes a Quaternion from w, x, y, z values */    
71 <            Quaternion(Real w, Real x, Real y, Real z) {
72 <                this->data_[0] = w;
73 <                this->data_[1] = x;
74 <                this->data_[2] = y;
75 <                this->data_[3] = z;                
76 <            }
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 <            }
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;
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 >     * Returns the value of the first element of this quaternion.
98 >     * @return the value of the first element of this quaternion
99 >     */
100 >    Real w() const {
101 >      return this->data_[0];
102 >    }
103  
104 <                Vector<Real, 4>::operator=(v);
105 <                
106 <                return *this;
107 <            }
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 w() const {
117 <                return this->data_[0];
118 <            }
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 first element of this quaternion.
122 <             * @return the reference of the first element of this quaternion
123 <             */
124 <            Real& w() {
125 <                return this->data_[0];    
126 <            }
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 first element of this quaternion.
130 <             * @return the value of the first element of this quaternion
131 <             */
132 <            Real x() const {
133 <                return this->data_[1];
134 <            }
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 second element of this quaternion.
138 <             * @return the reference of the second element of this quaternion
139 <             */
140 <            Real& x() {
141 <                return this->data_[1];    
142 <            }
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 thirf element of this quaternion.
146 <             * @return the value of the third element of this quaternion
147 <             */
148 <            Real y() const {
149 <                return this->data_[2];
150 <            }
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 <             * Returns the reference of the third element of this quaternion.
161 <             * @return the reference of the third element of this quaternion
162 <             */          
163 <            Real& y() {
164 <                return this->data_[2];    
139 <            }
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 <            /**
167 <             * Returns the value of the fourth element of this quaternion.
168 <             * @return the value of the fourth element of this quaternion
169 <             */
170 <            Real z() const {
146 <                return this->data_[3];
147 <            }
148 <            /**
149 <             * Returns the reference of the fourth element of this quaternion.
150 <             * @return the reference of the fourth element of this quaternion
151 <             */
152 <            Real& z() {
153 <                return this->data_[3];    
154 <            }
155 <
156 <            /**
157 <             * Tests if this quaternion is equal to other quaternion
158 <             * @return true if equal, otherwise return false
159 <             * @param q quaternion to be compared
160 <             */
161 <             inline bool operator ==(const Quaternion<Real>& q) {
162 <
163 <                for (unsigned int i = 0; i < 4; i ++) {
164 <                    if (!equal(this->data_[i], q[i])) {
165 <                        return false;
166 <                    }
167 <                }
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 <            }
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();
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;
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 <            }
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);
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 <            }
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 <            }
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 <            }
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 <            }
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 <            }
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 <            }
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 <            }
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() {
249 <                return Quaternion<Real>(w(), -x(), -y(), -z());            
250 <            }
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  
249 –            /**
250 –             * Returns the corresponding rotation matrix (3x3)
251 –             * @return a 3x3 rotation matrix
252 –             */
253 –            SquareMatrix<Real, 3> toRotationMatrix3() {
254 –                SquareMatrix<Real, 3> rotMat3;
252  
253 <                Real w2;
254 <                Real x2;
255 <                Real y2;
256 <                Real z2;
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 <                if (!this->isNormalized())
264 <                    this->normalize();
265 <                
266 <                w2 = w() * w();
267 <                x2 = x() * x();
268 <                y2 = y() * y();
269 <                z2 = z() * z();
270 <
271 <                rotMat3(0, 0) = w2 + x2 - y2 - z2;
272 <                rotMat3(0, 1) = 2.0 * ( x() * y() + w() * z() );
273 <                rotMat3(0, 2) = 2.0 * ( x() * z() - w() * y() );
274 <
275 <                rotMat3(1, 0) = 2.0 * ( x() * y() - w() * z() );
276 <                rotMat3(1, 1) = w2 - x2 + y2 - z2;
277 <                rotMat3(1, 2) = 2.0 * ( y() * z() + w() * x() );
278 <
279 <                rotMat3(2, 0) = 2.0 * ( x() * z() + w() * y() );
280 <                rotMat3(2, 1) = 2.0 * ( y() * z() - w() * x() );
281 <                rotMat3(2, 2) = w2 - x2 -y2 +z2;
282 <
283 <                return rotMat3;
284 <            }
285 <
286 <    };//end Quaternion
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 <     * Returns the vaule of scalar multiplication of this quaterion q (q * s).
303 <     * @return  the vaule of scalar multiplication of this vector
304 <     * @param q the source quaternion
305 <     * @param s the scalar value
306 <     */
307 <    template<typename Real, unsigned int Dim>                
308 <    Quaternion<Real> operator * ( const Quaternion<Real>& q, Real s) {      
309 <        Quaternion<Real> result(q);
310 <        result.mul(s);
311 <        return result;          
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      }
299 –    
300 –    /**
301 –     * Returns the vaule of scalar multiplication of this quaterion q (q * s).
302 –     * @return  the vaule of scalar multiplication of this vector
303 –     * @param s the scalar value
304 –     * @param q the source quaternion
305 –     */  
306 –    template<typename Real, unsigned int Dim>
307 –    Quaternion<Real> operator * ( const Real& s, const Quaternion<Real>& q ) {
308 –        Quaternion<Real> result(q);
309 –        result.mul(s);
310 –        return result;          
311 –    }    
331  
332 <    /**
333 <     * Returns the multiplication of two quaternion
315 <     * @return the multiplication of two quaternion
316 <     * @param q1 the first quaternion
317 <     * @param q2 the second quaternion
318 <     */
319 <    template<typename Real>
320 <    inline Quaternion<Real> operator *(const Quaternion<Real>& q1, const Quaternion<Real>& q2) {
321 <        Quaternion<Real> result(q1);
322 <        result *= q2;
323 <        return result;
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 division of two quaternion
540 <     * @param q1 divisor
329 <     * @param q2 dividen
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 <    template<typename Real>
551 <    inline Quaternion<Real> operator /( Quaternion<Real>& q1,  Quaternion<Real>& q2) {
552 <        return q1 * q2.inverse();
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 value of the division of a scalar by a quaternion
578 <     * @return the value of the division of a scalar by a quaternion
579 <     * @param s scalar
580 <     * @param q quaternion
342 <     * @note for a quaternion q, 1/q = q.inverse()
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>
583 <    Quaternion<Real> operator /(const Real& s,  Quaternion<Real>& q) {
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 <        Quaternion<Real> x;
603 <        x = q.inverse();
604 <        x *= s;
605 <        return x;
606 <    }
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 <    }
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<double> Quat4d;
647 >  typedef Quaternion<RealType> Quat4d;
648   }
649   #endif //MATH_QUATERNION_HPP

Diff Legend

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