CPPLapack Code

//=============================================================================
/*! conjuction */
inline dquater conj(const dquater& q)
{
  return dquater(-q(0),-q(1),-q(2), q(3));
}

//=============================================================================
/*! imag */
inline dcovec3 imag(const dquater& q)
{
  return dcovec3(q(0),q(1),q(2));
}

//=============================================================================
/*! inverse */
inline dquater inv(const dquater& q)
{
  return conj(q)/pow(nrm2(q),2);
}

///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////

//=============================================================================
/*!  */
inline dquater operator*(const dquater& q1, const dquater& q2)
{
  return dquater(q1(3)*q2(0) +q1(0)*q2(3) +q1(1)*q2(2) -q1(2)*q2(1),
                 q1(3)*q2(1) -q1(0)*q2(2) +q1(1)*q2(3) +q1(2)*q2(0),
                 q1(3)*q2(2) +q1(0)*q2(1) -q1(1)*q2(0) +q1(2)*q2(3),
                 q1(3)*q2(3) -q1(0)*q2(0) -q1(1)*q2(1) -q1(2)*q2(2) );
}

//=============================================================================
/*!  */
inline dquater operator/(const dquater& q1, const dquater& q2)
{
  return q1*inv(q2);
}

//=============================================================================
/*!  */
inline dquater operator*=(dquater& q1, const dquater& q2)
{
  q1 =q1*q2;
  return q1;
}

//=============================================================================
/*!  */
inline dquater operator/=(dquater& q1, const dquater& q2)
{
  q1 =q1/q2;
  return q1;
}

///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////

//=============================================================================
/*!  */
inline dquater vr2q(const dcovec3& v, const double& r)
{
  return dquater(v(0),v(1),v(2),r);
}

//=============================================================================
/*!  */
inline dquater vt2q(const dcovec3& v, const double& theta)
{
  return vr2q( v/nrm2(v)*std::sin(0.5*theta), std::cos(0.5*theta) );
}

///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////

//=============================================================================
/*! return vector from quaternion (|vector|=theta) */
inline dcovec3 q2vt(const dquater& q)
{
  double sin_theta_half;
  double theta( 2.*std::acos(q(3)) );
  
  if(theta<M_PI){
    sin_theta_half =std::sin(0.5*theta);
  }
  else{
    theta -=2.*M_PI;
    sin_theta_half =-std::sin(0.5*theta);
  }
  
  return dcovec3( theta*q(0)/sin_theta_half,
                  theta*q(1)/sin_theta_half,
                  theta*q(2)/sin_theta_half );
}

//=============================================================================
/*! return matrix from quaternion */
inline dgemat3 q2m(const dquater& q)
{
  dquater cq( conj(q) );
  dquater X( dquater(+q(3),+q(2),-q(1),-q(0))*cq );
  dquater Y( dquater(-q(2),+q(3),+q(0),-q(1))*cq );
  dquater Z( dquater(+q(1),-q(0),+q(3),-q(2))*cq );
  dgemat3 mat;
  mat(0,0)=X(0); mat(0,1)=Y(0); mat(0,2)=Z(0);
  mat(1,0)=X(1); mat(1,1)=Y(1); mat(1,2)=Z(1);
  mat(2,0)=X(2); mat(2,1)=Y(2); mat(2,2)=Z(2);
  return mat;
}

///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////

//=============================================================================
/*!  */
inline dcovec3 rotate(const dcovec3& v, const dquater& q)
{
  return imag( q*vr2q(v,0.)*conj(q) );
}

//=============================================================================
/*!  */
inline dgemat3 rotate(const dgemat3& m, const dquater& q)
{
  dgemat3 R(q2m(q));
  return R*m*t(R);
}

//=============================================================================
/*!  */
inline dsymat3 rotate(const dsymat3& m, const dquater& q)
{
  dgemat3 R(q2m(q));
  dgemat3 Rm(R*m);
  dsymat3 X(0.);
  for(long i=0; i<3; i++){
    for(long j=0; j<=i; j++){
      for(long k=0; k<3; k++){
        X(i,j) +=Rm(i,k)*R(j,k);
      }
    }
  }
  return X;
}