#include "c4d_quaternion.h"
#ifndef __API_INTERN__
	#include "c4d_tools.h"
#endif
#include "ge_math.h"

#define EPSILON 0.00001

Quaternion::Quaternion()
{
	w = 0.0;
	v = LVector(1,0,0);
}

void Quaternion::SetAxis(const LVector &ax, const LReal ww)
{
	v = (!ax)*Sin(0.5*ww);
	w = Cos(0.5*ww);
}

void Quaternion::SetMatrix(const LMatrix &_m)
{
	LMatrix m=_m;

	m.v1 = !m.v1;
	m.v2 = !m.v2;
	m.v3 = !m.v3;

  LReal trace = m.v1.x + m.v2.y + m.v3.z;

  if (trace>0.0)
	{
    LReal s = 0.5 / LSqrt(trace+1.0);
    w = 0.25 / s;
    v.x = m.v2.z - m.v3.y;
    v.y = m.v3.x - m.v1.z;
    v.z = m.v1.y - m.v2.x;

		v *= s;
		v = -v;
  }
	else
	{
    if ( (m.v1.x > m.v2.y) && (m.v1.x > m.v3.z) )
		{
      LReal s = 2.0 * LSqrt( 1.0 + m.v1.x - m.v2.y - m.v3.z);
      v.x = 0.25 * s;
      v.y = (m.v2.x + m.v1.y ) / s;
      v.z = (m.v3.x + m.v1.z ) / s;
      w = (m.v3.y - m.v2.z ) / s;

    }
		else if (m.v2.y > m.v3.z)
		{
      LReal s = 2.0 * LSqrt( 1.0 + m.v2.y - m.v1.x - m.v3.z);
      v.x = (m.v2.x + m.v1.y ) / s;
      v.y = 0.25 * s;
      v.z = (m.v3.y + m.v2.z ) / s;
      w = -(m.v3.x - m.v1.z ) / s;
    }
		else
		{
      LReal s = 2.0 * LSqrt( 1.0 + m.v3.z - m.v1.x - m.v2.y );
      v.x = (m.v3.x + m.v1.z ) / s;
      v.y = (m.v3.y + m.v2.z ) / s;
      v.z = 0.25 * s;
      w = (m.v2.x - m.v1.y ) / s;
    }
  }
}

LMatrix Quaternion::GetMatrix(void)
{
	LMatrix m;

	m.off = LVector(0,0,0);
	m.v1  = LVector(1.0-2.0*(v.y*v.y+v.z*v.z),2.0*(v.x*v.y-w*v.z)      ,2.0*(v.x*v.z+w*v.y));
	m.v2  = LVector(2.0*(v.x*v.y+w*v.z)      ,1.0-2.0*(v.x*v.x+v.z*v.z),2.0*(v.y*v.z-w*v.x));
	m.v3  = LVector(2.0*(v.x*v.z-w*v.y)      ,2.0*(v.y*v.z+w*v.x)      ,1.0-2.0*(v.x*v.x+v.y*v.y));

	return m;
}

Quaternion QPlus(const Quaternion &q1, const Quaternion &q2)
{
	Quaternion r;

  r.w = q1.w+q2.w;
	r.v = q1.v+q2.v;
	return r;
}

Quaternion QMul(const Quaternion &q1, const Quaternion &q2)
{
	Quaternion r;

  r.w = (q1.w*q2.w) - (q1.v*q2.v);
	r.v = q1.w*q2.v + q2.w*q1.v + (q1.v%q2.v);
	return r;
}

Quaternion QMul(const Quaternion &q, LReal s)
{
	Quaternion r=q;

	r.w *= s;
	r.v *= s;

	return r;
}


Quaternion QInvert(const Quaternion &q)
{
	Quaternion r;

  r.w = -q.w;
	r.v =  q.v;

	return r;
}


Quaternion QDeriv(const Quaternion &q, const LVector &w)
{
	Quaternion r;

  r.w   = 0.5 * (-q.v * w);
  r.v.x = 0.5 * ( q.w   * w.x - q.v.z * w.y + q.v.y * w.z);
  r.v.y = 0.5 * ( q.v.z * w.x + q.w   * w.y - q.v.x * w.z);
  r.v.z = 0.5 * (-q.v.y * w.x + q.v.x * w.y + q.w   * w.z);

	return r;
}


Quaternion QNorm(const Quaternion &q)
{
  Quaternion r;
	LReal       len;

  len = LSqrt(q.w*q.w + q.v*q.v); if (len==0.0) return q;

  r.w = q.w / len;
  r.v = q.v / len;

	return r;
}

Quaternion QSlerp(const Quaternion &q1, const Quaternion &q2, const LReal alfa)
{
	LReal       sum,teta,beta1,beta2;
	Quaternion q,qd=q2;

	sum = q1.w*q2.w + q1.v*q2.v;

	if (sum<0.0)
	{
		sum=-sum;
		qd.v=-qd.v;
		qd.w=-qd.w;
	}

	if (1.0-sum>EPSILON)
	{
		teta = ACos(sum);
		Real sn = 1.0/Sin(teta);
		beta1 = Sin((1.0-alfa)*teta)*sn;
		beta2 = Sin(alfa*teta)*sn;
	}
	else
	{
		beta1 = 1.0-alfa;
		beta2 = alfa;
	}

	q.w   = beta1*q1.w   + beta2*qd.w;
	q.v.x = beta1*q1.v.x + beta2*qd.v.x;
	q.v.y = beta1*q1.v.y + beta2*qd.v.y;
	q.v.z = beta1*q1.v.z + beta2*qd.v.z;

	return q;
}


// ****************************************
inline void LSinCos(LReal w, LReal &sn, LReal &cn) { sn=LSin(w); cn=LCos(w); }

LMatrix LHPBToMatrix(const LVector &w)
{
	LReal cn,sn,ck,sk,cs,ss,cosksinn,sinksinn;

	LSinCos(w.x,ss,cs);
	LSinCos(w.y,sn,cn);
	LSinCos(w.z,sk,ck);

	cosksinn=ck*sn;
	sinksinn=sk*sn;

	return LMatrix(LVector(0.0),
								 LVector(ck*cs-sinksinn*ss,-sk*cn,ck*ss+sinksinn*cs),
								 LVector(sk*cs+cosksinn*ss,ck*cn,sk*ss-cosksinn*cs),
								 LVector(-cn*ss,sn,cn*cs));
}
