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. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * 2. 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.
 *
 * SUPPORT OPEN SCIENCE!  If you use OpenMD or its source code in your
 * research, please cite the appropriate papers when you publish your
 * work.  Good starting points are:
 *                                                                      
 * [1]  Meineke, et al., J. Comp. Chem. 26, 252-271 (2005).             
 * [2]  Fennell & Gezelter, J. Chem. Phys. 124, 234104 (2006).          
 * [3]  Sun, Lin & Gezelter, J. Chem. Phys. 128, 24107 (2008).          
 * [4]  Kuang & Gezelter,  J. Chem. Phys. 133, 164101 (2010).
 * [5]  Vardeman, Stocker & Gezelter, J. Chem. Theory Comput. 7, 834 (2011).
 */
 
/**
 * @file Quaternion.hpp
 * @author Teng Lin
 * @date 10/11/2004
 * @version 1.0
 */

#ifndef MATH_QUATERNION_HPP
#define MATH_QUATERNION_HPP
#include "config.h"
#include <cmath>

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

namespace OpenMD{

  /**
   * @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);
      return *this;
    }
    
    /**
       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:	1767
Committed:	Fri Jul 6 22:01:58 2012 UTC (12 years, 9 months ago) by gezelter
File size:	20600 byte(s)
Log Message:	Various fixes required to compile OpenMD with the MS Visual C++ compiler
#	User	Rev	Content
1	gezelter	507	/*
2	gezelter	246	* Copyright (c) 2005 The University of Notre Dame. All Rights Reserved.
3	tim	92	*
4	gezelter	246	* 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	gezelter	1390	* 1. Redistributions of source code must retain the above copyright
10	gezelter	246	* notice, this list of conditions and the following disclaimer.
11			*
12	gezelter	1390	* 2. Redistributions in binary form must reproduce the above copyright
13	gezelter	246	* notice, this list of conditions and the following disclaimer in the
14			* documentation and/or other materials provided with the
15			* distribution.
16			*
17			* This software is provided "AS IS," without a warranty of any
18			* kind. All express or implied conditions, representations and
19			* warranties, including any implied warranty of merchantability,
20			* fitness for a particular purpose or non-infringement, are hereby
21			* excluded. The University of Notre Dame and its licensors shall not
22			* be liable for any damages suffered by licensee as a result of
23			* using, modifying or distributing the software or its
24			* derivatives. In no event will the University of Notre Dame or its
25			* licensors be liable for any lost revenue, profit or data, or for
26			* direct, indirect, special, consequential, incidental or punitive
27			* damages, however caused and regardless of the theory of liability,
28			* arising out of the use of or inability to use software, even if the
29			* University of Notre Dame has been advised of the possibility of
30			* such damages.
31	gezelter	1390	*
32			* SUPPORT OPEN SCIENCE! If you use OpenMD or its source code in your
33			* research, please cite the appropriate papers when you publish your
34			* work. Good starting points are:
35			*
36			* [1] Meineke, et al., J. Comp. Chem. 26, 252-271 (2005).
37			* [2] Fennell & Gezelter, J. Chem. Phys. 124, 234104 (2006).
38			* [3] Sun, Lin & Gezelter, J. Chem. Phys. 128, 24107 (2008).
39	gezelter	1665	* [4] Kuang & Gezelter, J. Chem. Phys. 133, 164101 (2010).
40			* [5] Vardeman, Stocker & Gezelter, J. Chem. Theory Comput. 7, 834 (2011).
41	tim	92	*/
42	gezelter	246
43	tim	92	/**
44			* @file Quaternion.hpp
45			* @author Teng Lin
46			* @date 10/11/2004
47			* @version 1.0
48			*/
49
50			#ifndef MATH_QUATERNION_HPP
51			#define MATH_QUATERNION_HPP
52	gezelter	1767	#include "config.h"
53			#include <cmath>
54	tim	92
55	cli2	1360	#include "math/Vector3.hpp"
56	tim	110	#include "math/SquareMatrix.hpp"
57	cli2	1360	#define ISZERO(a,eps) ( (a)>-(eps) && (a)<(eps) )
58			const RealType tiny=1.0e-6;
59	tim	93
60	gezelter	1390	namespace OpenMD{
61	tim	92
62	gezelter	507	/**
63			* @class Quaternion Quaternion.hpp "math/Quaternion.hpp"
64			* Quaternion is a sort of a higher-level complex number.
65			* It is defined as Q = w + xi + yj + z*k,
66	tim	963	* where w, x, y, and z are numbers of type T (e.g. RealType), and
67	gezelter	507	* ii = -1; jj = -1; k*k = -1;
68			* ij = k; jk = i; k*i = j;
69			*/
70			template<typename Real>
71			class Quaternion : public Vector<Real, 4> {
72	cli2	1360
73	gezelter	507	public:
74			Quaternion() : Vector<Real, 4>() {}
75	tim	92
76	gezelter	507	/** Constructs and initializes a Quaternion from w, x, y, z values */
77			Quaternion(Real w, Real x, Real y, Real z) {
78			this->data_[0] = w;
79			this->data_[1] = x;
80			this->data_[2] = y;
81			this->data_[3] = z;
82			}
83	tim	93
84	gezelter	507	/** Constructs and initializes a Quaternion from a Vector<Real,4> */
85			Quaternion(const Vector<Real,4>& v)
86			: Vector<Real, 4>(v){
87	cli2	1360	}
88	tim	92
89	gezelter	507	/** copy assignment */
90			Quaternion& operator =(const Vector<Real, 4>& v){
91			if (this == & v)
92			return *this;
93	cli2	1360
94	gezelter	507	Vector<Real, 4>::operator=(v);
95	cli2	1360
96	gezelter	507	return *this;
97			}
98	cli2	1360
99	gezelter	507	/**
100			* Returns the value of the first element of this quaternion.
101			* @return the value of the first element of this quaternion
102			*/
103			Real w() const {
104			return this->data_[0];
105			}
106	tim	93
107	gezelter	507	/**
108			* Returns the reference of the first element of this quaternion.
109			* @return the reference of the first element of this quaternion
110			*/
111			Real& w() {
112			return this->data_[0];
113			}
114	tim	93
115	gezelter	507	/**
116			* Returns the value of the first element of this quaternion.
117			* @return the value of the first element of this quaternion
118			*/
119			Real x() const {
120			return this->data_[1];
121			}
122	tim	93
123	gezelter	507	/**
124			* Returns the reference of the second element of this quaternion.
125			* @return the reference of the second element of this quaternion
126			*/
127			Real& x() {
128			return this->data_[1];
129			}
130	tim	93
131	gezelter	507	/**
132			* Returns the value of the thirf element of this quaternion.
133			* @return the value of the third element of this quaternion
134			*/
135			Real y() const {
136			return this->data_[2];
137			}
138	tim	93
139	gezelter	507	/**
140			* Returns the reference of the third element of this quaternion.
141			* @return the reference of the third element of this quaternion
142			*/
143			Real& y() {
144			return this->data_[2];
145			}
146	tim	93
147	gezelter	507	/**
148			* Returns the value of the fourth element of this quaternion.
149			* @return the value of the fourth element of this quaternion
150			*/
151			Real z() const {
152			return this->data_[3];
153			}
154			/**
155			* Returns the reference of the fourth element of this quaternion.
156			* @return the reference of the fourth element of this quaternion
157			*/
158			Real& z() {
159			return this->data_[3];
160			}
161	tim	93
162	gezelter	507	/**
163			* Tests if this quaternion is equal to other quaternion
164			* @return true if equal, otherwise return false
165			* @param q quaternion to be compared
166			*/
167			inline bool operator ==(const Quaternion<Real>& q) {
168	tim	110
169	gezelter	507	for (unsigned int i = 0; i < 4; i ++) {
170			if (!equal(this->data_[i], q[i])) {
171			return false;
172			}
173			}
174	tim	110
175	gezelter	507	return true;
176			}
177	tim	110
178	gezelter	507	/**
179			* Returns the inverse of this quaternion
180			* @return inverse
181			* @note since quaternion is a complex number, the inverse of quaternion
182			* q = w + xi + yj+ zk is inv_q = (w -xi - yj - zk)/(\|q\|^2)
183			*/
184			Quaternion<Real> inverse() {
185			Quaternion<Real> q;
186			Real d = this->lengthSquare();
187	tim	93
188	gezelter	507	q.w() = w() / d;
189			q.x() = -x() / d;
190			q.y() = -y() / d;
191			q.z() = -z() / d;
192	tim	93
193	gezelter	507	return q;
194			}
195	tim	93
196	gezelter	507	/**
197			* Sets the value to the multiplication of itself and another quaternion
198			* @param q the other quaternion
199			*/
200			void mul(const Quaternion<Real>& q) {
201			Quaternion<Real> tmp(*this);
202	tim	93
203	gezelter	507	this->data_[0] = (tmp[0]q[0]) -(tmp[1]q[1]) - (tmp[2]q[2]) - (tmp[3]q[3]);
204			this->data_[1] = (tmp[0]q[1]) + (tmp[1]q[0]) + (tmp[2]q[3]) - (tmp[3]q[2]);
205			this->data_[2] = (tmp[0]q[2]) + (tmp[2]q[0]) + (tmp[3]q[1]) - (tmp[1]q[3]);
206			this->data_[3] = (tmp[0]q[3]) + (tmp[3]q[0]) + (tmp[1]q[2]) - (tmp[2]q[1]);
207			}
208	tim	93
209	gezelter	507	void mul(const Real& s) {
210			this->data_[0] *= s;
211			this->data_[1] *= s;
212			this->data_[2] *= s;
213			this->data_[3] *= s;
214			}
215	tim	93
216	gezelter	507	/** Set the value of this quaternion to the division of itself by another quaternion */
217			void div(Quaternion<Real>& q) {
218			mul(q.inverse());
219			}
220	tim	110
221	gezelter	507	void div(const Real& s) {
222			this->data_[0] /= s;
223			this->data_[1] /= s;
224			this->data_[2] /= s;
225			this->data_[3] /= s;
226			}
227	tim	93
228	gezelter	507	Quaternion<Real>& operator *=(const Quaternion<Real>& q) {
229			mul(q);
230			return *this;
231			}
232	tim	110
233	gezelter	507	Quaternion<Real>& operator *=(const Real& s) {
234			mul(s);
235			return *this;
236			}
237	tim	93
238	gezelter	507	Quaternion<Real>& operator /=(Quaternion<Real>& q) {
239			this = q.inverse();
240			return *this;
241			}
242	tim	110
243	gezelter	507	Quaternion<Real>& operator /=(const Real& s) {
244			div(s);
245			return *this;
246			}
247			/**
248			* Returns the conjugate quaternion of this quaternion
249			* @return the conjugate quaternion of this quaternion
250			*/
251	cli2	1360	Quaternion<Real> conjugate() const {
252	gezelter	507	return Quaternion<Real>(w(), -x(), -y(), -z());
253			}
254	tim	93
255	cli2	1360
256	gezelter	507	/**
257	cli2	1360	return rotation angle from -PI to PI
258			*/
259			inline Real get_rotation_angle() const{
260	gezelter	1686	if( w() < (Real)0.0 )
261	cli2	1360	return 2.0atan2(-sqrt( x()x() + y()y() + z()z() ), -w() );
262			else
263			return 2.0atan2( sqrt( x()x() + y()y() + z()z() ), w() );
264			}
265
266			/**
267			create a unit quaternion from axis angle representation
268			*/
269			Quaternion<Real> fromAxisAngle(const Vector3<Real>& axis,
270			const Real& angle){
271			Vector3<Real> v(axis);
272			v.normalize();
273			Real half_angle = angle*0.5;
274			Real sin_a = sin(half_angle);
275			*this = Quaternion<Real>(cos(half_angle),
276			v.x()*sin_a,
277			v.y()*sin_a,
278			v.z()*sin_a);
279	gezelter	1390	return *this;
280	cli2	1360	}
281
282			/**
283			convert a quaternion to axis angle representation,
284			preserve the axis direction and angle from -PI to +PI
285			*/
286			void toAxisAngle(Vector3<Real>& axis, Real& angle)const {
287			Real vl = sqrt( x()x() + y()y() + z()*z() );
288			if( vl > tiny ) {
289			Real ivl = 1.0/vl;
290			axis.x() = x() * ivl;
291			axis.y() = y() * ivl;
292			axis.z() = z() * ivl;
293
294			if( w() < 0 )
295			angle = 2.0*atan2(-vl, -w()); //-PI,0
296			else
297			angle = 2.0*atan2( vl, w()); //0,PI
298			} else {
299			axis = Vector3<Real>(0.0,0.0,0.0);
300			angle = 0.0;
301			}
302			}
303
304			/**
305			shortest arc quaternion rotate one vector to another by shortest path.
306			create rotation from -> to, for any length vectors.
307			*/
308			Quaternion<Real> fromShortestArc(const Vector3d& from,
309			const Vector3d& to ) {
310
311			Vector3d c( cross(from,to) );
312			*this = Quaternion<Real>(dot(from,to),
313			c.x(),
314			c.y(),
315			c.z());
316
317			this->normalize(); // if "from" or "to" not unit, normalize quat
318	gezelter	1686	w() += 1.0f; // reducing angle to halfangle
319			if( w() <= 1e-6 ) { // angle close to PI
320	cli2	1360	if( ( from.z()from.z() ) > ( from.x()from.x() ) ) {
321	gezelter	1686	this->data_[0] = w();
322	cli2	1360	this->data_[1] = 0.0; //cross(from , Vector3d(1,0,0))
323			this->data_[2] = from.z();
324			this->data_[3] = -from.y();
325			} else {
326	gezelter	1686	this->data_[0] = w();
327	cli2	1360	this->data_[1] = from.y(); //cross(from, Vector3d(0,0,1))
328			this->data_[2] = -from.x();
329			this->data_[3] = 0.0;
330			}
331			}
332			this->normalize();
333			}
334
335			Real ComputeTwist(const Quaternion& q) {
336			return (Real)2.0 * atan2(q.z(), q.w());
337			}
338
339			void RemoveTwist(Quaternion& q) {
340			Real t = ComputeTwist(q);
341			Quaternion rt = fromAxisAngle(V3Z, t);
342
343			q *= rt.inverse();
344			}
345
346			void getTwistSwingAxisAngle(Real& twistAngle, Real& swingAngle,
347			Vector3<Real>& swingAxis) {
348
349			twistAngle = (Real)2.0 * atan2(z(), w());
350			Quaternion rt, rs;
351			rt.fromAxisAngle(V3Z, twistAngle);
352			rs = this rt.inverse();
353
354			Real vl = sqrt( rs.x()rs.x() + rs.y()rs.y() + rs.z()*rs.z() );
355			if( vl > tiny ) {
356			Real ivl = 1.0 / vl;
357			swingAxis.x() = rs.x() * ivl;
358			swingAxis.y() = rs.y() * ivl;
359			swingAxis.z() = rs.z() * ivl;
360
361			if( rs.w() < 0.0 )
362			swingAngle = 2.0*atan2(-vl, -rs.w()); //-PI,0
363			else
364			swingAngle = 2.0*atan2( vl, rs.w()); //0,PI
365			} else {
366			swingAxis = Vector3<Real>(1.0,0.0,0.0);
367			swingAngle = 0.0;
368			}
369			}
370
371
372			Vector3<Real> rotate(const Vector3<Real>& v) {
373
374			Quaternion<Real> q(v.x() * w() + v.z() * y() - v.y() * z(),
375			v.y() * w() + v.x() * z() - v.z() * x(),
376			v.z() * w() + v.y() * x() - v.x() * y(),
377			v.x() * x() + v.y() * y() + v.z() * z());
378
379			return Vector3<Real>(w()q.x() + x()q.w() + y()q.z() - z()q.y(),
380			w()q.y() + y()q.w() + z()q.x() - x()q.z(),
381			w()q.z() + z()q.w() + x()q.y() - y()q.x())*
382			( 1.0/this->lengthSquare() );
383			}
384
385			Quaternion<Real>& align (const Vector3<Real>& V1,
386			const Vector3<Real>& V2) {
387
388			// If V1 and V2 are not parallel, the axis of rotation is the unit-length
389			// vector U = Cross(V1,V2)/Length(Cross(V1,V2)). The angle of rotation,
390			// A, is the angle between V1 and V2. The quaternion for the rotation is
391			// q = cos(A/2) + sin(A/2)(uxi+uyj+uzk) where U = (ux,uy,uz).
392			//
393			// (1) Rather than extract A = acos(Dot(V1,V2)), multiply by 1/2, then
394			// compute sin(A/2) and cos(A/2), we reduce the computational costs
395			// by computing the bisector B = (V1+V2)/Length(V1+V2), so cos(A/2) =
396			// Dot(V1,B).
397			//
398			// (2) The rotation axis is U = Cross(V1,B)/Length(Cross(V1,B)), but
399			// Length(Cross(V1,B)) = Length(V1)Length(B)sin(A/2) = sin(A/2), in
400			// which case sin(A/2)(uxi+uyj+uzk) = (cxi+cyj+cz*k) where
401			// C = Cross(V1,B).
402			//
403			// If V1 = V2, then B = V1, cos(A/2) = 1, and U = (0,0,0). If V1 = -V2,
404			// then B = 0. This can happen even if V1 is approximately -V2 using
405			// floating point arithmetic, since Vector3::Normalize checks for
406			// closeness to zero and returns the zero vector accordingly. The test
407			// for exactly zero is usually not recommend for floating point
408			// arithmetic, but the implementation of Vector3::Normalize guarantees
409			// the comparison is robust. In this case, the A = pi and any axis
410			// perpendicular to V1 may be used as the rotation axis.
411
412			Vector3<Real> Bisector = V1 + V2;
413			Bisector.normalize();
414
415			Real CosHalfAngle = dot(V1,Bisector);
416
417			this->data_[0] = CosHalfAngle;
418
419			if (CosHalfAngle != (Real)0.0) {
420			Vector3<Real> Cross = cross(V1, Bisector);
421			this->data_[1] = Cross.x();
422			this->data_[2] = Cross.y();
423			this->data_[3] = Cross.z();
424			} else {
425			Real InvLength;
426			if (fabs(V1[0]) >= fabs(V1[1])) {
427			// V1.x or V1.z is the largest magnitude component
428			InvLength = (Real)1.0/sqrt(V1[0]V1[0] + V1[2]V1[2]);
429
430			this->data_[1] = -V1[2]*InvLength;
431			this->data_[2] = (Real)0.0;
432			this->data_[3] = +V1[0]*InvLength;
433			} else {
434			// V1.y or V1.z is the largest magnitude component
435			InvLength = (Real)1.0 / sqrt(V1[1]V1[1] + V1[2]V1[2]);
436
437			this->data_[1] = (Real)0.0;
438			this->data_[2] = +V1[2]*InvLength;
439			this->data_[3] = -V1[1]*InvLength;
440			}
441			}
442			return *this;
443			}
444
445			void toTwistSwing ( Real& tw, Real& sx, Real& sy ) {
446
447			// First test if the swing is in the singularity:
448
449			if ( ISZERO(z(),tiny) && ISZERO(w(),tiny) ) { sx=sy=M_PI; tw=0; return; }
450
451			// Decompose into twist-swing by solving the equation:
452			//
453			// Qtwist(t2) Qswing(s*2) = q
454			//
455			// note: (x,y) is the normalized swing axis (xx+yy=1)
456			//
457			// ( Ct 0 0 St ) * ( Cs xSs ySs 0 ) = ( qw qx qy qz )
458			// ( CtCs xSsCt-yStSs xStSs+ySsCt StCs ) = ( qw qx qy qz ) (1)
459			// From (1): CtCs / StCs = qw/qz => Ct/St = qw/qz => tan(t) = qz/qw (2)
460			//
461			// The swing rotation/2 s comes from:
462			//
463			// From (1): (CtCs)^2 + (StCs)^2 = qw^2 + qz^2 =>
464			// Cs = sqrt ( qw^2 + qz^2 ) (3)
465			//
466			// From (1): (xSsCt-yStSs)^2 + (xStSs+ySsCt)^2 = qx^2 + qy^2 =>
467			// Ss = sqrt ( qx^2 + qy^2 ) (4)
468			// From (1): \|SsCt -StSs\| \|x\| = \|qx\|
469			// \|StSs +SsCt\| \|y\| \|qy\| (5)
470
471			Real qw, qx, qy, qz;
472
473			if ( w()<0 ) {
474			qw=-w();
475			qx=-x();
476			qy=-y();
477			qz=-z();
478			} else {
479			qw=w();
480			qx=x();
481			qy=y();
482			qz=z();
483			}
484
485			Real t = atan2 ( qz, qw ); // from (2)
486			Real s = atan2( sqrt(qxqx+qyqy), sqrt(qzqz+qwqw) ); // from (3)
487			// and (4)
488
489			Real x=0.0, y=0.0, sins=sin(s);
490
491			if ( !ISZERO(sins,tiny) ) {
492			Real sint = sin(t);
493			Real cost = cos(t);
494
495			// by solving the linear system in (5):
496			y = (-qxsint + qycost)/sins;
497			x = ( qxcost + qysint)/sins;
498			}
499
500			tw = (Real)2.0*t;
501			sx = (Real)2.0xs;
502			sy = (Real)2.0ys;
503			}
504
505			void toSwingTwist(Real& sx, Real& sy, Real& tw ) {
506
507			// Decompose q into swing-twist using a similar development as
508			// in function toTwistSwing
509
510			if ( ISZERO(z(),tiny) && ISZERO(w(),tiny) ) { sx=sy=M_PI; tw=0; }
511
512			Real qw, qx, qy, qz;
513			if ( w() < 0 ){
514			qw=-w();
515			qx=-x();
516			qy=-y();
517			qz=-z();
518			} else {
519			qw=w();
520			qx=x();
521			qy=y();
522			qz=z();
523			}
524
525			// Get the twist t:
526			Real t = 2.0 * atan2(qz,qw);
527
528			Real bet = atan2( sqrt(qxqx+qyqy), sqrt(qzqz+qwqw) );
529			Real gam = t/2.0;
530			Real sinc = ISZERO(bet,tiny)? 1.0 : sin(bet)/bet;
531			Real singam = sin(gam);
532			Real cosgam = cos(gam);
533
534			sx = Real( (2.0/sinc) * (cosgamqx - singamqy) );
535			sy = Real( (2.0/sinc) * (singamqx + cosgamqy) );
536			tw = Real( t );
537			}
538
539
540
541			/**
542	gezelter	507	* Returns the corresponding rotation matrix (3x3)
543			* @return a 3x3 rotation matrix
544			*/
545			SquareMatrix<Real, 3> toRotationMatrix3() {
546			SquareMatrix<Real, 3> rotMat3;
547	cli2	1360
548	gezelter	507	Real w2;
549			Real x2;
550			Real y2;
551			Real z2;
552	tim	93
553	gezelter	507	if (!this->isNormalized())
554			this->normalize();
555	tim	93
556	gezelter	507	w2 = w() * w();
557			x2 = x() * x();
558			y2 = y() * y();
559			z2 = z() * z();
560	tim	93
561	gezelter	507	rotMat3(0, 0) = w2 + x2 - y2 - z2;
562			rotMat3(0, 1) = 2.0 * ( x() * y() + w() * z() );
563			rotMat3(0, 2) = 2.0 * ( x() * z() - w() * y() );
564	tim	93
565	gezelter	507	rotMat3(1, 0) = 2.0 * ( x() * y() - w() * z() );
566			rotMat3(1, 1) = w2 - x2 + y2 - z2;
567			rotMat3(1, 2) = 2.0 * ( y() * z() + w() * x() );
568	tim	93
569	gezelter	507	rotMat3(2, 0) = 2.0 * ( x() * z() + w() * y() );
570			rotMat3(2, 1) = 2.0 * ( y() * z() - w() * x() );
571			rotMat3(2, 2) = w2 - x2 -y2 +z2;
572	tim	110
573	gezelter	507	return rotMat3;
574			}
575	tim	93
576	gezelter	507	};//end Quaternion
577	tim	93
578	tim	110
579	tim	93	/**
580	tim	110	* Returns the vaule of scalar multiplication of this quaterion q (q * s).
581			* @return the vaule of scalar multiplication of this vector
582			* @param q the source quaternion
583			* @param s the scalar value
584			*/
585	gezelter	507	template<typename Real, unsigned int Dim>
586			Quaternion<Real> operator * ( const Quaternion<Real>& q, Real s) {
587			Quaternion<Real> result(q);
588			result.mul(s);
589			return result;
590			}
591	tim	110
592	gezelter	507	/**
593			* Returns the vaule of scalar multiplication of this quaterion q (q * s).
594			* @return the vaule of scalar multiplication of this vector
595			* @param s the scalar value
596			* @param q the source quaternion
597			*/
598			template<typename Real, unsigned int Dim>
599			Quaternion<Real> operator * ( const Real& s, const Quaternion<Real>& q ) {
600			Quaternion<Real> result(q);
601			result.mul(s);
602			return result;
603			}
604	tim	110
605	gezelter	507	/**
606			* Returns the multiplication of two quaternion
607			* @return the multiplication of two quaternion
608			* @param q1 the first quaternion
609			* @param q2 the second quaternion
610			*/
611			template<typename Real>
612			inline Quaternion<Real> operator *(const Quaternion<Real>& q1, const Quaternion<Real>& q2) {
613			Quaternion<Real> result(q1);
614			result *= q2;
615			return result;
616			}
617	tim	93
618	gezelter	507	/**
619			* Returns the division of two quaternion
620			* @param q1 divisor
621			* @param q2 dividen
622			*/
623	tim	93
624	gezelter	507	template<typename Real>
625			inline Quaternion<Real> operator /( Quaternion<Real>& q1, Quaternion<Real>& q2) {
626			return q1 * q2.inverse();
627			}
628	tim	93
629	gezelter	507	/**
630			* Returns the value of the division of a scalar by a quaternion
631			* @return the value of the division of a scalar by a quaternion
632			* @param s scalar
633			* @param q quaternion
634			* @note for a quaternion q, 1/q = q.inverse()
635			*/
636			template<typename Real>
637			Quaternion<Real> operator /(const Real& s, Quaternion<Real>& q) {
638	tim	93
639	gezelter	507	Quaternion<Real> x;
640			x = q.inverse();
641			x *= s;
642			return x;
643			}
644	tim	110
645	gezelter	507	template <class T>
646			inline bool operator==(const Quaternion<T>& lhs, const Quaternion<T>& rhs) {
647			return equal(lhs[0] ,rhs[0]) && equal(lhs[1] , rhs[1]) && equal(lhs[2], rhs[2]) && equal(lhs[3], rhs[3]);
648			}
649	tim	110
650	tim	963	typedef Quaternion<RealType> Quat4d;
651	tim	92	}
652			#endif //MATH_QUATERNION_HPP