src/math/Quaternion.hpp

/*
 * Copyright (c) 2005 The University of Notre Dame. All Rights Reserved.
 *
 * The University of Notre Dame grants you ("Licensee") a
 * non-exclusive, royalty free, license to use, modify and
 * redistribute this software in source and binary code form, provided
 * that the following conditions are met:
 *
 * 1. Acknowledgement of the program authors must be made in any
 *    publication of scientific results based in part on use of the
 *    program.  An acceptable form of acknowledgement is citation of
 *    the article in which the program was described (Matthew
 *    A. Meineke, Charles F. Vardeman II, Teng Lin, Christopher
 *    J. Fennell and J. Daniel Gezelter, "OOPSE: An Object-Oriented
 *    Parallel Simulation Engine for Molecular Dynamics,"
 *    J. Comput. Chem. 26, pp. 252-271 (2005))
 *
 * 2. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * 3. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the
 *    distribution.
 *
 * This software is provided "AS IS," without a warranty of any
 * kind. All express or implied conditions, representations and
 * warranties, including any implied warranty of merchantability,
 * fitness for a particular purpose or non-infringement, are hereby
 * excluded.  The University of Notre Dame and its licensors shall not
 * be liable for any damages suffered by licensee as a result of
 * using, modifying or distributing the software or its
 * derivatives. In no event will the University of Notre Dame or its
 * licensors be liable for any lost revenue, profit or data, or for
 * direct, indirect, special, consequential, incidental or punitive
 * damages, however caused and regardless of the theory of liability,
 * arising out of the use of or inability to use software, even if the
 * University of Notre Dame has been advised of the possibility of
 * such damages.
 */
 
/**
 * @file Quaternion.hpp
 * @author Teng Lin
 * @date 10/11/2004
 * @version 1.0
 */

#ifndef MATH_QUATERNION_HPP
#define MATH_QUATERNION_HPP

#include "math/Vector3.hpp"
#include "math/SquareMatrix.hpp"
#define ISZERO(a,eps) ( (a)>-(eps) && (a)<(eps) )
const RealType tiny=1.0e-6;     

namespace oopse{

  /**
   * @class Quaternion Quaternion.hpp "math/Quaternion.hpp"
   * Quaternion is a sort of a higher-level complex number.
   * It is defined as Q = w + x*i + y*j + z*k,
   * where w, x, y, and z are numbers of type T (e.g. RealType), and
   * i*i = -1; j*j = -1; k*k = -1;
   * i*j = k; j*k = i; k*i = j;
   */
  template<typename Real>
  class Quaternion : public Vector<Real, 4> {

  public:
    Quaternion() : Vector<Real, 4>() {}

    /** Constructs and initializes a Quaternion from w, x, y, z values */     
    Quaternion(Real w, Real x, Real y, Real z) {
      this->data_[0] = w;
      this->data_[1] = x;
      this->data_[2] = y;
      this->data_[3] = z;                
    }
            
    /** Constructs and initializes a Quaternion from a  Vector<Real,4> */     
    Quaternion(const Vector<Real,4>& v) 
      : Vector<Real, 4>(v){
    }

    /** copy assignment */
    Quaternion& operator =(const Vector<Real, 4>& v){
      if (this == & v)
        return *this;
      
      Vector<Real, 4>::operator=(v);
      
      return *this;
    }
    
    /**
     * Returns the value of the first element of this quaternion.
     * @return the value of the first element of this quaternion
     */
    Real w() const {
      return this->data_[0];
    }

    /**
     * Returns the reference of the first element of this quaternion.
     * @return the reference of the first element of this quaternion
     */
    Real& w() {
      return this->data_[0];    
    }

    /**
     * Returns the value of the first element of this quaternion.
     * @return the value of the first element of this quaternion
     */
    Real x() const {
      return this->data_[1];
    }

    /**
     * Returns the reference of the second element of this quaternion.
     * @return the reference of the second element of this quaternion
     */
    Real& x() {
      return this->data_[1];    
    }

