source: orxonox.OLD/orxonox/branches/physics/src/lib/math/quaternion.h @ 4076

/*! 
    \file vector.h
    \brief A basic 3D math framework
    
    Contains classes to handle vectors, lines, rotations and planes
*/ 

#ifndef _QUATERNION_H
#define _QUATERNION_H

#include <math.h>
//! PI the circle-constant

//! Quaternion
/**
        Class to handle 3-dimensional rotation efficiently
*/
class Quaternion
{
 public:
  Vector v;     //!< Imaginary Vector
  float w;        //!< Real part of the number
  
  Quaternion ();
  Quaternion (float m[4][4]);
  Quaternion (float angle, const Vector& axis);
  Quaternion (const Vector& dir, const Vector& up);
  Quaternion (float roll, float pitch, float yaw);
  
  Quaternion operator/ (const float& f) const;
  Quaternion operator* (const float& f) const;
  Quaternion operator* (const Quaternion& q) const;
  Quaternion operator+ (const Quaternion& q) const;
  Quaternion operator- (const Quaternion& q) const;
  Quaternion conjugate () const;
  Quaternion inverse () const;
  Vector apply (Vector& f) const;
  float norm () const;
  void matrix (float m[4][4]) const;
  void quatSlerp(const Quaternion* from, const Quaternion* to, const float t, Quaternion* res);
  
  void debug();
 private:
  float DELTA;      //!< resolution of calculation

};


/**
        \brief creates a multiplicational identity Quaternion
*/
Quaternion::Quaternion ()
{
        w = 1;
        v = Vector(0,0,0);
}

/**
        \brief turns a rotation along an axis into a Quaternion
        \param angle: the amount of radians to rotate
        \param axis: the axis to rotate around
*/
Quaternion::Quaternion (float angle, const Vector& axis)
{
        w = cos(angle/2);
        v = axis * sin(angle/2);
}

/**
   \brief calculates a lookAt rotation
   \param dir: the direction you want to look
   \param up: specify what direction up should be
   
   Mathematically this determines the rotation a (0,0,1)-Vector has to undergo to point
   the same way as dir. If you want to use this with cameras, you'll have to reverse the
   dir Vector (Vector(0,0,0) - your viewing direction) or you'll point the wrong way. You
   can use this for meshes as well (then you do not have to reverse the vector), but keep 
   in mind that if you do that, the model's front has to point in +z direction, and left 
   and right should be -x or +x respectively or the mesh wont rotate correctly.
*/
Quaternion::Quaternion (const Vector& dir, const Vector& up)
{
  Vector z = dir;
  z.normalize(); 
  Vector x = up.cross(z);
  x.normalize(); 
  Vector y = z.cross(x);
  
  float m[4][4];
  m[0][0] = x.x;
  m[0][1] = x.y;
  m[0][2] = x.z;
  m[0][3] = 0;
  m[1][0] = y.x;
  m[1][1] = y.y;
  m[1][2] = y.z;
  m[1][3] = 0;
  m[2][0] = z.x;
  m[2][1] = z.y;
  m[2][2] = z.z;
  m[2][3] = 0;
  m[3][0] = 0;
  m[3][1] = 0;
  m[3][2] = 0;
  m[3][3] = 1;
  
  *this = Quaternion (m);
}

/**
        \brief calculates a rotation from euler angles
        \param roll: the roll in radians
        \param pitch: the pitch in radians
        \param yaw: the yaw in radians
        
        I DO HONESTLY NOT EXACTLY KNOW WHICH ANGLE REPRESENTS WHICH ROTATION. And I do not know 
        in what order they are applied, I just copy-pasted the code.
*/
Quaternion::Quaternion (float roll, float pitch, float yaw)
{
        float cr, cp, cy, sr, sp, sy, cpcy, spsy;
        
        // calculate trig identities
        cr = cos(roll/2);
        cp = cos(pitch/2);
        cy = cos(yaw/2);
        
        sr = sin(roll/2);
        sp = sin(pitch/2);
        sy = sin(yaw/2);
        
        cpcy = cp * cy;
        spsy = sp * sy;
        
        w = cr * cpcy + sr * spsy;
        v.x = sr * cpcy - cr * spsy;
        v.y = cr * sp * cy + sr * cp * sy;
        v.z = cr * cp * sy - sr * sp * cy;
}

