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, 9 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
#	Content
1	/*
2	* Copyright (c) 2005 The University of Notre Dame. All Rights Reserved.
3	*
4	* The University of Notre Dame grants you ("Licensee") a
5	* non-exclusive, royalty free, license to use, modify and
6	* redistribute this software in source and binary code form, provided
7	* that the following conditions are met:
8	*
9	* 1. 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	*/
41
42	/**
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	#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 + xi + yj + z*k,
63	* where w, x, y, and z are numbers of type T (e.g. RealType), and
64	* 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
70	public:
71	Quaternion() : Vector<Real, 4>() {}
72
73	/** Constructs and initializes a Quaternion from w, x, y, z values */
74	Quaternion(Real w, Real x, Real y, Real z) {
75	this->data_[0] = w;
76	this->data_[1] = x;
77	this->data_[2] = y;
78	this->data_[3] = z;
79	}
80
81	/** Constructs and initializes a Quaternion from a Vector<Real,4> */
82	Quaternion(const Vector<Real,4>& v)
83	: Vector<Real, 4>(v){
84	}
85
86	/** copy assignment */
87	Quaternion& operator =(const Vector<Real, 4>& v){
88	if (this == & v)
89	return *this;
90
91	Vector<Real, 4>::operator=(v);
92
93	return *this;
94	}
95
96	/**
97	* 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	/**
105	* Returns the reference of the first element of this quaternion.
106	* @return the reference of the first element of this quaternion
107	*/
108	Real& w() {
109	return this->data_[0];
110	}
111
112	/**
113	* Returns the value of the first element of this quaternion.
114	* @return the value of the first element of this quaternion
115	*/
116	Real x() const {
117	return this->data_[1];
118	}
119
120	/**
121	* Returns the reference of the second element of this quaternion.
122	* @return the reference of the second element of this quaternion
123	*/
124	Real& x() {
125	return this->data_[1];
126	}
127
128	/**
129	* Returns the value of the thirf element of this quaternion.
130	* @return the value of the third element of this quaternion
131	*/
132	Real y() const {
133	return this->data_[2];
134	}
135
136	/**
137	* Returns the reference of the third element of this quaternion.
138	* @return the reference of the third element of this quaternion
139	*/
140	Real& y() {
141	return this->data_[2];
142	}
143
144	/**
145	* Returns the value of the fourth element of this quaternion.
146	* @return the value of the fourth element of this quaternion
147	*/
148	Real z() const {
149	return this->data_[3];
150	}
151	/**
152	* Returns the reference of the fourth element of this quaternion.
153	* @return the reference of the fourth element of this quaternion
154	*/
155	Real& z() {
156	return this->data_[3];
157	}
158
159	/**
160	* Tests if this quaternion is equal to other quaternion
161	* @return true if equal, otherwise return false
162	* @param q quaternion to be compared
163	*/
164	inline bool operator ==(const Quaternion<Real>& q) {
165
166	for (unsigned int i = 0; i < 4; i ++) {
167	if (!equal(this->data_[i], q[i])) {
168	return false;
169	}
170	}
171
172	return true;
173	}
174
175	/**
176	* Returns the inverse of this quaternion
177	* @return inverse
178	* @note since quaternion is a complex number, the inverse of quaternion
179	* q = w + xi + yj+ zk is inv_q = (w -xi - yj - zk)/(\|q\|^2)
180	*/
181	Quaternion<Real> inverse() {
182	Quaternion<Real> q;
183	Real d = this->lengthSquare();
184
185	q.w() = w() / d;
186	q.x() = -x() / d;
187	q.y() = -y() / d;
188	q.z() = -z() / d;
189
190	return q;
191	}
192
193	/**
194	* Sets the value to the multiplication of itself and another quaternion
195	* @param q the other quaternion
196	*/
197	void mul(const Quaternion<Real>& q) {
198	Quaternion<Real> tmp(*this);
199
200	this->data_[0] = (tmp[0]q[0]) -(tmp[1]q[1]) - (tmp[2]q[2]) - (tmp[3]q[3]);
201	this->data_[1] = (tmp[0]q[1]) + (tmp[1]q[0]) + (tmp[2]q[3]) - (tmp[3]q[2]);
202	this->data_[2] = (tmp[0]q[2]) + (tmp[2]q[0]) + (tmp[3]q[1]) - (tmp[1]q[3]);
203	this->data_[3] = (tmp[0]q[3]) + (tmp[3]q[0]) + (tmp[1]q[2]) - (tmp[2]q[1]);
204	}
205
206	void mul(const Real& s) {
207	this->data_[0] *= s;
208	this->data_[1] *= s;
209	this->data_[2] *= s;
210	this->data_[3] *= s;
211	}
212
213	/** Set the value of this quaternion to the division of itself by another quaternion */
214	void div(Quaternion<Real>& q) {
215	mul(q.inverse());
216	}
217
218	void div(const Real& s) {
219	this->data_[0] /= s;
220	this->data_[1] /= s;
221	this->data_[2] /= s;
222	this->data_[3] /= s;
223	}
224
225	Quaternion<Real>& operator *=(const Quaternion<Real>& q) {
226	mul(q);
227	return *this;
228	}
229
230	Quaternion<Real>& operator *=(const Real& s) {
231	mul(s);
232	return *this;
233	}
234
235	Quaternion<Real>& operator /=(Quaternion<Real>& q) {
236	this = q.inverse();
237	return *this;
238	}
239
240	Quaternion<Real>& operator /=(const Real& s) {
241	div(s);
242	return *this;
243	}
244	/**
245	* Returns the conjugate quaternion of this quaternion
246	* @return the conjugate quaternion of this quaternion
247	*/
248	Quaternion<Real> conjugate() const {
249	return Quaternion<Real>(w(), -x(), -y(), -z());
250	}
251
252
253	/**
254	return rotation angle from -PI to PI
255	*/
256	inline Real get_rotation_angle() const{
257	if( w < (Real)0.0 )
258	return 2.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	* 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	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 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, 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	/**
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	}
645
646	typedef Quaternion<RealType> Quat4d;
647	}
648	#endif //MATH_QUATERNION_HPP