#include "SM_MathPCH.h"
#include <minmax.h>

// Codigo basado en el de Dave Eberly... BOFA!!! XD

static float epsilon = 1e-03f;  // cutoff for sin(angle) near zero
static float pi = 4.0f*float(atan(1.0f));

Quaternion Quaternion::ZERO(0.0f,0.0f,0.0f,0.0f);
Quaternion Quaternion::IDENTITY(1.0f,0.0f,0.0f,0.0f);

// Nombre      : Quaternion::Quaternion()
// Parametros  : float _w, float _x, float _y, float _z
// Retorno     : No
// Descripcion : Constructor de un cuaternion
Quaternion::Quaternion(float _w, float _x, float _y, float _z)
{
    w = _w;
    x = _x;
    y = _y;
    z = _z;
}

// Nombre      : Quaternion::Quaternion()
// Parametros  : const Quaternion& q
// Retorno     : No
// Descripcion : Constructor
Quaternion::Quaternion(const Quaternion& q)
{
    w = q.w;
    x = q.x;
    y = q.y;
    z = q.z;
}

// Nombre      : Quaternion::Quaternion()
// Parametros  : const float& fAngle, const Vector3D& v3dAxis
// Retorno     : Quaternion
// Descripcion : Construye un cuaternion a partir de un angulo y un vector
Quaternion::Quaternion(const float& fAngle, const Vector3D& v3dAxis)
{
  FromAngleAxis(fAngle, v3dAxis);
}

// Nombre      : Quaternion::operator==()
// Parametros  : const Quaternion& q
// Retorno     : 1: Igual : 0: No igual
// Descripcion : Operador de desigualdad
int Quaternion::operator==(const Quaternion& q) const
{
  return ( w==q.w && x==q.x && y==q.y && z==q.z);
}

// Nombre      : Quaternion::operator!=()
// Parametros  : const Quaternion& q
// Retorno     : 1: Desigual ; 0: Igual
// Descripcion : Operador desigualdad
int Quaternion::operator!=(const Quaternion& q) const
{
  return ( w!=q.w || x!=q.x || y!=q.y || z!=q.z);
}


void Quaternion::FromFrame(Vector3D& v3dVPN, Vector3D& v3dUp, Vector3D& v3dRight)
{
  float R[3][3];

  R[0][0]=v3dRight.x; R[0][1]=v3dRight.y; R[0][2]=v3dRight.z;
  R[1][0]=v3dUp.x; R[1][1]=v3dUp.y; R[1][2]=v3dUp.z;
  R[2][0]=v3dVPN.x; R[2][1]=v3dVPN.y; R[2][2]=v3dVPN.z;

  FromRotationMatrix(R);
}

// Nombre      : Quaternion::FromRotationMatrix ()
// Parametros  : const float R[3][3]
// Retorno     : No
// Descripcion : Crea un cuaternion desde una matriz de rotacion 3x3
void Quaternion::FromRotationMatrix (const float R[3][3])
{
    // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
    // article "Quaternion Calculus and Fast Animation".

    float trace = R[0][0]+R[1][1]+R[2][2];
    float root;

    if ( trace > 0.0f )
    {
        // |w| > 1/2, may as well choose w > 1/2
        root = float(sqrt(trace+1.0f));  // 2w
        w = 0.5f*root;
        root = 0.5f/root;  // 1/(4w)
        x = (R[2][1]-R[1][2])*root;
        y = (R[0][2]-R[2][0])*root;
        z = (R[1][0]-R[0][1])*root;
    }
    else
    {
        // |w| <= 1/2
        static int next[3] = { 1, 2, 0 };
        int i = 0;
        if ( R[1][1] > R[0][0] )
            i = 1;
        if ( R[2][2] > R[i][i] )
            i = 2;
        int j = next[i];
        int k = next[j];

        root = float(sqrt(R[i][i]-R[j][j]-R[k][k]+1.0f));
        float* quat[3] = { &x, &y, &z };
        *quat[i] = 0.5f*root;
        root = 0.5f/root;
        w = (R[k][j]-R[j][k])*root;
        *quat[j] = (R[j][i]+R[i][j])*root;
        *quat[k] = (R[k][i]+R[i][k])*root;
    }
}


