/**** EulerAngles.c - Convert Euler angles to/from matrix or quat ****/
/* Ken Shoemake, 1993 */
#include "EulerAngles.h"

quaternion Eul_(float ai, float aj, float ah, int order)
{
    quaternion ea;
    ea.x = ai; ea.y = aj; ea.z = ah;
    ea.w = order;
    return (ea);
}

/* Construct quaternion from Euler angles (in radians). */
quaternion Eul_ToQuat(quaternion ea)
{
    quaternion qu;
    double a[3], ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
    int i,j,k,h,n,s,f;

    EulGetOrd(ea.w,i,j,k,h,n,s,f);

    if(f == EulFrmR)
	{
		float t = ea.x;
		ea.x = ea.z;
		ea.z = t;
	}

    if(n == EulParOdd) ea.y = -ea.y;

    ti = ea.x*0.5; tj = ea.y*0.5; th = ea.z*0.5;
    ci = cos(ti);  cj = cos(tj);  ch = cos(th);
    si = sin(ti);  sj = sin(tj);  sh = sin(th);
    cc = ci*ch; cs = ci*sh; sc = si*ch; ss = si*sh;

    if(s==EulRepYes)
	{
		a[i] = cj*(cs + sc);	/* Could speed up with */
		a[j] = sj*(cc + ss);	/* trig identities. */
		a[k] = sj*(cs - sc);
		qu.w = cj*(cc - ss);
    } else {
		a[i] = cj*sc - sj*cs;
		a[j] = cj*ss + sj*cc;
		a[k] = cj*cs - sj*sc;
		qu.w = cj*cc + sj*ss;
    }

    if(n == EulParOdd) a[j] = -a[j];
    qu.x = a[0]; qu.y = a[1]; qu.z = a[2];

    return (qu);
}

/* Construct matrix from Euler angles (in radians). */
void Eul_ToMatrix(quaternion ea, matrix M)
{
    double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
    int i,j,k,h,n,s,f;

    EulGetOrd(ea.w,i,j,k,h,n,s,f);

    if(f == EulFrmR)
	{
		float t = ea.x;
		ea.x = ea.z;
		ea.z = t;
	}
    if(n == EulParOdd)
	{
		ea.x = -ea.x;
		ea.y = -ea.y;
		ea.z = -ea.z;
	}
    ti = ea.x;	  tj = ea.y;	th = ea.z;
    ci = cos(ti); cj = cos(tj); ch = cos(th);
    si = sin(ti); sj = sin(tj); sh = sin(th);
    cc = ci*ch; cs = ci*sh; sc = si*ch; ss = si*sh;
    if(s == EulRepYes)
	{
		M.m[i][i] = cj;	  M.m[i][j] =  sj*si;    M.m[i][k] =  sj*ci;
		M.m[j][i] = sj*sh;  M.m[j][j] = -cj*ss+cc; M.m[j][k] = -cj*cs-sc;
		M.m[k][i] = -sj*ch; M.m[k][j] =  cj*sc+cs; M.m[k][k] =  cj*cc-ss;
    } else {
		M.m[i][i] = cj*ch; M.m[i][j] = sj*sc-cs; M.m[i][k] = sj*cc+ss;
		M.m[j][i] = cj*sh; M.m[j][j] = sj*ss+cc; M.m[j][k] = sj*cs-sc;
		M.m[k][i] = -sj;	 M.m[k][j] = cj*si;    M.m[k][k] = cj*ci;
    }
    M.m[3][0]=M.m[3][1]=M.m[3][2]=M.m[0][3]=M.m[1][3]=M.m[2][3]=0.0; M.m[3][3]=1.0;
}

/* Convert matrix to Euler angles (in radians). */
quaternion Eul_FromMatrix(matrix M, int order)
{
    quaternion ea;
    int i,j,k,h,n,s,f;

    EulGetOrd(order,i,j,k,h,n,s,f);

    if(s == EulRepYes)
	{
		double sy = sqrt(M.m[i][j]*M.m[i][j] + M.m[i][k]*M.m[i][k]);
		if(sy > 16*FLT_EPSILON)
		{
			ea.x = atan2(M.m[i][j], M.m[i][k]);
			ea.y = atan2(sy, M.m[i][i]);
			ea.z = atan2(M.m[j][i], -M.m[k][i]);
		} else {
			ea.x = atan2(-M.m[j][k], M.m[j][j]);
			ea.y = atan2(sy, M.m[i][i]);
			ea.z = 0;
		}
    } else {
		double cy = sqrt(M.m[i][i]*M.m[i][i] + M.m[j][i]*M.m[j][i]);
		if(cy > 16*FLT_EPSILON)
		{
			ea.x = atan2(M.m[k][j], M.m[k][k]);
			ea.y = atan2(-M.m[k][i], cy);
			ea.z = atan2(M.m[j][i], M.m[i][i]);
		} else {
			ea.x = atan2(-M.m[j][k], M.m[j][j]);
			ea.y = atan2(-M.m[k][i], cy);
			ea.z = 0;
		}
    }

    if(n == EulParOdd)
	{
		ea.x = -ea.x;
		ea.y = - ea.y;
		ea.z = -ea.z;
	}
    if(f == EulFrmR)
	{
		float t = ea.x;
		ea.x = ea.z;
		ea.z = t;
	}

    ea.w = order;

    return (ea);
}

/* Convert quaternion to Euler angles (in radians). */
quaternion Eul_FromQuat(quaternion q, int order)
{
    matrix M;
    double Nq = q.x*q.x+q.y*q.y+q.z*q.z+q.w*q.w;
    double s = (Nq > 0.0) ? (2.0 / Nq) : 0.0;
    double xs = q.x*s,	  ys = q.y*s,	 zs = q.z*s;
    double wx = q.w*xs,	  wy = q.w*ys,	 wz = q.w*zs;
    double xx = q.x*xs,	  xy = q.x*ys,	 xz = q.x*zs;
    double yy = q.y*ys,	  yz = q.y*zs,	 zz = q.z*zs;
    M.m[0][0] = 1.0 - (yy + zz); M.m[0][1] = xy - wz; M.m[0][2] = xz + wy;
    M.m[1][0] = xy + wz; M.m[1][1] = 1.0 - (xx + zz); M.m[1][2] = yz - wx;
    M.m[2][0] = xz - wy; M.m[2][1] = yz + wx; M.m[2][2] = 1.0 - (xx + yy);
    M.m[3][0]=M.m[3][1]=M.m[3][2]=M.m[0][3]=M.m[1][3]=M.m[2][3]=0.0; M.m[3][3]=1.0;

    return (Eul_FromMatrix(M, order));
}