/**
        \brief rotates one Quaternion by another
        \param q: another Quaternion to rotate this by
        \return a quaternion that represents the first one rotated by the second one (WARUNING: this operation is not commutative! e.g. (A*B) != (B*A))
*/
Quaternion Quaternion::operator*(const Quaternion& q) const
{
        float A, B, C, D, E, F, G, H;
        Quaternion r;
        
        A = (w   + v.x)*(q.w   + q.v.x);
        B = (v.z - v.y)*(q.v.y - q.v.z);
        C = (w   - v.x)*(q.v.y + q.v.z); 
        D = (v.y + v.z)*(q.w   - q.v.x);
        E = (v.x + v.z)*(q.v.x + q.v.y);
        F = (v.x - v.z)*(q.v.x - q.v.y);
        G = (w   + v.y)*(q.w   - q.v.z);
        H = (w   - v.y)*(q.w   + q.v.z);
        
        r.w = B +  (-E - F + G + H)/2;
        r.v.x = A - (E + F + G + H)/2; 
        r.v.y = C + (E - F + G - H)/2; 
        r.v.z = D + (E - F - G + H)/2;
        
        return r;
}

/**
        \brief add two Quaternions
        \param q: another Quaternion
        \return the sum of both Quaternions
*/
Quaternion Quaternion::operator+(const Quaternion& q) const
{
        Quaternion r(*this);
        r.w = r.w + q.w;
        r.v = r.v + q.v;
        return r;
}

/**
        \brief subtract two Quaternions
        \param q: another Quaternion
        \return the difference of both Quaternions
*/
Quaternion Quaternion::operator- (const Quaternion& q) const
{
        Quaternion r(*this);
        r.w = r.w - q.w;
        r.v = r.v - q.v;
        return r;
}

/**
        \brief rotate a Vector by a Quaternion
        \param v: the Vector
        \return a new Vector representing v rotated by the Quaternion
*/
Vector Quaternion::apply (Vector& v) const
{
        Quaternion q;
        q.v = v;
        q.w = 0;
        q = *this * q * conjugate();
        return q.v;
}

/**
        \brief multiply a Quaternion with a real value
        \param f: a real value
        \return a new Quaternion containing the product
*/
Quaternion Quaternion::operator*(const float& f) const
{
        Quaternion r(*this);
        r.w = r.w*f;
        r.v = r.v*f;
        return r;
}

/**
        \brief divide a Quaternion by a real value
        \param f: a real value
        \return a new Quaternion containing the quotient
*/
Quaternion Quaternion::operator/(const float& f) const
{
        if( f == 0) return Quaternion();
        Quaternion r(*this);
        r.w = r.w/f;
        r.v = r.v/f;
        return r;
}

/**
        \brief calculate the conjugate value of the Quaternion
        \return the conjugate Quaternion
*/
Quaternion Quaternion::conjugate() const
{
        Quaternion r(*this);
        r.v = Vector() - r.v;
        return r;
}

/**
        \brief calculate the norm of the Quaternion
        \return the norm of The Quaternion
*/
float Quaternion::norm() const
{
        return w*w + v.x*v.x + v.y*v.y + v.z*v.z;
}

/**
        \brief calculate the inverse value of the Quaternion
        \return the inverse Quaternion
        
        Note that this is equal to conjugate() if the Quaternion's norm is 1
*/
Quaternion Quaternion::inverse() const
{
        float n = norm();
        if (n != 0)
        {
                return conjugate() / norm();
        }
        else return Quaternion();
}