// Nombre      : Quaternion::ToRotationMatrix()
// Parametros  : float R[3][3]
// Retorno     : No
// Descripcion : Convierte el cuaternion a la matriz de rotacion pasada como parametro
void Quaternion::ToRotationMatrix(float R[3][3]) const
{
    float tx  = 2.0f*x;
    float ty  = 2.0f*y;
    float tz  = 2.0f*z;
    float twx = tx*w;
    float twy = ty*w;
    float twz = tz*w;
    float txx = tx*x;
    float txy = ty*x;
    float txz = tz*x;
    float tyy = ty*y;
    float tyz = tz*y;
    float tzz = tz*z;

    R[0][0] = 1.0f-(tyy+tzz);
    R[1][0] = txy-twz;
    R[2][0] = txz+twy;
    R[0][1] = txy+twz;
    R[1][1] = 1.0f-(txx+tzz);
    R[2][1] = tyz-twx;
    R[0][2] = txz-twy;
    R[1][2] = tyz+twx;
    R[2][2] = 1.0f-(txx+tyy);
}


// Nombre      : Quaternion::FromAngleAxis()
// Parametros  : const float& angle, const float& ax, const float& ay, const float& az
// Retorno     : No
// Descripcion : Crea un cuaternion desde un angulo y un eje    
void Quaternion::FromAngleAxis(const float& angle, const float& ax,
                                const float& ay, const float& az)
{
    // assert:  axis[] is unit length
    //
  // The quaternion representing the rotation is
  //   q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)

    float halfAngle = 0.5f*angle;
    float sn = float(sin(halfAngle));
    w = float(cos(halfAngle));
    x = sn*ax;
    y = sn*ay;
    z = sn*az;
}

// Nombre      : Quaternion::ToAngleAxis()
// Parametros  : float& angle, float& ax, float& ay, float& az
// Retorno     : void
// Descripcion : Convierte un cuaternion a un angulo y eje
void Quaternion::ToAngleAxis(float& angle, float& ax, float& ay,
                              float& az) const
{
  // The quaternion representing the rotation is
  //   q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)

    float length2 = x*x+y*y+z*z;
    if ( length2 > 0.0f )
    {
        angle = 2.0f*float(acos(w));
        float invlen = 1.0f/float(sqrt(length2));
        ax = x*invlen;
        ay = y*invlen;
        az = z*invlen;
    }
    else
    {
        // angle is 0 (mod 2*pi), so any axis will do
        angle = 0;
        ax = 1.0f;
        ay = 0.0f;
        az = 0.0f;
    }
}

// Nombre      : Quaternion::FromAngleAxis()
// Parametros  : const float& angle, const Vector3D& v3dAxis
// Retorno     : No
// Descripcion : Crea un cuaternion desde un angulo y un eje
void Quaternion::FromAngleAxis(const float& angle, const Vector3D& v3dAxis)
{
  FromAngleAxis(angle, v3dAxis.x, v3dAxis.y, v3dAxis.z);
}

// Nombre      : Quaternion::ToAngleAxis()
// Parametros  : float& angle, Vector3D& v3dAxis
// Retorno     : No
// Descripcion : Convierte de cuaternion a un angulo y a un eje
void Quaternion::ToAngleAxis(float& angle, Vector3D& v3dAxis) const
{
  ToAngleAxis(angle, v3dAxis.x, v3dAxis.y, v3dAxis.z);
}

// Nombre      : Quaternion::operator=()
// Parametros  : const Quaternion& q
// Retorno     : Quaternion
// Descripcion : Asignacion de cuaterniones
Quaternion& Quaternion::operator=(const Quaternion& q)
{
    w = q.w;
    x = q.x;
    y = q.y;
    z = q.z;

    return *this;
}

// Nombre      : Quaternion::operator+((
// Parametros  : const Quaternion& q
// Retorno     : Quaternion 
// Descripcion : Suma de cuaterniones
Quaternion Quaternion::operator+(const Quaternion& q) const
{
    return Quaternion(w+q.w,x+q.x,y+q.y,z+q.z);
}

