/***************************************************************
 * PrQuaternion.cpp
 *
 * This implements a quaternion class.
 *
 ****************************************************************/

/*
 * modification history
 *----------------------
 *
 * 11/20/97: K.C. Chang: added inline methods.
 * 11/05/97: K.C. Chang: created.
 */

#include "PrQuaternion.h"
#include "PrMatrix3.h"
#include <stdio.h>

#define PR_QUATERNION_EPSILON 0.00001
#define PR_QUATERNION_COS_THRESHHOLD (1.0 - PR_QUATERNION_EPSILON)

int PrQuaternion::operator==(const PrQuaternion &q)
{
    Float mag2 = dot(q);
    return (mag2 >= PR_QUATERNION_COS_THRESHHOLD
	    || mag2 <= -PR_QUATERNION_COS_THRESHHOLD);
}

PrQuaternion PrQuaternion::operator-() const
{
    return PrQuaternion(-w_,
			-v_[0], 
			-v_[1], 
			-v_[2]);
}

void PrQuaternion::negate(PrQuaternion &dest) const
{
    dest.w_ = -w_;
    v_.negate(dest.v_);
}

PrQuaternion PrQuaternion::operator~() const
{
    return PrQuaternion(w_,
			-v_[0], 
			-v_[1], 
			-v_[2]);
}

void PrQuaternion::inverse(PrQuaternion &dest) const
{
    dest.w_ = w_;
    v_.negate(dest.v_);
}

PrQuaternion PrQuaternion::operator+(const PrQuaternion &q) const
{
    return PrQuaternion(w_ + q.w_,
			v_[0] + q.v_[0],
			v_[1] + q.v_[1],
			v_[2] + q.v_[2]);
}

void PrQuaternion::add(const PrQuaternion &q,PrQuaternion &dest) const
{
    dest.w_ = w_ + q.w_;
    v_.add(q.v_,dest.v_);
}

PrQuaternion PrQuaternion::operator-(const PrQuaternion &q) const
{
    return PrQuaternion(w_ - q.w_,
			v_[0] - q.v_[0],
			v_[1] - q.v_[1],
			v_[2] - q.v_[2]);
}

void PrQuaternion::subtract(const PrQuaternion &q,PrQuaternion &dest) const
{
    dest.w_ = w_ - q.w_;
    v_.subtract(q.v_,dest.v_);
}

// [w1,v1]*[w2,v2] = [(w1*w2 - v1 dot v2), (w1*v2+w2*v1+ v1 cross v2)]
PrQuaternion PrQuaternion::operator*(const PrQuaternion &q) const
{
    PrQuaternion tmpQ;
    multiply(q,tmpQ);
    return tmpQ;
}

// 16 mult + 8 add
void PrQuaternion::multiply(const PrQuaternion &q,PrQuaternion &dest) const
{
    PrVector3 tmpV;

    dest.w_ = w_*q.w_ - v_.dot(q.v_);

    q.v_.multiply(w_,dest.v_);
    v_.multiply(q.w_,tmpV);
    dest.v_ += tmpV;
    v_.cross(q.v_,tmpV);
    dest.v_ += tmpV;
}

PrQuaternion PrQuaternion::operator*(Float s) const
{
    return PrQuaternion(w_ * s,
			v_[0] * s,
			v_[1] * s,
			v_[2] * s);
}

void PrQuaternion::multiply(Float s,PrQuaternion &dest) const
{
    dest.w_ = w_*s;
    v_.multiply(s,dest.v_);
}

// [w,v'] = q * [0,v] * ~q  <==> v' = R v
// = [w1,v1]*[0,v2]*[w1,-v1] = [ -v1.v2, w1v2+ v1 x v2]*[w1,-v1]
// = [(-v1.v2)(w1)+(w1v2+ v1 x v2).v1,
//    (v1.v2)v1 + w1(w1v2+ v1 x v2) + v1 x (w1v2+ v1 x v2)]
// since A x (B + C) = A x B + A x C
// = [w, (w1*w1)v2 + 2*w1(v1 x v2) + (v1.v2)v1 + v1 x (v1 x v2)]
// since A x (B x C) = (A dot C)B - (A dot B)C
// = [w, (w1*w1)v2 + 2*w1(v1 x v2) + (v1.v2)v1 + (v1.v2)v1 - (v1.v1)v2]
// = [w, (w1*w1 - v1.v1)v2 + 2*w1(v1 x v2) + 2*(v1.v2)v1]
// v' = [(w1*w1 - v1.v1)v2 + 2*w1(v1 x v2) + 2*(v1.v2)v1]
PrVector3 PrQuaternion::operator*(const PrVector3 &v) const
{
//    return ((*this)*PrQuaternion(0.0,v)*~(*this)).v_;

    PrVector3 tmpV;
    multiply(v,tmpV);
    return tmpV;
}

