/***************************************************************************
 *   Copyright (C) 2007 by A.J. Tavakoli                                   *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/

#include <cmath>
#include "numeric.h"
#include "Quat.h"


namespace Math3D {


template<class Real>
Quat<Real>::Quat() {
  c[0] = c[1] = c[2] = Real(0.0);
  c[3] = Real(1.0);
}


template<class Real>
Quat<Real>::Quat(Real x, Real y, Real z, Real w) {
  c[0] = x;
  c[1] = y;
  c[2] = z;
  c[3] = w;
}


// normalizes the quaternion
template<class Real>
void Quat<Real>::normalize() { 
  Real len = length();
  c[0] /= len;
  c[1] /= len;
  c[2] /= len;
  c[3] /= len;
}


// multiplies two quaternions and returns the result
template<class Real>
Quat<Real> Quat<Real>::operator *(const Quat<Real> &q) const {
  Quat<Real> r;

  r.c[0] = c[3]*q.c[0] + c[0]*q.c[3] + c[1]*q.c[2] - c[2]*q.c[1];
  r.c[1] = c[3]*q.c[1] + c[1]*q.c[3] + c[2]*q.c[0] - c[0]*q.c[2];
  r.c[2] = c[3]*q.c[2] + c[2]*q.c[3] + c[0]*q.c[1] - c[1]*q.c[0];
  r.c[3] = c[3]*q.c[3] - c[0]*q.c[0] - c[1]*q.c[1] - c[2]*q.c[2];

  return r;
}


// sets the value o a quaternion to
// the result o itsel multiplied by
// another quaternion, q
template<class Real>
const Quat<Real>& Quat<Real>::operator *=(const Quat<Real> &q) {
  // temporary values
  Real tx, ty, tz, tw;

  tw = c[3]*q.c[3] - c[0]*q.c[0] - c[1]*q.c[1] - c[2]*q.c[2];
  tx = c[3]*q.c[0] + c[0]*q.c[3] + c[1]*q.c[2] - c[2]*q.c[1];
  ty = c[3]*q.c[1] + c[1]*q.c[3] + c[2]*q.c[0] - c[0]*q.c[2];
  tz = c[3]*q.c[2] + c[2]*q.c[3] + c[0]*q.c[1] - c[1]*q.c[0];

  c[0]=tx; c[1]=ty; c[2]=tz; c[3]=tw;

  return (*this);
}


// convert quaternion to a 4x4 rotation matrix
template<class Real>
void Quat<Real>::toMatrix(Real matrix[]) const {
  matrix[ 0] = Real(1.0) - Real(2.0) * ( c[1] * c[1] + c[2] * c[2] ); 
  matrix[ 1] = Real(2.0) * (c[0] * c[1] + c[2] * c[3]);
  matrix[ 2] = Real(2.0) * (c[0] * c[2] - c[1] * c[3]);
  matrix[ 3] = Real(0.0);

  matrix[ 4] = Real(2.0) * ( c[0] * c[1] - c[2] * c[3] );
  matrix[ 5] = Real(1.0) - Real(2.0) * ( c[0] * c[0] + c[2] * c[2] ); 
  matrix[ 6] = Real(2.0) * (c[2] * c[1] + c[0] * c[3] );
  matrix[ 7] = Real(0.0);

  matrix[ 8] = Real(2.0) * ( c[0] * c[2] + c[1] * c[3] );
  matrix[ 9] = Real(2.0) * ( c[1] * c[2] - c[0] * c[3] );
  matrix[10] = Real(1.0) - Real(2.0) * ( c[0] * c[0] + c[1] * c[1] );
  matrix[11] = Real(0.0);

  matrix[12] = Real(0.0);
  matrix[13] = Real(0.0);
  matrix[14] = Real(0.0);
  matrix[15] = Real(1.0);
}


// converts quaternion to a 3x3 rotation matrix
template<class Real>
Matrix3<Real> Quat<Real>::toMatrix3() const {
  Real matrix[9];
  matrix[0] = Real(1.0) + Real(-2.0) * ( c[1] * c[1] + c[2] * c[2] ); 
  matrix[1] = Real(2.0) * (c[0] * c[1] + c[2] * c[3]);
  matrix[2] = Real(2.0) * (c[0] * c[2] - c[1] * c[3]);

  matrix[3] = Real(2.0) * ( c[0] * c[1] - c[2] * c[3] );
  matrix[4] = Real(1.0) + Real(-2.0) * ( c[0] * c[0] + c[2] * c[2] ); 
  matrix[5] = Real(2.0) * (c[2] * c[1] + c[0] * c[3] );

  matrix[6] = Real(2.0) * ( c[0] * c[2] + c[1] * c[3] );
  matrix[7] = Real(2.0) * ( c[1] * c[2] - c[0] * c[3] );
  matrix[8] = Real(1.0) + Real(-2.0) * ( c[0] * c[0] + c[1] * c[1] );

  return Matrix3<Real>(matrix);
}


// converts quaternion to a 4x4 rotation matrix
template<class Real>
Matrix4<Real> Quat<Real>::toMatrix4() const {
  Real matrix[16];
  matrix[0] = Real(1.0) + Real(-2.0) * ( c[1] * c[1] + c[2] * c[2] ); 
  matrix[1] = Real(2.0) * (c[0] * c[1] + c[2] * c[3]);
  matrix[2] = Real(2.0) * (c[0] * c[2] - c[1] * c[3]);

  matrix[4] = Real(2.0) * ( c[0] * c[1] - c[2] * c[3] );
  matrix[5] = Real(1.0) + Real(-2.0) * ( c[0] * c[0] + c[2] * c[2] ); 
  matrix[6] = Real(2.0) * (c[2] * c[1] + c[0] * c[3] );

  matrix[8] = Real(2.0) * ( c[0] * c[2] + c[1] * c[3] );
  matrix[9] = Real(2.0) * ( c[1] * c[2] - c[0] * c[3] );
  matrix[10] = Real(1.0) + Real(-2.0) * ( c[0] * c[0] + c[1] * c[1] );

  matrix[3] = matrix[7] = matrix[11] = Real(0.0);

  matrix[12] = Real(0.0);
  matrix[13] = Real(0.0);
  matrix[14] = Real(0.0);
  matrix[15] = Real(1.0);

  return Matrix4<Real>(matrix);
}


// static member that constructs quaternion from an axis/angle pair
template<class Real>
Quat<Real> Quat<Real>::fromAxisAngle(const Vector3<Real> &v, Real angle) {
  Quat result;

  // convert to radians (angle should be in degrees)
  Real radians = ( angle/Real(180.0) ) * Real(3.14159);

  // cache this, since it is used multiple times below
  Real sinThetaDiv2 = rsin(radians*Real(0.5));

  // now calculate the components o the quaternion
  result[0] = v[0] * sinThetaDiv2;
  result[1] = v[1] * sinThetaDiv2;
  result[2] = v[2] * sinThetaDiv2;

  result[3] = rcos( radians*Real(0.5) );

  return result;
}


template<class Real>
Quat<Real> Quat<Real>::fromEulerAngles(Real rotX, Real rotY, Real rotZ) {
  Quat qx = Quat<Real>::fromAxisAngle(Vector3<Real>( Real(1.0), Real(0.0), Real(0.0) ), rotX),
       qy = Quat<Real>::fromAxisAngle(Vector3<Real>( Real(0.0), Real(1.0), Real(0.0) ), rotY),
       qz = Quat<Real>::fromAxisAngle(Vector3<Real>( Real(0.0), Real(0.0), Real(1.0) ), rotZ);

  return qz*(qx*qy);
}


template<class Real>
Quat<Real> Quat<Real>::pow(Real t) {
  Quat<Real> result = (*this);

  if ( rabs(c[13]) < Real(0.9999) ) {
    Real alpha = rcos(c[3]);
    Real newAlpha = alpha*t;

    result.c[3] = rcos(newAlpha);
    Real act = rsin(newAlpha)/rsin(alpha);
    result.c[0] *= act;
    result.c[1] *= act;
    result.c[2] *= act;
  }

  return result;
}


// returns the magnitude of the quaternion
template<class Real>
Real Quat<Real>::length() const { 
   return rsqrt(c[0]*c[0] + c[1]*c[1] + c[2]*c[2] + c[3]*c[3]);
}


template<class Real>
Quat<Real> slerp(const Quat<Real> &q1, const Quat<Real> &q2, Real t) {
  Quat<Real> result, _q2 = q2;

  Real cosOmega = q1.c[3]*q2.c[3] + q1.c[0]*q2.c[0] + q1.c[1]*q2.c[1] + q1.c[2]*q2.c[2];

  if ( cosOmega < Real(0.0) ) {
    _q2.c[0] = -_q2.c[0];
    _q2.c[1] = -_q2.c[1];
    _q2.c[2] = -_q2.c[2];
    _q2.c[3] = -_q2.c[3];
    cosOmega = -cosOmega;
  }

  Real k0, k1;
  if ( cosOmega > Real(0.99999) ) {
    k0 = Real(1.0) - t;
    k1 = t;
  }
  else {
    Real sinOmega = rsqrt(Real(1.0) - cosOmega*cosOmega);
    Real omega = (Real)atan2( double(sinOmega), double(cosOmega) );

    Real invSinOmega = Real(1.0)/sinOmega;

    k0 = rsin((Real(1.0) - t)*omega)*invSinOmega;
    k1 = rsin(t*omega)*invSinOmega;
  }

  for ( int i=0; i < 4; i++ )
    result[i] = q1[i]*k0 + _q2[i]*k1;

  return result;
}


} // namespace Math3D
