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 963 by tim, Wed May 17 21:51:42 2006 UTC vs.
Revision 1360 by cli2, Mon Sep 7 16:31:51 2009 UTC

# 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  
# Line 64 | Line 66 | namespace oopse{
66     */
67    template<typename Real>
68    class Quaternion : public Vector<Real, 4> {
69 +
70    public:
71      Quaternion() : Vector<Real, 4>() {}
72  
# Line 78 | Line 81 | namespace oopse{
81      /** Constructs and initializes a Quaternion from a  Vector<Real,4> */    
82      Quaternion(const Vector<Real,4>& v)
83        : Vector<Real, 4>(v){
84 <      }
84 >    }
85  
86      /** copy assignment */
87      Quaternion& operator =(const Vector<Real, 4>& v){
88        if (this == & v)
89          return *this;
90 <
90 >      
91        Vector<Real, 4>::operator=(v);
92 <                
92 >      
93        return *this;
94      }
95 <
95 >    
96      /**
97       * Returns the value of the first element of this quaternion.
98       * @return the value of the first element of this quaternion
# Line 242 | Line 245 | namespace oopse{
245       * Returns the conjugate quaternion of this quaternion
246       * @return the conjugate quaternion of this quaternion
247       */
248 <    Quaternion<Real> conjugate() {
248 >    Quaternion<Real> conjugate() const {
249        return Quaternion<Real>(w(), -x(), -y(), -z());            
250 +    }
251 +
252 +
253 +    /**
254 +       return rotation angle from -PI to PI
255 +    */
256 +    inline Real get_rotation_angle() const{
257 +      if( w < (Real)0.0 )
258 +        return 2.0*atan2(-sqrt( x()*x() + y()*y() + z()*z() ), -w() );
259 +      else
260 +        return 2.0*atan2( sqrt( x()*x() + y()*y() + z()*z() ),  w() );
261 +    }
262 +
263 +    /**
264 +       create a unit quaternion from axis angle representation
265 +    */
266 +    Quaternion<Real> fromAxisAngle(const Vector3<Real>& axis,
267 +                                   const Real& angle){
268 +      Vector3<Real> v(axis);
269 +      v.normalize();
270 +      Real half_angle = angle*0.5;
271 +      Real sin_a = sin(half_angle);
272 +      *this = Quaternion<Real>(cos(half_angle),
273 +                               v.x()*sin_a,
274 +                               v.y()*sin_a,
275 +                               v.z()*sin_a);
276 +    }
277 +    
278 +    /**
279 +       convert a quaternion to axis angle representation,
280 +       preserve the axis direction and angle from -PI to +PI
281 +    */
282 +    void toAxisAngle(Vector3<Real>& axis, Real& angle)const {
283 +      Real vl = sqrt( x()*x() + y()*y() + z()*z() );
284 +      if( vl > tiny ) {
285 +        Real ivl = 1.0/vl;
286 +        axis.x() = x() * ivl;
287 +        axis.y() = y() * ivl;
288 +        axis.z() = z() * ivl;
289 +
290 +        if( w() < 0 )
291 +          angle = 2.0*atan2(-vl, -w()); //-PI,0
292 +        else
293 +          angle = 2.0*atan2( vl,  w()); //0,PI
294 +      } else {
295 +        axis = Vector3<Real>(0.0,0.0,0.0);
296 +        angle = 0.0;
297 +      }
298 +    }
299 +
300 +    /**
301 +       shortest arc quaternion rotate one vector to another by shortest path.
302 +       create rotation from -> to, for any length vectors.
303 +    */
304 +    Quaternion<Real> fromShortestArc(const Vector3d& from,
305 +                                     const Vector3d& to ) {
306 +      
307 +      Vector3d c( cross(from,to) );
308 +      *this = Quaternion<Real>(dot(from,to),
309 +                               c.x(),
310 +                               c.y(),
311 +                               c.z());
312 +
313 +      this->normalize();    // if "from" or "to" not unit, normalize quat
314 +      w += 1.0f;            // reducing angle to halfangle
315 +      if( w <= 1e-6 ) {     // angle close to PI
316 +        if( ( from.z()*from.z() ) > ( from.x()*from.x() ) ) {
317 +          this->data_[0] =  w;    
318 +          this->data_[1] =  0.0;       //cross(from , Vector3d(1,0,0))
319 +          this->data_[2] =  from.z();
320 +          this->data_[3] = -from.y();
321 +        } else {
322 +          this->data_[0] =  w;
323 +          this->data_[1] =  from.y();  //cross(from, Vector3d(0,0,1))
324 +          this->data_[2] = -from.x();
325 +          this->data_[3] =  0.0;
326 +        }
327 +      }
328 +      this->normalize();
329 +    }
330 +
331 +    Real ComputeTwist(const Quaternion& q) {
332 +      return  (Real)2.0 * atan2(q.z(), q.w());
333 +    }
334 +
335 +    void RemoveTwist(Quaternion& q) {
336 +      Real t = ComputeTwist(q);
337 +      Quaternion rt = fromAxisAngle(V3Z, t);
338 +      
339 +      q *= rt.inverse();
340 +    }
341 +
342 +    void getTwistSwingAxisAngle(Real& twistAngle, Real& swingAngle,
343 +                                Vector3<Real>& swingAxis) {
344 +      
345 +      twistAngle = (Real)2.0 * atan2(z(), w());
346 +      Quaternion rt, rs;
347 +      rt.fromAxisAngle(V3Z, twistAngle);
348 +      rs = *this * rt.inverse();
349 +      
350 +      Real vl = sqrt( rs.x()*rs.x() + rs.y()*rs.y() + rs.z()*rs.z() );
351 +      if( vl > tiny ) {
352 +        Real ivl = 1.0 / vl;
353 +        swingAxis.x() = rs.x() * ivl;
354 +        swingAxis.y() = rs.y() * ivl;
355 +        swingAxis.z() = rs.z() * ivl;
356 +
357 +        if( rs.w() < 0.0 )
358 +          swingAngle = 2.0*atan2(-vl, -rs.w()); //-PI,0
359 +        else
360 +          swingAngle = 2.0*atan2( vl,  rs.w()); //0,PI
361 +      } else {
362 +        swingAxis = Vector3<Real>(1.0,0.0,0.0);
363 +        swingAngle = 0.0;
364 +      }          
365 +    }
366 +
367 +
368 +    Vector3<Real> rotate(const Vector3<Real>& v) {
369 +
370 +      Quaternion<Real> q(v.x() * w() + v.z() * y() - v.y() * z(),
371 +                         v.y() * w() + v.x() * z() - v.z() * x(),
372 +                         v.z() * w() + v.y() * x() - v.x() * y(),
373 +                         v.x() * x() + v.y() * y() + v.z() * z());
374 +
375 +      return Vector3<Real>(w()*q.x() + x()*q.w() + y()*q.z() - z()*q.y(),
376 +                           w()*q.y() + y()*q.w() + z()*q.x() - x()*q.z(),
377 +                           w()*q.z() + z()*q.w() + x()*q.y() - y()*q.x())*
378 +        ( 1.0/this->lengthSquare() );      
379 +    }  
380 +
381 +    Quaternion<Real>& align (const Vector3<Real>& V1,
382 +                             const Vector3<Real>& V2) {
383 +
384 +      // If V1 and V2 are not parallel, the axis of rotation is the unit-length
385 +      // vector U = Cross(V1,V2)/Length(Cross(V1,V2)).  The angle of rotation,
386 +      // A, is the angle between V1 and V2.  The quaternion for the rotation is
387 +      // q = cos(A/2) + sin(A/2)*(ux*i+uy*j+uz*k) where U = (ux,uy,uz).
388 +      //
389 +      // (1) Rather than extract A = acos(Dot(V1,V2)), multiply by 1/2, then
390 +      //     compute sin(A/2) and cos(A/2), we reduce the computational costs
391 +      //     by computing the bisector B = (V1+V2)/Length(V1+V2), so cos(A/2) =
392 +      //     Dot(V1,B).
393 +      //
394 +      // (2) The rotation axis is U = Cross(V1,B)/Length(Cross(V1,B)), but
395 +      //     Length(Cross(V1,B)) = Length(V1)*Length(B)*sin(A/2) = sin(A/2), in
396 +      //     which case sin(A/2)*(ux*i+uy*j+uz*k) = (cx*i+cy*j+cz*k) where
397 +      //     C = Cross(V1,B).
398 +      //
399 +      // If V1 = V2, then B = V1, cos(A/2) = 1, and U = (0,0,0).  If V1 = -V2,
400 +      // then B = 0.  This can happen even if V1 is approximately -V2 using
401 +      // floating point arithmetic, since Vector3::Normalize checks for
402 +      // closeness to zero and returns the zero vector accordingly.  The test
403 +      // for exactly zero is usually not recommend for floating point
404 +      // arithmetic, but the implementation of Vector3::Normalize guarantees
405 +      // the comparison is robust.  In this case, the A = pi and any axis
406 +      // perpendicular to V1 may be used as the rotation axis.
407 +
408 +      Vector3<Real> Bisector = V1 + V2;
409 +      Bisector.normalize();
410 +
411 +      Real CosHalfAngle = dot(V1,Bisector);
412 +
413 +      this->data_[0] = CosHalfAngle;
414 +
415 +      if (CosHalfAngle != (Real)0.0) {
416 +        Vector3<Real> Cross = cross(V1, Bisector);
417 +        this->data_[1] = Cross.x();
418 +        this->data_[2] = Cross.y();
419 +        this->data_[3] = Cross.z();
420 +      } else {
421 +        Real InvLength;
422 +        if (fabs(V1[0]) >= fabs(V1[1])) {
423 +          // V1.x or V1.z is the largest magnitude component
424 +          InvLength = (Real)1.0/sqrt(V1[0]*V1[0] + V1[2]*V1[2]);
425 +
426 +          this->data_[1] = -V1[2]*InvLength;
427 +          this->data_[2] = (Real)0.0;
428 +          this->data_[3] = +V1[0]*InvLength;
429 +        } else {
430 +          // V1.y or V1.z is the largest magnitude component
431 +          InvLength = (Real)1.0 / sqrt(V1[1]*V1[1] + V1[2]*V1[2]);
432 +          
433 +          this->data_[1] = (Real)0.0;
434 +          this->data_[2] = +V1[2]*InvLength;
435 +          this->data_[3] = -V1[1]*InvLength;
436 +        }
437 +      }
438 +      return *this;
439      }
440  
441 +    void toTwistSwing ( Real& tw, Real& sx, Real& sy ) {
442 +      
443 +      // First test if the swing is in the singularity:
444 +
445 +      if ( ISZERO(z(),tiny) && ISZERO(w(),tiny) ) { sx=sy=M_PI; tw=0; return; }
446 +
447 +      // Decompose into twist-swing by solving the equation:
448 +      //
449 +      //                       Qtwist(t*2) * Qswing(s*2) = q
450 +      //
451 +      // note: (x,y) is the normalized swing axis (x*x+y*y=1)
452 +      //
453 +      //          ( Ct 0 0 St ) * ( Cs xSs ySs 0 ) = ( qw qx qy qz )
454 +      //  ( CtCs  xSsCt-yStSs  xStSs+ySsCt  StCs ) = ( qw qx qy qz )      (1)
455 +      // From (1): CtCs / StCs = qw/qz => Ct/St = qw/qz => tan(t) = qz/qw (2)
456 +      //
457 +      // The swing rotation/2 s comes from:
458 +      //
459 +      // From (1): (CtCs)^2 + (StCs)^2 = qw^2 + qz^2 =>  
460 +      //                                       Cs = sqrt ( qw^2 + qz^2 ) (3)
461 +      //
462 +      // From (1): (xSsCt-yStSs)^2 + (xStSs+ySsCt)^2 = qx^2 + qy^2 =>
463 +      //                                       Ss = sqrt ( qx^2 + qy^2 ) (4)
464 +      // From (1):  |SsCt -StSs| |x| = |qx|
465 +      //            |StSs +SsCt| |y|   |qy|                              (5)
466 +
467 +      Real qw, qx, qy, qz;
468 +      
469 +      if ( w()<0 ) {
470 +        qw=-w();
471 +        qx=-x();
472 +        qy=-y();
473 +        qz=-z();
474 +      } else {
475 +        qw=w();
476 +        qx=x();
477 +        qy=y();
478 +        qz=z();
479 +      }
480 +      
481 +      Real t = atan2 ( qz, qw ); // from (2)
482 +      Real s = atan2( sqrt(qx*qx+qy*qy), sqrt(qz*qz+qw*qw) ); // from (3)
483 +                                                              // and (4)
484 +
485 +      Real x=0.0, y=0.0, sins=sin(s);
486 +
487 +      if ( !ISZERO(sins,tiny) ) {
488 +        Real sint = sin(t);
489 +        Real cost = cos(t);
490 +        
491 +        // by solving the linear system in (5):
492 +        y = (-qx*sint + qy*cost)/sins;
493 +        x = ( qx*cost + qy*sint)/sins;
494 +      }
495 +
496 +      tw = (Real)2.0*t;
497 +      sx = (Real)2.0*x*s;
498 +      sy = (Real)2.0*y*s;
499 +    }
500 +
501 +    void toSwingTwist(Real& sx, Real& sy, Real& tw ) {
502 +
503 +      // Decompose q into swing-twist using a similar development as
504 +      // in function toTwistSwing
505 +
506 +      if ( ISZERO(z(),tiny) && ISZERO(w(),tiny) ) { sx=sy=M_PI; tw=0; }
507 +      
508 +      Real qw, qx, qy, qz;
509 +      if ( w() < 0 ){
510 +        qw=-w();
511 +        qx=-x();
512 +        qy=-y();
513 +        qz=-z();
514 +      } else {
515 +        qw=w();
516 +        qx=x();
517 +        qy=y();
518 +        qz=z();
519 +      }
520 +
521 +      // Get the twist t:
522 +      Real t = 2.0 * atan2(qz,qw);
523 +      
524 +      Real bet = atan2( sqrt(qx*qx+qy*qy), sqrt(qz*qz+qw*qw) );
525 +      Real gam = t/2.0;
526 +      Real sinc = ISZERO(bet,tiny)? 1.0 : sin(bet)/bet;
527 +      Real singam = sin(gam);
528 +      Real cosgam = cos(gam);
529 +
530 +      sx = Real( (2.0/sinc) * (cosgam*qx - singam*qy) );
531 +      sy = Real( (2.0/sinc) * (singam*qx + cosgam*qy) );
532 +      tw = Real( t );
533 +    }
534 +      
535 +    
536 +    
537      /**
538       * Returns the corresponding rotation matrix (3x3)
539       * @return a 3x3 rotation matrix
540       */
541      SquareMatrix<Real, 3> toRotationMatrix3() {
542        SquareMatrix<Real, 3> rotMat3;
543 <
543 >      
544        Real w2;
545        Real x2;
546        Real y2;

Diff Legend

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