    /**
     * Returns the value of the thirf element of this quaternion.
     * @return the value of the third element of this quaternion
     */
    Real y() const {
      return this->data_[2];
    }

    /**
     * Returns the reference of the third element of this quaternion.
     * @return the reference of the third element of this quaternion
     */           
    Real& y() {
      return this->data_[2];    
    }

    /**
     * Returns the value of the fourth element of this quaternion.
     * @return the value of the fourth element of this quaternion
     */
    Real z() const {
      return this->data_[3];
    }
    /**
     * Returns the reference of the fourth element of this quaternion.
     * @return the reference of the fourth element of this quaternion
     */
    Real& z() {
      return this->data_[3];    
    }

    /**
     * Tests if this quaternion is equal to other quaternion
     * @return true if equal, otherwise return false
     * @param q quaternion to be compared
     */
    inline bool operator ==(const Quaternion<Real>& q) {

      for (unsigned int i = 0; i < 4; i ++) {
        if (!equal(this->data_[i], q[i])) {
          return false;
        }
      }
                
      return true;
    }
            
    /**
     * Returns the inverse of this quaternion
     * @return inverse
     * @note since quaternion is a complex number, the inverse of quaternion
     * q = w + xi + yj+ zk is inv_q = (w -xi - yj - zk)/(|q|^2)
     */
    Quaternion<Real> inverse() {
      Quaternion<Real> q;
      Real d = this->lengthSquare();
                
      q.w() = w() / d;
      q.x() = -x() / d;
      q.y() = -y() / d;
      q.z() = -z() / d;
                
      return q;
    }

    /**
     * Sets the value to the multiplication of itself and another quaternion
     * @param q the other quaternion
     */
    void mul(const Quaternion<Real>& q) {
      Quaternion<Real> tmp(*this);

      this->data_[0] = (tmp[0]*q[0]) -(tmp[1]*q[1]) - (tmp[2]*q[2]) - (tmp[3]*q[3]);
      this->data_[1] = (tmp[0]*q[1]) + (tmp[1]*q[0]) + (tmp[2]*q[3]) - (tmp[3]*q[2]);
      this->data_[2] = (tmp[0]*q[2]) + (tmp[2]*q[0]) + (tmp[3]*q[1]) - (tmp[1]*q[3]);
      this->data_[3] = (tmp[0]*q[3]) + (tmp[3]*q[0]) + (tmp[1]*q[2]) - (tmp[2]*q[1]);                
    }

    void mul(const Real& s) {
      this->data_[0] *= s;
      this->data_[1] *= s;
      this->data_[2] *= s;
      this->data_[3] *= s;
    }

    /** Set the value of this quaternion to the division of itself by another quaternion */
    void div(Quaternion<Real>& q) {
      mul(q.inverse());
    }

    void div(const Real& s) {
      this->data_[0] /= s;
      this->data_[1] /= s;
      this->data_[2] /= s;
      this->data_[3] /= s;
    }
            
    Quaternion<Real>& operator *=(const Quaternion<Real>& q) {
      mul(q);
      return *this;
    }

    Quaternion<Real>& operator *=(const Real& s) {
      mul(s);
      return *this;
    }
            
    Quaternion<Real>& operator /=(Quaternion<Real>& q) {                
      *this *= q.inverse();
      return *this;
    }

    Quaternion<Real>& operator /=(const Real& s) {
      div(s);
      return *this;
    }            
    /**
     * Returns the conjugate quaternion of this quaternion
     * @return the conjugate quaternion of this quaternion
     */
    Quaternion<Real> conjugate() const {
      return Quaternion<Real>(w(), -x(), -y(), -z());            
    }


    /**
       return rotation angle from -PI to PI 
    */
    inline Real get_rotation_angle() const{
      if( w < (Real)0.0 )
        return 2.0*atan2(-sqrt( x()*x() + y()*y() + z()*z() ), -w() );
      else
        return 2.0*atan2( sqrt( x()*x() + y()*y() + z()*z() ),  w() );
    }

