Biomechanical Joint Model  Author: Anderson Maciel

---

quaternion.cpp

Go to the documentation of this file.

/****************************************************************************
 * Quaternion code by BLACKAXE / kolor aka Laurent Schmalen
 * Version 0.9
 * Read QUAT.DOC for further informations
 */
#include <iostream>
#include <math.h>
#include <float.h>
#include <algebra/quaternion.h>
#include <algebra/comevector3d.h>


Quaternion::Quaternion()
{
  W = 1.0;
  X = 0.0;
  Y = 0.0;
  Z = 0.0;
}

Quaternion::Quaternion(const double w, const double x, const double y, const double z)
{
  W = w;
  X = x;
  Y = y;
  Z = z;
}


Quaternion operator * (const Quaternion &a, const Quaternion &b)
{
  double w,x,y,z;

  w = a.W*b.W - (a.X*b.X + a.Y*b.Y + a.Z*b.Z);
  
  x = a.W*b.X + b.W*a.X + a.Y*b.Z - a.Z*b.Y;
  y = a.W*b.Y + b.W*a.Y + a.Z*b.X - a.X*b.Z;
  z = a.W*b.Z + b.W*a.Z + a.X*b.Y - a.Y*b.X;

  return Quaternion(w,x,y,z); 
}

const Quaternion& Quaternion::operator *= (const Quaternion &q)
{
  double w = W*q.W - (X*q.X + Y*q.Y + Z*q.Z);

  double x = W*q.X + q.W*X + Y*q.Z - Z*q.Y;
  double y = W*q.Y + q.W*Y + Z*q.X - X*q.Z;
  double z = W*q.Z + q.W*Z + X*q.Y - Y*q.X;

  W = w;
  X = x;
  Y = y;
  Z = z;
  return *this;
}

const Quaternion& Quaternion::operator ~ ()
{
  X = -X;
  Y = -Y;
  Z = -Z;
  return *this;
}

const Quaternion& Quaternion::operator - ()
{
  double norme = sqrt(W*W + X*X + Y*Y + Z*Z);
  if (norme == 0.0)
    norme = 1.0;

  double recip = 1.0 / norme;

  W =  W * recip;
  X = -X * recip;
  Y = -Y * recip;
  Z = -Z * recip;

  return *this;
}

const Quaternion& Quaternion::Normalize()
{
  double norme = sqrt(W*W + X*X + Y*Y + Z*Z);
  if (norme == 0.0)
    {
          W = 1.0; 
          X = Y = Z = 0.0;
        }
  else
    {
          double recip = 1.0/norme;

          W *= recip;
          X *= recip;
          Y *= recip;
          Z *= recip;
        }
  return *this;
}

const Quaternion& Quaternion::FromAxis(const double Angle, double x, double y, double z)
{
  double omega, s, c;
    
  s = sqrt(x*x + y*y + z*z);

  // amaciel debug
  //if( s == 0.0 ) s = 1.0;
  
  if (fabs(s) > FLT_EPSILON)
    {
          c = 1.0/s;
          
          x *= c;
          y *= c;
          z *= c;

          omega = -0.5f * Angle;
          s = (double)sin(omega);

          X = s*x;
          Y = s*y;
          Z = s*z;
          W = (double)cos(omega);
        }
  else
    {
          X = Y = 0.0f;
          Z = 0.0f;
          W = 1.0f;
        }
  Normalize();
  return *this;
}


void Quaternion::ToMatrix(double matrix[3][3]) const
{
  matrix[0][0] = 1.0 - 2*Y*Y - 2*Z*Z;
  matrix[1][0] = 2*X*Y + 2*W*Z;
  matrix[2][0] = 2*X*Z - 2*W*Y;

  matrix[0][1] = 2*X*Y - 2*W*Z;
  matrix[1][1] = 1.0 - 2*X*X - 2*Z*Z;
  matrix[2][1] = 2*Y*Z + 2*W*X;

  matrix[0][2] = 2*X*Z + 2*W*Y;
  matrix[1][2] = 2*Y*Z - 2*W*X;
  matrix[2][2] = 1.0 - 2*X*X - 2*Y*Y;
}