// Nombre      : Quaternion::operator-()
// Parametros  : const Quaternion& q
// Retorno     : Quaternion
// Descripcion : Resta de cuaterniones
Quaternion Quaternion::operator-(const Quaternion& q) const
{
    return Quaternion(w-q.w,x-q.x,y-q.y,z-q.z);
}

// Nombre      : Quaternion::operator*()
// Parametros  : const Quaternion& q
// Retorno     : Quaternion
// Descripcion : Producto de un cuaternion por otro
Quaternion Quaternion::operator*(const Quaternion& q) const
{
    // NOTE:  Multiplication is not generally commutative, so in most
    // cases p*q != q*p.

#if 1
    return Quaternion
    (
    w*q.w-x*q.x-y*q.y-z*q.z,
    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
    );
#else
    // The above code uses 16 multiplications and 12 additions.  The code
    // below uses 12 multiplications (you might be able to avoid the
    // divisions by 2 and manipulate the floating point exponent directly)
    // and 28 additions.  For an architecture where multiplications are more
    // expensive than additions, you should do the cycle count and consider
    // using the code below.  On an Intel Pentium, multiplications and
    // additions cost the same, so the code below is slower than the code
    // above.

    float A = (w+x)*(q.w+q.x);
    float B = (z-y)*(q.y-q.z);
    float C = (x-w)*(q.y+q.z);
    float D = (y+z)*(q.x-q.w);
    float E = (x+z)*(q.x+q.y);
    float F = (x-z)*(q.x-q.y);
    float G = (w+y)*(q.w-q.z);
    float H = (w-y)*(q.w+q.z);

    float EpF = E+F, EmF = E-F;
    float GpH = G+H, GmH = G-H;

    return Quaternion
    (
         B+0.5f*(GpH-EpF),
         A-0.5f*(GpH+EpF),
        -C+0.5f*(EmF+GmH),
        -D+0.5f*(EmF-GmH)
    );
#endif
}

// Nombre      : Quaternion::operator*()
// Parametros  : float c
// Retorno     : Quaternion
// Descripcion : Producto de un cuaternion por un escalar
Quaternion Quaternion::operator*(float c) const
{
    return Quaternion(c*w,c*x,c*y,c*z);
}

// Nombre      : operator* (
// Parametros  : float c, const Quaternion& q
// Retorno     : Quaternion 
// Descripcion : Procucto por un escalar (escalar preoperando)
Quaternion operator* (float c, const Quaternion& q)
{
    return Quaternion(c*q.w,c*q.x,c*q.y,c*q.z);
}

// Nombre      : Quaternion::operator-
// Parametros  : No
// Retorno     : Quaternion  
// Descripcion : Inversa respecto a suma de un cuaternion
Quaternion Quaternion::operator- () const
{
    return Quaternion(-w,-x,-y,-z);
}

// Nombre      : Quaternion::Dot()
// Parametros  : const Quaternion& q
// Retorno     : float
// Descripcion : Producto escalar de un cuaternion
float Quaternion::Dot(const Quaternion& q) const
{
    return w*q.w+x*q.x+y*q.y+z*q.z;
}

// Nombre      : Quaternion::Norm()
// Parametros  : No
// Retorno     : float
// Descripcion : Norma de un cuaternion 
float Quaternion::Norm() const
{
    return w*w+x*x+y*y+z*z;
}

// Nombre      : Quaternion::Inverse()
// Parametros  : No
// Retorno     : Quaternion
// Descripcion : Inversa de un cuaternion. (Conjugado dividido entre cuadrado modulo)
Quaternion Quaternion::Inverse() const
{
    float norm = w*w+x*x+y*y+z*z;
    if ( norm > 0.0f )
    {
        norm = 1.0f/norm;
        return Quaternion(w*norm,-x*norm,-y*norm,-z*norm);
    }
    else
    {
        // return an invalid result to flag the error
        return ZERO;
    }
}