    /**
       create a unit quaternion from axis angle representation
    */
    Quaternion<Real> fromAxisAngle(const Vector3<Real>& axis, 
                                   const Real& angle){
      Vector3<Real> v(axis);
      v.normalize();
      Real half_angle = angle*0.5;
      Real sin_a = sin(half_angle);
      *this = Quaternion<Real>(cos(half_angle), 
                               v.x()*sin_a, 
                               v.y()*sin_a, 
                               v.z()*sin_a);
    }
    
    /**
       convert a quaternion to axis angle representation, 
       preserve the axis direction and angle from -PI to +PI
    */
    void toAxisAngle(Vector3<Real>& axis, Real& angle)const {
      Real vl = sqrt( x()*x() + y()*y() + z()*z() );
      if( vl > tiny ) {
        Real ivl = 1.0/vl;
        axis.x() = x() * ivl;
        axis.y() = y() * ivl;
        axis.z() = z() * ivl;

        if( w() < 0 )
          angle = 2.0*atan2(-vl, -w()); //-PI,0 
        else
          angle = 2.0*atan2( vl,  w()); //0,PI 
      } else {
        axis = Vector3<Real>(0.0,0.0,0.0);
        angle = 0.0;
      }
    }

    /**
       shortest arc quaternion rotate one vector to another by shortest path.
       create rotation from -> to, for any length vectors.
    */
    Quaternion<Real> fromShortestArc(const Vector3d& from, 
                                     const Vector3d& to ) {
      
      Vector3d c( cross(from,to) );
      *this = Quaternion<Real>(dot(from,to), 
                               c.x(), 
                               c.y(),
                               c.z());

      this->normalize();    // if "from" or "to" not unit, normalize quat
      w += 1.0f;            // reducing angle to halfangle
      if( w <= 1e-6 ) {     // angle close to PI
        if( ( from.z()*from.z() ) > ( from.x()*from.x() ) ) {
          this->data_[0] =  w;    
          this->data_[1] =  0.0;       //cross(from , Vector3d(1,0,0))
          this->data_[2] =  from.z();
          this->data_[3] = -from.y();
        } else {
          this->data_[0] =  w;
          this->data_[1] =  from.y();  //cross(from, Vector3d(0,0,1))
          this->data_[2] = -from.x();
          this->data_[3] =  0.0;
        }
      }
      this->normalize(); 
    }

    Real ComputeTwist(const Quaternion& q) {
      return  (Real)2.0 * atan2(q.z(), q.w());
    }

    void RemoveTwist(Quaternion& q) {
      Real t = ComputeTwist(q);
      Quaternion rt = fromAxisAngle(V3Z, t);
      
      q *= rt.inverse();
    }

    void getTwistSwingAxisAngle(Real& twistAngle, Real& swingAngle, 
                                Vector3<Real>& swingAxis) {
      
      twistAngle = (Real)2.0 * atan2(z(), w());
      Quaternion rt, rs;
      rt.fromAxisAngle(V3Z, twistAngle);
      rs = *this * rt.inverse();
      
      Real vl = sqrt( rs.x()*rs.x() + rs.y()*rs.y() + rs.z()*rs.z() );
      if( vl > tiny ) {
        Real ivl = 1.0 / vl;
        swingAxis.x() = rs.x() * ivl;
        swingAxis.y() = rs.y() * ivl;
        swingAxis.z() = rs.z() * ivl;

        if( rs.w() < 0.0 )
          swingAngle = 2.0*atan2(-vl, -rs.w()); //-PI,0 
        else
          swingAngle = 2.0*atan2( vl,  rs.w()); //0,PI 
      } else {
        swingAxis = Vector3<Real>(1.0,0.0,0.0);
        swingAngle = 0.0;
      }           
    }


    Vector3<Real> rotate(const Vector3<Real>& v) {

      Quaternion<Real> q(v.x() * w() + v.z() * y() - v.y() * z(),
                         v.y() * w() + v.x() * z() - v.z() * x(),
                         v.z() * w() + v.y() * x() - v.x() * y(),
                         v.x() * x() + v.y() * y() + v.z() * z());

      return Vector3<Real>(w()*q.x() + x()*q.w() + y()*q.z() - z()*q.y(),
                           w()*q.y() + y()*q.w() + z()*q.x() - x()*q.z(),
                           w()*q.z() + z()*q.w() + x()*q.y() - y()*q.x())*
        ( 1.0/this->lengthSquare() );      
    }   

