ViewVC Help
View File | Revision Log | Show Annotations | View Changeset | Root Listing
root/OpenMD/trunk/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 1360 by cli2, Mon Sep 7 16:31:51 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 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{
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();
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 <                rotMat3(0, 0) = w2 + x2 - y2 - z2;
291 <                rotMat3(0, 1) = 2.0 * ( x() * y() + w() * z() );
292 <                rotMat3(0, 2) = 2.0 * ( x() * z() - w() * y() );
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 <                rotMat3(1, 0) = 2.0 * ( x() * y() - w() * z() );
301 <                rotMat3(1, 1) = w2 - x2 + y2 - z2;
302 <                rotMat3(1, 2) = 2.0 * ( y() * z() + w() * x() );
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 <                rotMat3(2, 0) = 2.0 * ( x() * z() + w() * y() );
314 <                rotMat3(2, 1) = 2.0 * ( y() * z() - w() * x() );
315 <                rotMat3(2, 2) = w2 - x2 -y2 +z2;
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 <                return rotMat3;
332 <            }
331 >    Real ComputeTwist(const Quaternion& q) {
332 >      return  (Real)2.0 * atan2(q.z(), q.w());
333 >    }
334  
335 <    };//end Quaternion
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 <    /**
358 <     * Returns the vaule of scalar multiplication of this quaterion q (q * s).
359 <     * @return  the vaule of scalar multiplication of this vector
360 <     * @param q the source quaternion
361 <     * @param s the scalar value
362 <     */
363 <    template<typename Real, unsigned int Dim>                
364 <    Quaternion<Real> operator * ( const Quaternion<Real>& q, Real s) {      
295 <        Quaternion<Real> result(q);
296 <        result.mul(s);
297 <        return result;          
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      }
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 –    }    
366  
367 <    /**
368 <     * Returns the multiplication of two quaternion
369 <     * @return the multiplication of two quaternion
370 <     * @param q1 the first quaternion
371 <     * @param q2 the second quaternion
372 <     */
373 <    template<typename Real>
374 <    inline Quaternion<Real> operator *(const Quaternion<Real>& q1, const Quaternion<Real>& q2) {
375 <        Quaternion<Real> result(q1);
376 <        result *= q2;
377 <        return result;
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 division of two quaternion
539 <     * @param q1 divisor
329 <     * @param q2 dividen
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 <    template<typename Real>
550 <    inline Quaternion<Real> operator /( Quaternion<Real>& q1,  Quaternion<Real>& q2) {
551 <        return q1 * q2.inverse();
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 value of the division of a scalar by a quaternion
577 <     * @return the value of the division of a scalar by a quaternion
578 <     * @param s scalar
579 <     * @param q quaternion
342 <     * @note for a quaternion q, 1/q = q.inverse()
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>
582 <    Quaternion<Real> operator /(const Real& s,  Quaternion<Real>& q) {
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 <        Quaternion<Real> x;
602 <        x = q.inverse();
603 <        x *= s;
604 <        return x;
605 <    }
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 <    }
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<double> Quat4d;
646 >  typedef Quaternion<RealType> Quat4d;
647   }
648   #endif //MATH_QUATERNION_HPP

Diff Legend

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