void Quaternion::Slerp(const Quaternion &a,const Quaternion &b, const double t)
{
  double omega, cosom, sinom, sclp, sclq;


  cosom = a.X*b.X + a.Y*b.Y + a.Z*b.Z + a.W*b.W;


  if ((1.0f+cosom) > FLT_EPSILON)
        {
          if ((1.0f-cosom) > FLT_EPSILON)
                {
                  omega = acos(cosom);
                  sinom = sin(omega);

                  // amaciel debug
                  //if( sinom == 0.0 ) sinom = 1.0;

                  sclp = sin((1.0f-t)*omega) / sinom;
                  sclq = sin(t*omega) / sinom;
                }
          else
                {
                  sclp = 1.0f - t;
                  sclq = t;
                }

      X = sclp*a.X + sclq*b.X;
          Y = sclp*a.Y + sclq*b.Y;
          Z = sclp*a.Z + sclq*b.Z;
          W = sclp*a.W + sclq*b.W;

        }
  else
        {
          X =-a.Y;
          Y = a.X;
          Z =-a.W;
          W = a.Z;

          sclp = sin((1.0f-t) * PI * 0.5);
          sclq = sin(t * PI * 0.5);

          X = sclp*a.X + sclq*b.X;
          Y = sclp*a.Y + sclq*b.Y;
          Z = sclp*a.Z + sclq*b.Z;

        }
}

const Quaternion& Quaternion::exp()
{                               
  double Mul;
  double Length = sqrt(X*X + Y*Y + Z*Z);

  if (Length > 1.0e-4)
    Mul = sin(Length)/Length;
  else
    Mul = 1.0;

  W = cos(Length);

  X *= Mul;
  Y *= Mul;
  Z *= Mul; 

  return *this;
}
/*
const Quaternion& Quaternion::log()
{
  double Length;

  Length = sqrt(X*X + Y*Y + Z*Z);

  // amaciel debug
  if( W == 0.0 ) W = 1.0;

  Length = atan(Length/W);

  W = 0.0;

  X *= Length;
  Y *= Length;
  Z *= Length;

  return *this;
}
*/

void Quaternion::setAxisAngle(COME_Vector3D axis, double angle)  // radians
{       
        double* aa = new double[4];
    aa[0] = axis.x;
    aa[1] = axis.y;
    aa[2] = axis.z;
    aa[3] = -angle;
    setAxisAngle(aa);
}


void Quaternion::setAxisAngle(double axis_angle[])  // radians - tested ok
{

    double Mysin;
    double length_sqr;
    double one_over_length;
        
    if (axis_angle[3] == 0.0f)
        {
                W = 1.0;
                X = 0.0;
                Y = 0.0;
                Z = 0.0;
        }
        
    length_sqr = axis_angle[0]*axis_angle[0] + axis_angle[1]*axis_angle[1] + axis_angle[2]*axis_angle[2];
    if (length_sqr < 0.0001)
        {
                W = 1.0;
                X = 0.0;
                Y = 0.0;
                Z = 0.0;
                
                // amaciel debug
                return;
        }

        // amaciel debug
        //if( length_sqr == 0.0 ) length_sqr = 1.0;
        
    one_over_length = 1.0f/(double)sqrt(length_sqr);
    axis_angle[0] = axis_angle[0] * one_over_length;
    axis_angle[1] = axis_angle[1] * one_over_length;
    axis_angle[2] = axis_angle[2] * one_over_length;
        
    Mysin = (double)sin(axis_angle[3]*0.5f);
    X = axis_angle[0]*Mysin;
    Y = axis_angle[1]*Mysin;
    Z = axis_angle[2]*Mysin;
    W = (double)cos(axis_angle[3]*0.5f);
}

void Quaternion::makeFromVecs(COME_Vector3D vec1, COME_Vector3D vec2) // modified Ken Shoemake - tested ok
{    
        COME_Vector3D axis;
        COME_Vector3D temp;
   
    
        double dot;
    double angle;
    double abs_angle;

    vec1.vpNormalize();
    vec2.vpNormalize();

    dot = vec1.vpDotProduct(vec2);
    angle = (double)acos(dot);       // -PI < angle < +PI
    if (angle > 0) abs_angle = angle;// 0 <= angle < PI
        else abs_angle= -angle;
                

    if (abs_angle < 0.0001f)             // vec's parallel
      {
                W = 1.0;
                X = 0.0;
                Y = 0.0;
                Z = 0.0;
                return;
      }

    if (abs_angle > ((double)PI)- 0.0001f ) // vec's oppose
      {
                  // Can't use cross product for axis.  Find arbitrary axis.
                  // First try cross with X-axis, else just use Z-axis.
                  axis.setXYZ(1.0f, 0.0f, 0.0f);
                  if (axis.vpDotProduct(vec1) < 0.1f)
                        {
                        temp = axis.vpCrossProduct(vec1);
                        temp.vpNormalize();
                        }
                  else
                        {
                        temp.setXYZ(0.0f, 0.0f, 1.0f);
                        }
                  setAxisAngle(temp, angle);
                  return;
      }

    // normal case
    temp = vec1.vpCrossProduct(vec2);
    temp.vpNormalize();
    setAxisAngle(temp, angle);
}

---