    Quaternion<Real>& align (const Vector3<Real>& V1,
                             const Vector3<Real>& V2) {

      // If V1 and V2 are not parallel, the axis of rotation is the unit-length
      // vector U = Cross(V1,V2)/Length(Cross(V1,V2)).  The angle of rotation,
      // A, is the angle between V1 and V2.  The quaternion for the rotation is
      // q = cos(A/2) + sin(A/2)*(ux*i+uy*j+uz*k) where U = (ux,uy,uz).
      //
      // (1) Rather than extract A = acos(Dot(V1,V2)), multiply by 1/2, then
      //     compute sin(A/2) and cos(A/2), we reduce the computational costs
      //     by computing the bisector B = (V1+V2)/Length(V1+V2), so cos(A/2) =
      //     Dot(V1,B).
      //
      // (2) The rotation axis is U = Cross(V1,B)/Length(Cross(V1,B)), but
      //     Length(Cross(V1,B)) = Length(V1)*Length(B)*sin(A/2) = sin(A/2), in
      //     which case sin(A/2)*(ux*i+uy*j+uz*k) = (cx*i+cy*j+cz*k) where
      //     C = Cross(V1,B).
      //
      // If V1 = V2, then B = V1, cos(A/2) = 1, and U = (0,0,0).  If V1 = -V2,
      // then B = 0.  This can happen even if V1 is approximately -V2 using
      // floating point arithmetic, since Vector3::Normalize checks for
      // closeness to zero and returns the zero vector accordingly.  The test
      // for exactly zero is usually not recommend for floating point
      // arithmetic, but the implementation of Vector3::Normalize guarantees
      // the comparison is robust.  In this case, the A = pi and any axis
      // perpendicular to V1 may be used as the rotation axis.

      Vector3<Real> Bisector = V1 + V2;
      Bisector.normalize();

      Real CosHalfAngle = dot(V1,Bisector);

      this->data_[0] = CosHalfAngle;

      if (CosHalfAngle != (Real)0.0) {
        Vector3<Real> Cross = cross(V1, Bisector);
        this->data_[1] = Cross.x();
        this->data_[2] = Cross.y();
        this->data_[3] = Cross.z();
      } else {
        Real InvLength;
        if (fabs(V1[0]) >= fabs(V1[1])) {
          // V1.x or V1.z is the largest magnitude component
          InvLength = (Real)1.0/sqrt(V1[0]*V1[0] + V1[2]*V1[2]);

          this->data_[1] = -V1[2]*InvLength;
          this->data_[2] = (Real)0.0;
          this->data_[3] = +V1[0]*InvLength;
        } else {
          // V1.y or V1.z is the largest magnitude component
          InvLength = (Real)1.0 / sqrt(V1[1]*V1[1] + V1[2]*V1[2]);
          
          this->data_[1] = (Real)0.0;
          this->data_[2] = +V1[2]*InvLength;
          this->data_[3] = -V1[1]*InvLength;
        }
      }
      return *this;
    }