// Nombre      : Quaternion::UnitInverse()
// Parametros  : No
// Retorno     : Quaternion
// Descripcion : Inversa de un quaternion unitario (su conjugado)
Quaternion Quaternion::UnitInverse() const
{
    // assert:  'this' is unit length
    return Quaternion(w,-x,-y,-z);
}

// Nombre      : Quaternion::Exp()
// Parametros  : No
// Retorno     : Quaternion
// Descripcion : 
Quaternion Quaternion::Exp() const
{
    // If q = A*(x*i+y*j+z*k) where (x,y,z) is unit length, then
    // exp(q) = cos(A)+sin(A)*(x*i+y*j+z*k).  If sin(A) is near zero,
    // use exp(q) = cos(A)+A*(x*i+y*j+z*k) since A/sin(A) has limit 1.

    float angle = float(sqrt(x*x+y*y+z*z));
    float sn = float(sin(angle));

    Quaternion result;
    result.w = float(cos(angle));

    if ( fabs(sn) >= epsilon )
    {
        float coeff = sn/angle;
        result.x = coeff*x;
        result.y = coeff*y;
        result.z = coeff*z;
    }
    else
    {
        result.x = x;
        result.y = y;
        result.z = z;
    }

    return result;
}

// Nombre      : Quaternion::Log()
// Parametros  : No
// Retorno     : Quaternion
// Descripcion : Lograitmo de un cuaternion
Quaternion Quaternion::Log() const
{
    // If q = cos(A)+sin(A)*(x*i+y*j+z*k) where (x,y,z) is unit length, then
    // log(q) = A*(x*i+y*j+z*k).  If sin(A) is near zero, use log(q) =
    // sin(A)*(x*i+y*j+z*k) since sin(A)/A has limit 1.

    Quaternion result;
    result.w = 0.0f;

    if ( fabs(w) < 1.0f )
    {
        float angle = float(acos(w));
        float sn = float(sin(angle));
        if ( fabs(sn) >= epsilon )
        {
            float coeff = angle/sn;
            result.x = coeff*x;
            result.y = coeff*y;
            result.z = coeff*z;
            return result;
        }
    }

    result.x = x;
    result.y = y;
    result.z = z;

    return result;
}

// Nombre      : Quaternion::operator*()
// Parametros  : const Vector3D& pt
// Retorno     : Vector3D
// Descripcion : operador rotacion de vectores
Vector3D Quaternion::operator*(const Vector3D& pt) const
{
    // Given a vector u = (x0,y0,z0) and a unit length quaternion
    // q = <w,x,y,z>, the vector v = (x1,y1,z1) which represents the
    // rotation of u by q is v = q*u*q^{-1} where * indicates quaternion
    // multiplication and where u is treated as the quaternion <0,x0,y0,z0>.
    // Note that q^{-1} = <w,-x,-y,-z>, so no real work is required to
    // invert q.  Now
    //
    //   q*u*q^{-1} = q*<0,x0,y0,z0>*q^{-1}
    //     = q*(x0*i+y0*j+z0*k)*q^{-1}
    //     = x0*(q*i*q^{-1})+y0*(q*j*q^{-1})+z0*(q*k*q^{-1})
    //
    // As 3-vectors, q*i*q^{-1}, q*j*q^{-1}, and 2*k*q^{-1} are the columns
    // of the rotation matrix computed in Quaternion::ToRotationMatrix.  The
    // vector v is obtained as the product of that rotation matrix with
    // vector u.  As such, the quaternion representation of a rotation
    // matrix requires less space than the matrix and more time to compute
    // the rotated vector.  Typical space-time tradeoff...

    float R[3][3];
    ToRotationMatrix(R);

    Vector3D result;
    result.x = R[0][0]*pt.x+R[0][1]*pt.y+R[0][2]*pt.z;
    result.y = R[1][0]*pt.x+R[1][1]*pt.y+R[1][2]*pt.z;
    result.z = R[2][0]*pt.x+R[2][1]*pt.y+R[2][2]*pt.z;

    return result;
}