// 24 mult + 8 add
void PrQuaternion::multiply(const PrVector3 &v,PrVector3 &dest) const
{
    PrVector3 tmpV;
    v.multiply(w_*w_-v_.dot(v_),dest);
    v_.cross(v,tmpV);
    tmpV *= w_+w_;
    dest += tmpV;
    v_.multiply(2*v_.dot(v),tmpV);
    dest += tmpV;
}

PrQuaternion &PrQuaternion::operator+=(const PrQuaternion &q)
{
    w_ += q.w_;
    v_ += q.v_;
    return (*this);
}

PrQuaternion &PrQuaternion::operator-=(const PrQuaternion &q)
{
    w_ -= q.w_;
    v_ -= q.v_;
    return (*this);
}

PrQuaternion &PrQuaternion::operator*=(const PrQuaternion &q)
{
    PrQuaternion tmpQ;
    multiply(q,tmpQ);
    (*this) = tmpQ;
    return (*this);
}

PrQuaternion &PrQuaternion::operator*=(Float s)
{
    w_ *= s;
    v_ *= s;
    return (*this);
}

Float PrQuaternion::dot(const PrQuaternion &q) const
{
    return (w_*q.w_ + v_.dot(q.v_));
}

void PrQuaternion::normalize()
{
    (*this) *= 1.0/sqrt(dot(*this));
}

void PrQuaternion::values(const PrVector3 &axis,Float angle)
{
    w_ = cos(0.5*angle);
    v_ = axis*sin(0.5*angle);
}

void PrQuaternion::axisAngle(PrVector3 &axis, Float &angle) const
{
  if (w_ >= PR_QUATERNION_COS_THRESHHOLD 
      || w_ <= -PR_QUATERNION_COS_THRESHHOLD)
    {
      // no rotation
      angle = 0.0;
      axis.values(0.0,0.0,1.0);
    }
  else
    {
      angle = 2*acos(w_);
      Float magnitude = v_.magnitude();
      assert(magnitude >= PR_QUATERNION_EPSILON);
      axis = v_*(1.0/magnitude);
    }
}

PrMatrix3 PrQuaternion::matrix() const
{
    PrMatrix3 tmpM;
    matrix(tmpM);
    return tmpM;
}

// 27 mult + 12 add
void PrQuaternion::matrix(PrMatrix3 &dest) const
{
    dest[0][0]=1.0 - 2*(v_[1]*v_[1] + v_[2]*v_[2]);
    dest[0][1]=2*(v_[0]*v_[1] - w_*v_[2]);
    dest[0][2]=2*(v_[0]*v_[2] + w_*v_[1]);

    dest[1][0]=2*(v_[0]*v_[1] + w_*v_[2]);
    dest[1][1]=1.0 - 2*(v_[0]*v_[0] + v_[2]*v_[2]);
    dest[1][2]=2*(v_[1]*v_[2] - w_*v_[0]);

    dest[2][0]=2*(v_[0]*v_[2] - w_*v_[1]);
    dest[2][1]=2*(v_[1]*v_[2] + w_*v_[0]);
    dest[2][2]=1.0 - 2*(v_[0]*v_[0] + v_[1]*v_[1]);
}
/*
PrVector3 PrQuaternion::thirdColumn() const
{
    PrVector3 tmpV;
    thirdColumn(tmpV);
    return tmpV;
}

void PrQuaternion::thirdColumn(PrVector3 &dest) const
{
    dest[0] = 2*(v_[0]*v_[2] + w_*v_[1]);
    dest[1] = 2*(v_[1]*v_[2] - w_*v_[0]);
//    dest[2] = w_*w_ - v_[0]*v_[0] - v_[1]*v_[1] + v_[2]*v_[2];
    dest[2] = 1.0 - 2*(v_[0]*v_[0] + v_[1]*v_[1]);
}
*/
PrVector3 PrQuaternion::column(int col) const
{
    PrVector3 tmpV;
    column(col,tmpV);
    return tmpV;
}