    void toTwistSwing ( Real& tw, Real& sx, Real& sy ) {
      
      // First test if the swing is in the singularity:

      if ( ISZERO(z(),tiny) && ISZERO(w(),tiny) ) { sx=sy=M_PI; tw=0; return; }

      // Decompose into twist-swing by solving the equation:
      //
      //                       Qtwist(t*2) * Qswing(s*2) = q
      //
      // note: (x,y) is the normalized swing axis (x*x+y*y=1)
      //
      //          ( Ct 0 0 St ) * ( Cs xSs ySs 0 ) = ( qw qx qy qz )
      //  ( CtCs  xSsCt-yStSs  xStSs+ySsCt  StCs ) = ( qw qx qy qz )      (1)
      // From (1): CtCs / StCs = qw/qz => Ct/St = qw/qz => tan(t) = qz/qw (2)
      //
      // The swing rotation/2 s comes from:
      //
      // From (1): (CtCs)^2 + (StCs)^2 = qw^2 + qz^2 =>  
      //                                       Cs = sqrt ( qw^2 + qz^2 ) (3)
      //
      // From (1): (xSsCt-yStSs)^2 + (xStSs+ySsCt)^2 = qx^2 + qy^2 => 
      //                                       Ss = sqrt ( qx^2 + qy^2 ) (4)
      // From (1):  |SsCt -StSs| |x| = |qx|
      //            |StSs +SsCt| |y|   |qy|                              (5)

      Real qw, qx, qy, qz;
      
      if ( w()<0 ) {
        qw=-w(); 
        qx=-x(); 
        qy=-y(); 
        qz=-z();
      } else {
        qw=w(); 
        qx=x(); 
        qy=y(); 
        qz=z();
      }
      
      Real t = atan2 ( qz, qw ); // from (2)
      Real s = atan2( sqrt(qx*qx+qy*qy), sqrt(qz*qz+qw*qw) ); // from (3)
                                                              // and (4)

      Real x=0.0, y=0.0, sins=sin(s);

      if ( !ISZERO(sins,tiny) ) {
        Real sint = sin(t);
        Real cost = cos(t);
        
        // by solving the linear system in (5):
        y = (-qx*sint + qy*cost)/sins;
        x = ( qx*cost + qy*sint)/sins;
      }

      tw = (Real)2.0*t;
      sx = (Real)2.0*x*s;
      sy = (Real)2.0*y*s;
    }

    void toSwingTwist(Real& sx, Real& sy, Real& tw ) {

      // Decompose q into swing-twist using a similar development as
      // in function toTwistSwing

      if ( ISZERO(z(),tiny) && ISZERO(w(),tiny) ) { sx=sy=M_PI; tw=0; }
      
      Real qw, qx, qy, qz;
      if ( w() < 0 ){
        qw=-w(); 
        qx=-x(); 
        qy=-y(); 
        qz=-z();
      } else {
        qw=w(); 
        qx=x(); 
        qy=y(); 
        qz=z(); 
      }

      // Get the twist t:
      Real t = 2.0 * atan2(qz,qw);
      
      Real bet = atan2( sqrt(qx*qx+qy*qy), sqrt(qz*qz+qw*qw) );
      Real gam = t/2.0;
      Real sinc = ISZERO(bet,tiny)? 1.0 : sin(bet)/bet;
      Real singam = sin(gam);
      Real cosgam = cos(gam);

      sx = Real( (2.0/sinc) * (cosgam*qx - singam*qy) );
      sy = Real( (2.0/sinc) * (singam*qx + cosgam*qy) );
      tw = Real( t );
    }
      
    
    /**
     * Returns the corresponding rotation matrix (3x3)
     * @return a 3x3 rotation matrix
     */
    SquareMatrix<Real, 3> toRotationMatrix3() {
      SquareMatrix<Real, 3> rotMat3;
      
      Real w2;
      Real x2;
      Real y2;
      Real z2;

      if (!this->isNormalized())
        this->normalize();
                
      w2 = w() * w();
      x2 = x() * x();
      y2 = y() * y();
      z2 = z() * z();

      rotMat3(0, 0) = w2 + x2 - y2 - z2;
      rotMat3(0, 1) = 2.0 * ( x() * y() + w() * z() );
      rotMat3(0, 2) = 2.0 * ( x() * z() - w() * y() );

      rotMat3(1, 0) = 2.0 * ( x() * y() - w() * z() );
      rotMat3(1, 1) = w2 - x2 + y2 - z2;
      rotMat3(1, 2) = 2.0 * ( y() * z() + w() * x() );

      rotMat3(2, 0) = 2.0 * ( x() * z() + w() * y() );
      rotMat3(2, 1) = 2.0 * ( y() * z() - w() * x() );
      rotMat3(2, 2) = w2 - x2 -y2 +z2;

      return rotMat3;
    }

  };//end Quaternion