/**
        \brief convert the Quaternion to a 4x4 rotational glMatrix
        \param m: a buffer to store the Matrix in
*/
void Quaternion::matrix (float m[4][4]) const
{
  float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2; 
  
  // calculate coefficients
  x2 = v.x + v.x;
  y2 = v.y + v.y; 
  z2 = v.z + v.z;
  xx = v.x * x2; xy = v.x * y2; xz = v.x * z2;
  yy = v.y * y2; yz = v.y * z2; zz = v.z * z2;
  wx = w * x2; wy = w * y2; wz = w * z2;
  
  m[0][0] = 1.0 - (yy + zz); m[1][0] = xy - wz;
  m[2][0] = xz + wy; m[3][0] = 0.0;
        
  m[0][1] = xy + wz; m[1][1] = 1.0 - (xx + zz);
  m[2][1] = yz - wx; m[3][1] = 0.0;
  
  m[0][2] = xz - wy; m[1][2] = yz + wx;
  m[2][2] = 1.0 - (xx + yy); m[3][2] = 0.0;
  
  m[0][3] = 0; m[1][3] = 0;
  m[2][3] = 0; m[3][3] = 1;
}

/**
   \brief performs a smooth move.
   \param from from where
   \param to to where
   \param t the time this transformation should take
   \param res The approximation-density
*/
void Quaternion::quatSlerp(const Quaternion* from, const Quaternion* to, float t, Quaternion* res)
{
  float tol[4];
  double omega, cosom, sinom, scale0, scale1;
  DELTA = 0.2;

  cosom = from->v.x * to->v.x + from->v.y * to->v.y + from->v.z * to->v.z + from->w * to->w;

  if( cosom < 0.0 ) 
    { 
      cosom = -cosom; 
      tol[0] = -to->v.x;
      tol[1] = -to->v.y;
      tol[2] = -to->v.z;
      tol[3] = -to->w;
    }
  else
    {
      tol[0] = to->v.x;
      tol[1] = to->v.y;
      tol[2] = to->v.z;
      tol[3] = to->w;
    }
  
  //if( (1.0 - cosom) > DELTA )
  //{
      omega = acos(cosom);
      sinom = sin(omega);
      scale0 = sin((1.0 - t) * omega) / sinom;
      scale1 = sin(t * omega) / sinom;
      //}
      /*
  else
    {
      scale0 = 1.0 - t;
      scale1 = t;
    }
      */
  res->v.x = scale0 * from->v.x + scale1 * tol[0];
  res->v.y = scale0 * from->v.y + scale1 * tol[1];
  res->v.z = scale0 * from->v.z + scale1 * tol[2];
  res->w = scale0 * from->w + scale1 * tol[3];
}


/**
   \brief convert a rotational 4x4 glMatrix into a Quaternion
   \param m: a 4x4 matrix in glMatrix order
*/
Quaternion::Quaternion (float m[4][4])
{
        
  float  tr, s, q[4];
  int    i, j, k;

  int nxt[3] = {1, 2, 0};

  tr = m[0][0] + m[1][1] + m[2][2];

        // check the diagonal
  if (tr > 0.0) 
  {
    s = sqrt (tr + 1.0);
    w = s / 2.0;
    s = 0.5 / s;
    v.x = (m[1][2] - m[2][1]) * s;
    v.y = (m[2][0] - m[0][2]) * s;
    v.z = (m[0][1] - m[1][0]) * s;
        } 
        else 
        {
                // diagonal is negative
        i = 0;
        if (m[1][1] > m[0][0]) i = 1;
    if (m[2][2] > m[i][i]) i = 2;
    j = nxt[i];
    k = nxt[j];

    s = sqrt ((m[i][i] - (m[j][j] + m[k][k])) + 1.0);
      
    q[i] = s * 0.5;
            
    if (s != 0.0) s = 0.5 / s;
        
          q[3] = (m[j][k] - m[k][j]) * s;
    q[j] = (m[i][j] + m[j][i]) * s;
    q[k] = (m[i][k] + m[k][i]) * s;

        v.x = q[0];
        v.y = q[1];
        v.z = q[2];
        w = q[3];
  }
}

/**
   \brief outputs some nice formated debug information about this quaternion
*/
void Quaternion::debug(void)
{
  PRINT(0)("Quaternion Debug Information\n");
  PRINT(0)("real a=%f; imag: x=%f y=%f z=%f\n", w, v.x, v.y, v.z);
}



#endif /* _QUATERNION_H */