void PrQuaternion::column(int col,PrVector3 &dest) const
{
  if (col == 2)
    {
      dest[0] = 2*(v_[0]*v_[2] + w_*v_[1]);
      dest[1] = 2*(v_[1]*v_[2] - w_*v_[0]);
      //    dest[2] = w_*w_ - v_[0]*v_[0] - v_[1]*v_[1] + v_[2]*v_[2];
      dest[2] = 1.0 - 2*(v_[0]*v_[0] + v_[1]*v_[1]);
    }
  else if (col == 1)
    {
      dest[0]=2*(v_[0]*v_[1] - w_*v_[2]);
      dest[1]=1.0 - 2*(v_[0]*v_[0] + v_[2]*v_[2]);
      dest[2]=2*(v_[1]*v_[2] + w_*v_[0]);
    }
  else // if (col == 0)
    {
      dest[0]=1.0 - 2*(v_[1]*v_[1] + v_[2]*v_[2]);
      dest[1]=2*(v_[0]*v_[1] + w_*v_[2]);
      dest[2]=2*(v_[0]*v_[2] - w_*v_[1]);
    }
}
void PrQuaternion::display(char *name)
{
    if (name != NULL)
      printf("%s =\n",name);

    printf("[%.6f, ",w_);
    v_.display();
    printf("]");
}

// dPhi = Einv ( q - qd) = -2*q_tilde^T qd <-- since (q_tilde^T q) = 0
// E = 0.5*q_tilde
// Einv = (EtE)inv Et = 4Et = 2*qtilde^T <-- since (q_tilde^T q_tilde) = I
/*
  // Featherstone, Mirtich
   q_tilde = [ -x -y -z
                w -z  y
                z  w -x
               -y  x  w ] 
   q_tilde^T = [ -x  w  z -y
                 -y -z  w  x
                 -z  y -x  w ] 	       
 // Khatib
   q_tilde = [ -x -y -z
                w  z -y
               -z  w  x
                y -x  w ] 
   q_tilde^T = [ -x  w -z  y
                 -y  z  w -x
                 -z -y  x  w ] 	       
*/
void PrQuaternion::angularError(const PrQuaternion &qd,PrVector3 &dPhi) const
{
  // Khtib
  dPhi[0] = -2*(-v_[0]*qd.w_ +w_*qd.v_[0] -v_[2]*qd.v_[1] +v_[1]*qd.v_[2]);
  dPhi[1] = -2*(-v_[1]*qd.w_ +v_[2]*qd.v_[0] +w_*qd.v_[1] -v_[0]*qd.v_[2]);
  dPhi[2] = -2*(-v_[2]*qd.w_ -v_[1]*qd.v_[0] +v_[0]*qd.v_[1] +w_*qd.v_[2]);
  /*
    // Featherstone, Mirtich
  dPhi[0] = -2*(-v_[0]*qd.w_ +w_*qd.v_[0] +v_[2]*qd.v_[1] -v_[1]*qd.v_[2]);
  dPhi[1] = -2*(-v_[1]*qd.w_ -v_[2]*qd.v_[0] +w_*qd.v_[1] +v_[0]*qd.v_[2]);
  dPhi[2] = -2*(-v_[2]*qd.w_ +v_[1]*qd.v_[0] -v_[0]*qd.v_[1] +w_*qd.v_[2]);
  */
}

// dq = E omega
// dq = 0.5 q_tilde omega
void PrQuaternion::velocity(const PrVector3 &omega,PrQuaternion &dq) const
{
  // Khtib
  dq.values(-v_[0]*omega[0] -v_[1]*omega[1] -v_[2]*omega[2], 
	    +   w_*omega[0] +v_[2]*omega[1] -v_[1]*omega[2], 
	    -v_[2]*omega[0] +   w_*omega[1] +v_[0]*omega[2], 
	    +v_[1]*omega[0] -v_[1]*omega[1] +   w_*omega[2]);
  /*
    // Featherstone, Mirtich
  dq.values(-v_[0]*omega[0] -v_[1]*omega[1] -v_[2]*omega[2], 
	    +   w_*omega[0] -v_[2]*omega[1] +v_[1]*omega[2], 
	    +v_[2]*omega[0] +   w_*omega[1] -v_[0]*omega[2], 
	    -v_[1]*omega[0] +v_[1]*omega[1] +   w_*omega[2]);
  */
}