    /**
     * Returns the vaule of scalar multiplication of this quaterion q (q * s). 
     * @return  the vaule of scalar multiplication of this vector
     * @param q the source quaternion
     * @param s the scalar value
     */
  template<typename Real, unsigned int Dim>                 
  Quaternion<Real> operator * ( const Quaternion<Real>& q, Real s) {       
    Quaternion<Real> result(q);
    result.mul(s);
    return result;           
  }
    
  /**
   * Returns the vaule of scalar multiplication of this quaterion q (q * s). 
   * @return  the vaule of scalar multiplication of this vector
   * @param s the scalar value
   * @param q the source quaternion
   */  
  template<typename Real, unsigned int Dim>
  Quaternion<Real> operator * ( const Real& s, const Quaternion<Real>& q ) {
    Quaternion<Real> result(q);
    result.mul(s);
    return result;           
  }    

  /**
   * Returns the multiplication of two quaternion
   * @return the multiplication of two quaternion
   * @param q1 the first quaternion
   * @param q2 the second quaternion
   */
  template<typename Real>
  inline Quaternion<Real> operator *(const Quaternion<Real>& q1, const Quaternion<Real>& q2) {
    Quaternion<Real> result(q1);
    result *= q2;
    return result;
  }

  /**
   * Returns the division of two quaternion
   * @param q1 divisor
   * @param q2 dividen
   */

  template<typename Real>
  inline Quaternion<Real> operator /( Quaternion<Real>& q1,  Quaternion<Real>& q2) {
    return q1 * q2.inverse();
  }

  /**
   * Returns the value of the division of a scalar by a quaternion
   * @return the value of the division of a scalar by a quaternion
   * @param s scalar
   * @param q quaternion
   * @note for a quaternion q, 1/q = q.inverse()
   */
  template<typename Real>
  Quaternion<Real> operator /(const Real& s,  Quaternion<Real>& q) {

    Quaternion<Real> x;
    x = q.inverse();
    x *= s;
    return x;
  }
    
  template <class T>
  inline bool operator==(const Quaternion<T>& lhs, const Quaternion<T>& rhs) {
    return equal(lhs[0] ,rhs[0]) && equal(lhs[1] , rhs[1]) && equal(lhs[2], rhs[2]) && equal(lhs[3], rhs[3]);
  }
    