// Nombre      : Quaternion::Slerp()
// Parametros  : float t, const Quaternion& p, const Quaternion& q
// Retorno     : Quaternion
// Descripcion : Da la interpolacion lineal esferica
Quaternion Quaternion::Slerp(float t, const Quaternion& p,
    const Quaternion& q)
{

  // eberly version wrong?

  Quaternion retQuat, tempQuat;
	float  omega, cosOmega, sinOmega, sclq1, sclq2;

  cosOmega = p.Dot(q);

  if((1.0f - cosOmega) > 0.001f)
	{
		omega	 = acosf(cosOmega);
		sinOmega = sinf(omega);
		sclq1	 = sinf((1.0f - t) * omega) / sinOmega;
		sclq2	 = sinf(t * omega) / sinOmega;
	}
	else
	{
		sclq1 = 1.0f - t;
		sclq2 = t;
	}

	retQuat = p * sclq1 + q * sclq2;	

	return retQuat;

  /*
    Quaternion q2;


    // assert:  p.Dot(q) >= 0 (obtained by preprocessing input)


    float cs = p.Dot(q);
    float sn = float(sqrt(fabs(1-cs*cs)));
    if ( fabs(sn) < epsilon )
        return p;

    float angle = float(atan2(sn,cs));
    float invSn = 1.0f/sn;
    float c0 = float(sin((1-t)*angle)*invSn);
    float c1 = float(sin(t*angle)*invSn);

    return c0*p + c1*q;
    */
    
}

// Nombre      : Quaternion::Intermediate()
// Parametros  : const Quaternion& q0, const Quaternion& q1,
//        const Quaternion& q2, Quaternion& a, Quaternion& b
// Retorno     : No 
// Descripcion : Setup de interpolacion esferica cuadratica
void Quaternion::Intermediate(const Quaternion& q0, const Quaternion& q1,
    const Quaternion& q2, Quaternion& a, Quaternion& b)
{
    // assert:  q0, q1, q2 are unit quaternions

    Quaternion q0inv = q0.UnitInverse();
    Quaternion q1inv = q1.UnitInverse();
    Quaternion p0 = q0inv*q1;
    Quaternion p1 = q1inv*q2;
    Quaternion arg = 0.25*(p0.Log()-p1.Log());
    Quaternion marg = -arg;

    a = q1*arg.Exp();
    b = q1*marg.Exp();
}

// Nombre      : Quaternion::Squad()
// Parametros  : float t, const Quaternion& p,
//               const Quaternion& a, const Quaternion& b, const Quaternion& q
// Retorno     : Quaternion
// Descripcion : Interpolacion cuadratica esferica de cuaterniones
Quaternion Quaternion::Squad(float t, const Quaternion& p,
    const Quaternion& a, const Quaternion& b, const Quaternion& q)
{
    return Slerp(2*t*(1-t),Slerp(t,p,q),Slerp(t,a,b));
}


// Nombre      : Quaternion::Normalize
// Parametros  : 
// Retorno     : 
// Descripcion : Normalización del quaternion.
Quaternion &Quaternion::Normalize ()
{
  //FIXME: Seguro que se puede optimizar. O:)
  float   matrix[3][3];
  float   fMod;
  int     i;

  ToRotationMatrix (matrix);
  for (i = 0; i < 3; i++)
  {
    fMod =  matrix[i][0] * matrix[i][0];
    fMod += matrix[i][1] * matrix[i][1];
    fMod += matrix[i][2] * matrix[i][2];

    fMod = (float)sqrt (fMod);
    matrix[i][0] /= fMod;
    matrix[i][1] /= fMod;
    matrix[i][2] /= fMod;
  }

  FromRotationMatrix (matrix);


  return *this;
}

float Quaternion::Angle(Quaternion& q0, Quaternion& q1)
{
  q0.Normalize();
  q1.Normalize();

  double dDot=double(q0.x)*double(q1.x)+double(q0.y)*double(q1.y)+double(q0.z*q1.z)+double(q0.w*q1.w);
  double d=min(1.0, max(-1.0f, dDot));
  return ((float)acos(fabs(d)));
}