Quaternion.h

#ifndef _QUATERNION_H_
#define _QUATERNION_H_
 
#include <math.h>
#include "Matrix4.h"
#include "Matrix3x3d.h"
 
class Quaternion
{
  
  public:
 
    static const Quaternion ZERO;
    static const Quaternion IDENTITY;
 
    Quaternion()
    {
      w = 1.0f;
      x = 0.0f;
      y = 0.0f;
      z = 0.0f;
    }
  
    Quaternion(float ww, float xx, float yy, float zz)
    {
      w = ww;
      x = xx;
      y = yy;
      z = zz;
    }
 
    //-----------------------------------------------------------------------
    Quaternion operator+ (const Quaternion& rkQ) const
    {
        return Quaternion(w+rkQ.w,x+rkQ.x,y+rkQ.y,z+rkQ.z);
    }
    //-----------------------------------------------------------------------
    Quaternion operator- (const Quaternion& rkQ) const
    {
        return Quaternion(w-rkQ.w,x-rkQ.x,y-rkQ.y,z-rkQ.z);
    }
    //-----------------------------------------------------------------------
    Quaternion operator* (const Quaternion& rkQ) const
    {
        // NOTE:  Multiplication is not generally commutative, so in most
        // cases p*q != q*p.
 
        return Quaternion
        (
            w * rkQ.w - x * rkQ.x - y * rkQ.y - z * rkQ.z,
            w * rkQ.x + x * rkQ.w + y * rkQ.z - z * rkQ.y,
            w * rkQ.y + y * rkQ.w + z * rkQ.x - x * rkQ.z,
            w * rkQ.z + z * rkQ.w + x * rkQ.y - y * rkQ.x
        );
    }
 
    Vector3d operator* (const Vector3d& v) const
    {
      Vector3d uv, uuv;
      Vector3d qvec(x, y, z);
      uv = qvec.CrossProduct(v);
      uuv = qvec.CrossProduct(uv);
      uv *= (2.0f * w);
      uuv *= 2.0f;
 
      return v + uv + uuv;
    }
 
    Quaternion Inverse () const
    {
        float fNorm = w*w+x*x+y*y+z*z;
        if ( fNorm > 0.0 )
        {
            float fInvNorm = 1.0f/fNorm;
            return Quaternion(w*fInvNorm,-x*fInvNorm,-y*fInvNorm,-z*fInvNorm);
        }
        else
        {
            // return an invalid result to flag the error
            return Quaternion();
        }
    }
  
    static Quaternion FromRotationMatrix(double* m)
    {
      float t = (float)(m[0] + m[5] + m[10]);
      float s, w, x, y, z;
      if (t >= 0.0f) {
        s = sqrt(t + 1.0f);
        w = 0.5f * s;
        s = 0.5f / s;
        x = (float)(m[9] - m[6]) * s;
        y = (float)(m[2] - m[8]) * s;
        z = (float)(m[4] - m[1]) * s;
      }
      else
      {
        if ((m[0] > m[5]) && (m[0] > m[10])) {
          s = (float)sqrt(1.0f + m[0] - m[5] - m[10]);
          x = s * 0.5f;
          s = 0.5F / s;
          y = (float)(m[4] + m[1]) * s;
          z = (float)(m[2] + m[8]) * s;
          w = (float)(m[9] - m[6]) * s;
        }
        else
        {
          if (m[5] > m[10]) {
            s = (float)sqrt(1.0f + m[5] - m[0] - m[10]);
            y = s * 0.5f;
            s = 0.5f / s;
            x = (float)(m[4] + m[1]) * s;
            z = (float)(m[9] + m[6]) * s;
            w = (float)(m[2] - m[8]) * s;
          }
          else {
            s = (float)sqrt(1.0f + m[10] - m[0] - m[5]);
            z = s * 0.5f;
            s = 0.5f / s;
            x = (float)(m[2] + m[8]) * s;
            y = (float)(m[9] + m[6]) * s;
            w = (float)(m[4] - m[1]) * s;
          }
        }
      }
 
      return Quaternion(w, x, y, z);
    }
  
    static Quaternion FromRotationMatrix(Matrix3x3d m)
    {
      double rotationMatrix[16];
      for (int r = 0; r < 3; r++)
      {
        for (int c = 0; c < 3; c++)
        {
          rotationMatrix[(4 * c + r)] = m.get(r, c);
        }
      }
      double tmp62_61 = (rotationMatrix[11] = 0.0); rotationMatrix[7] = tmp62_61; rotationMatrix[3] = tmp62_61;
      double tmp86_85 = (rotationMatrix[14] = 0.0); rotationMatrix[13] = tmp86_85; rotationMatrix[12] = tmp86_85;
      
      rotationMatrix[15] = 1.0;      
      return FromRotationMatrix(rotationMatrix);
    }
  
    double getRoll(bool reprojectAxis = true) const
    {
      if (reprojectAxis)
      {
        double fTy  = 2.0f*y;
        double fTz  = 2.0f*z;
        double fTwz = fTz*w;
        double fTxy = fTy*x;
        double fTyy = fTy*y;
        double fTzz = fTz*z;
 
        return atan2(fTxy+fTwz, 1.0f-(fTyy+fTzz));
 
      }
      else
      {
        return atan2(2*(x*y + w*z), w*w + x*x - y*y - z*z);
      }
    }
  
    double getPitch(bool reprojectAxis = true) const
    {
      if (reprojectAxis)
      {
        double fTx  = 2.0f*x;
        double fTz  = 2.0f*z;
        double fTwx = fTx*w;
        double fTxx = fTx*x;
        double fTyz = fTz*y;
        double fTzz = fTz*z;
 
        return atan2(fTyz+fTwx, 1.0f-(fTxx+fTzz));
      }
      else
      {
        return atan2(2*(y*z + w*x), w*w - x*x - y*y + z*z);
      }
    }
  
    double getYaw(bool reprojectAxis = true) const
    {
      if (reprojectAxis)
      {
        double fTx  = 2.0f*x;
        double fTy  = 2.0f*y;
        double fTz  = 2.0f*z;
        double fTwy = fTy*w;
        double fTxx = fTx*x;
        double fTxz = fTz*x;
        double fTyy = fTy*y;
 
        return atan2(fTxz+fTwy, 1.0f-(fTxx+fTyy));
      }
      else
      {
        return asin(-2*(x*z - w*y));
      }
    }
 
    protected:
    private:
 
    public:
      float x;
      float y;
      float z;
      float w;
    protected:
    private:
};
 
#endif