  typedef Quaternion<RealType> Quat4d;
}
#endif //MATH_QUATERNION_HPP 
Revision:	3520
Committed:	Mon Sep 7 16:31:51 2009 UTC (14 years, 10 months ago) by cli2
File size:	20425 byte(s)
Log Message:	Added new restraint infrastructure Added MolecularRestraints Added ObjectRestraints Added RestraintStamp Updated thermodynamic integration to use ObjectRestraints Added Quaternion mathematics for twist swing decompositions Significantly updated RestWriter and RestReader to use dump-like files Added selections for x, y, and z coordinates of atoms Removed monolithic Restraints class Fixed a few bugs in gradients of Euler angles in DirectionalAtom and RigidBody Added some rotational capabilities to prinicpalAxisCalculator
#	User	Rev	Content
1	gezelter	2204	/*
2	gezelter	1930	* Copyright (c) 2005 The University of Notre Dame. All Rights Reserved.
3	tim	1585	*
4	gezelter	1930	* The University of Notre Dame grants you ("Licensee") a
5			* non-exclusive, royalty free, license to use, modify and
6			* redistribute this software in source and binary code form, provided
7			* that the following conditions are met:
8			*
9			* 1. Acknowledgement of the program authors must be made in any
10			* publication of scientific results based in part on use of the
11			* program. An acceptable form of acknowledgement is citation of
12			* the article in which the program was described (Matthew
13			* A. Meineke, Charles F. Vardeman II, Teng Lin, Christopher
14			* J. Fennell and J. Daniel Gezelter, "OOPSE: An Object-Oriented
15			* Parallel Simulation Engine for Molecular Dynamics,"
16			* J. Comput. Chem. 26, pp. 252-271 (2005))
17			*
18			* 2. Redistributions of source code must retain the above copyright
19			* notice, this list of conditions and the following disclaimer.
20			*
21			* 3. Redistributions in binary form must reproduce the above copyright
22			* notice, this list of conditions and the following disclaimer in the
23			* documentation and/or other materials provided with the
24			* distribution.
25			*
26			* This software is provided "AS IS," without a warranty of any
27			* kind. All express or implied conditions, representations and
28			* warranties, including any implied warranty of merchantability,
29			* fitness for a particular purpose or non-infringement, are hereby
30			* excluded. The University of Notre Dame and its licensors shall not
31			* be liable for any damages suffered by licensee as a result of
32			* using, modifying or distributing the software or its
33			* derivatives. In no event will the University of Notre Dame or its
34			* licensors be liable for any lost revenue, profit or data, or for
35			* direct, indirect, special, consequential, incidental or punitive
36			* damages, however caused and regardless of the theory of liability,
37			* arising out of the use of or inability to use software, even if the
38			* University of Notre Dame has been advised of the possibility of
39			* such damages.
40	tim	1585	*/
41	gezelter	1930
42	tim	1585	/**
43			* @file Quaternion.hpp
44			* @author Teng Lin
45			* @date 10/11/2004
46			* @version 1.0
47			*/
48
49			#ifndef MATH_QUATERNION_HPP
50			#define MATH_QUATERNION_HPP
51
52	cli2	3520	#include "math/Vector3.hpp"
53	tim	1603	#include "math/SquareMatrix.hpp"
54	cli2	3520	#define ISZERO(a,eps) ( (a)>-(eps) && (a)<(eps) )
55			const RealType tiny=1.0e-6;
56	tim	1586
57	tim	1585	namespace oopse{
58
59	gezelter	2204	/**
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 + xi + yj + z*k,
63	tim	2759	* where w, x, y, and z are numbers of type T (e.g. RealType), and
64	gezelter	2204	* ii = -1; jj = -1; k*k = -1;
65			* ij = k; jk = i; k*i = j;
66			*/
67			template<typename Real>
68			class Quaternion : public Vector<Real, 4> {
69	cli2	3520
70	gezelter	2204	public:
71			Quaternion() : Vector<Real, 4>() {}
72	tim	1585
73	gezelter	2204	/** 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	tim	1586
81	gezelter	2204	/** Constructs and initializes a Quaternion from a Vector<Real,4> */
82			Quaternion(const Vector<Real,4>& v)
83			: Vector<Real, 4>(v){
84	cli2	3520	}
85	tim	1585
86	gezelter	2204	/** copy assignment */
87			Quaternion& operator =(const Vector<Real, 4>& v){
88			if (this == & v)
89			return *this;
90	cli2	3520
91	gezelter	2204	Vector<Real, 4>::operator=(v);
92	cli2	3520
93	gezelter	2204	return *this;
94			}
95	cli2	3520
96	gezelter	2204	/**
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	tim	1586
104	gezelter	2204	/**
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	tim	1586
112	gezelter	2204	/**
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	tim	1586
120	gezelter	2204	/**
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	tim	1586
128	gezelter	2204	/**
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	tim	1586
136	gezelter	2204	/**
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	tim	1586
144	gezelter	2204	/**
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	tim	1586
159	gezelter	2204	/**
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	tim	1603
166	gezelter	2204	for (unsigned int i = 0; i < 4; i ++) {
167			if (!equal(this->data_[i], q[i])) {
168			return false;
169			}
170			}
171	tim	1603
172	gezelter	2204	return true;
173			}
174	tim	1603
175	gezelter	2204	/**
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	tim	1586
185	gezelter	2204	q.w() = w() / d;
186			q.x() = -x() / d;
187			q.y() = -y() / d;
188			q.z() = -z() / d;
189	tim	1586
190	gezelter	2204	return q;
191			}
192	tim	1586
193	gezelter	2204	/**
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	tim	1586
200	gezelter	2204	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	tim	1586
206	gezelter	2204	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	tim	1586
213	gezelter	2204	/** 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	tim	1603
218	gezelter	2204	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	tim	1586
225	gezelter	2204	Quaternion<Real>& operator *=(const Quaternion<Real>& q) {
226			mul(q);
227			return *this;
228			}
229	tim	1603
230	gezelter	2204	Quaternion<Real>& operator *=(const Real& s) {
231			mul(s);
232			return *this;
233			}
234	tim	1586
235	gezelter	2204	Quaternion<Real>& operator /=(Quaternion<Real>& q) {
236			this = q.inverse();
237			return *this;
238			}
239	tim	1603
240	gezelter	2204	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	cli2	3520	Quaternion<Real> conjugate() const {
249	gezelter	2204	return Quaternion<Real>(w(), -x(), -y(), -z());
250			}
251	tim	1586
252	cli2	3520
253	gezelter	2204	/**
254	cli2	3520	return rotation angle from -PI to PI
255			*/
256			inline Real get_rotation_angle() const{
257			if( w < (Real)0.0 )
258			return 2.0atan2(-sqrt( x()x() + y()y() + z()z() ), -w() );
259			else
260			return 2.0atan2( 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)(uxi+uyj+uzk) 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)(uxi+uyj+uzk) = (cxi+cyj+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(t2) Qswing(s*2) = q
450			//
451			// note: (x,y) is the normalized swing axis (xx+yy=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(qxqx+qyqy), sqrt(qzqz+qwqw) ); // 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 = (-qxsint + qycost)/sins;
493			x = ( qxcost + qysint)/sins;
494			}
495
496			tw = (Real)2.0*t;
497			sx = (Real)2.0xs;
498			sy = (Real)2.0ys;
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(qxqx+qyqy), sqrt(qzqz+qwqw) );
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) * (cosgamqx - singamqy) );
531			sy = Real( (2.0/sinc) * (singamqx + cosgamqy) );
532			tw = Real( t );
533			}
534
535
536
537			/**
538	gezelter	2204	* Returns the corresponding rotation matrix (3x3)
539			* @return a 3x3 rotation matrix
540			*/
541			SquareMatrix<Real, 3> toRotationMatrix3() {
542			SquareMatrix<Real, 3> rotMat3;
543	cli2	3520
544	gezelter	2204	Real w2;
545			Real x2;
546			Real y2;
547			Real z2;
548	tim	1586
549	gezelter	2204	if (!this->isNormalized())
550			this->normalize();
551	tim	1586
552	gezelter	2204	w2 = w() * w();
553			x2 = x() * x();
554			y2 = y() * y();
555			z2 = z() * z();
556	tim	1586
557	gezelter	2204	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	tim	1586
561	gezelter	2204	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	tim	1586
565	gezelter	2204	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	tim	1603
569	gezelter	2204	return rotMat3;
570			}
571	tim	1586
572	gezelter	2204	};//end Quaternion
573	tim	1586
574	tim	1603
575	tim	1586	/**
576	tim	1603	* 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	gezelter	2204	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	tim	1603
588	gezelter	2204	/**
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	tim	1603
601	gezelter	2204	/**
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	tim	1586
614	gezelter	2204	/**
615			* Returns the division of two quaternion
616			* @param q1 divisor
617			* @param q2 dividen
618			*/
619	tim	1586
620	gezelter	2204	template<typename Real>
621			inline Quaternion<Real> operator /( Quaternion<Real>& q1, Quaternion<Real>& q2) {
622			return q1 * q2.inverse();
623			}
624	tim	1586
625	gezelter	2204	/**
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	tim	1586
635	gezelter	2204	Quaternion<Real> x;
636			x = q.inverse();
637			x *= s;
638			return x;
639			}
640	tim	1603
641	gezelter	2204	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	tim	1603
646	tim	2759	typedef Quaternion<RealType> Quat4d;
647	tim	1585	}
648			#endif //MATH_QUATERNION_HPP