CWE-ou-minkata/MOULOpenSourceClientPlugin/Plasma20/Sources/Plasma/PubUtilLib/plTransform/hsEuler.cpp

/*==LICENSE==*

CyanWorlds.com Engine - MMOG client, server and tools
Copyright (C) 2011  Cyan Worlds, Inc.

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 3 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, see <http://www.gnu.org/licenses/>.

You can contact Cyan Worlds, Inc. by email legal@cyan.com
 or by snail mail at:
      Cyan Worlds, Inc.
      14617 N Newport Hwy
      Mead, WA   99021

*==LICENSE==*/
//
//////////////////////////////////////////////////////////////////////////
// EULER STUFF
// See Gems IV, Ken Shoemake
//////////////////////////////////////////////////////////////////////////
//
#include <float.h>	// for FLT_EPSILON
#include "hsEuler.h"
#include "hsQuat.h"
#include "hsMatrix44.h"

enum QuatPart 
{
	X, Y, Z, W
};

//
// Construct quaternion from Euler angles (in radians).
//
void hsEuler::GetQuat(hsQuat* 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;

	hsEuler ea=*this;	// copy
    EulGetOrd(ea.fOrder,i,j,k,h,n,s,f);
    if (f==EulFrmR) 
	{
		hsScalar t = ea.fX; ea.fX = ea.fZ; ea.fZ = t;
	}
    if (n==EulParOdd) 
		ea.fY = -ea.fY;
    ti = ea.fX*0.5; tj = ea.fY*0.5; th = ea.fZ*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->fW = static_cast<float>(cj*(cc - ss));
    } 
	else 
	{
		a[i] = cj*sc - sj*cs;
		a[j] = cj*ss + sj*cc;
		a[k] = cj*cs - sj*sc;
		qu->fW = static_cast<float>(cj*cc + sj*ss);
    }
    if (n==EulParOdd) 
		a[j] = -a[j];
    qu->fX = static_cast<float>(a[X]); 
	qu->fY = static_cast<float>(a[Y]);
	qu->fZ = static_cast<float>(a[Z]);
}

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

	hsEuler ea=*this;	// copy
    EulGetOrd(ea.fOrder,i,j,k,h,n,s,f);
    if (f==EulFrmR) 
	{
		hsScalar t = ea.fX; ea.fX = ea.fZ; ea.fZ = t;
	}
    if (n==EulParOdd) 
	{
		ea.fX = -ea.fX; ea.fY = -ea.fY; ea.fZ = -ea.fZ;
	}
    ti = ea.fX;	  tj = ea.fY;	th = ea.fZ;
    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) 
	{
		mat->fMap[i][i] = static_cast<float>(cj);
		mat->fMap[i][j] = static_cast<float>(sj*si);
		mat->fMap[i][k] = static_cast<float>(sj*ci);

		mat->fMap[j][i] = static_cast<float>(sj*sh);
		mat->fMap[j][j] = static_cast<float>(-cj*ss+cc);
		mat->fMap[j][k] = static_cast<float>(-cj*cs-sc);

		mat->fMap[k][i] = static_cast<float>(-sj*ch);
		mat->fMap[k][j] = static_cast<float>(cj*sc+cs);
		mat->fMap[k][k] = static_cast<float>(cj*cc-ss);
    } 
	else 
	{
		mat->fMap[i][i] = static_cast<float>(cj*ch);
		mat->fMap[i][j] = static_cast<float>(sj*sc-cs);
		mat->fMap[i][k] = static_cast<float>(sj*cc+ss);

		mat->fMap[j][i] = static_cast<float>(cj*sh);
		mat->fMap[j][j] = static_cast<float>(sj*ss+cc);
		mat->fMap[j][k] = static_cast<float>(sj*cs-sc);

		mat->fMap[k][i] = static_cast<float>(-sj);
		mat->fMap[k][j] = static_cast<float>(cj*si);
		mat->fMap[k][k] = static_cast<float>(cj*ci);
    }
    mat->fMap[W][X]=mat->fMap[W][Y]=mat->fMap[W][Z]=mat->fMap[X][W]=mat->fMap[Y][W]=mat->fMap[Z][W]=0.0; 
	mat->fMap[W][W]=1.0;
}

//
// Convert matrix to Euler angles (in radians)
//
void hsEuler::SetFromMatrix44(const hsMatrix44* mat, UInt32 order)
{
    int i,j,k,h,n,s,f;

    EulGetOrd(order,i,j,k,h,n,s,f);
    if (s==EulRepYes) 
	{
		double sy = sqrt(mat->fMap[i][j]*mat->fMap[i][j] + mat->fMap[i][k]*mat->fMap[i][k]);
		if (sy > 16*FLT_EPSILON) 
		{
			fX = static_cast<float>(atan2(mat->fMap[i][j], mat->fMap[i][k]));
			fY = static_cast<float>(atan2(sy, (double)mat->fMap[i][i]));
			fZ = static_cast<float>(atan2(mat->fMap[j][i], -mat->fMap[k][i]));
		} else 
		{
			fX = static_cast<float>(atan2(-mat->fMap[j][k], mat->fMap[j][j]));
			fY = static_cast<float>(atan2(sy, (double)mat->fMap[i][i]));
			fZ = 0;
		}
    } 
	else 
	{
		double cy = sqrt(mat->fMap[i][i]*mat->fMap[i][i] + mat->fMap[j][i]*mat->fMap[j][i]);
		if (cy > 16*FLT_EPSILON) 
		{
			fX = static_cast<float>(atan2(mat->fMap[k][j], mat->fMap[k][k]));
			fY = static_cast<float>(atan2((double)(-mat->fMap[k][i]), cy));
			fZ = static_cast<float>(atan2(mat->fMap[j][i], mat->fMap[i][i]));
		} 
		else 
		{
			fX = static_cast<float>(atan2(-mat->fMap[j][k], mat->fMap[j][j]));
			fY = static_cast<float>(atan2((double)(-mat->fMap[k][i]), cy));
			fZ = 0;
		}
    }
    if (n==EulParOdd) 
	{
		fX = -fX; fY = - fY; fZ = -fZ;
	}
    if (f==EulFrmR) 
	{
		hsScalar t = fX; fX = fZ; fZ = t;
	}
    fOrder = order;
}

//
// Convert quaternion to Euler angles (in radians)
//
void hsEuler::SetFromQuat(const hsQuat* q, UInt32 order)
{
    hsMatrix44 mat;
    double Nq = q->fX*q->fX+q->fY*q->fY+q->fZ*q->fZ+q->fW*q->fW;
    double s = (Nq > 0.0) ? (2.0 / Nq) : 0.0;
    double xs = q->fX*s,		ys = q->fY*s,	 zs = q->fZ*s;
    double wx = q->fW*xs,	wy = q->fW*ys,	 wz = q->fW*zs;
    double xx = q->fX*xs,	xy = q->fX*ys,	 xz = q->fX*zs;
    double yy = q->fY*ys,	yz = q->fY*zs,	 zz = q->fZ*zs;
    mat.fMap[X][X] = static_cast<float>(1.0 - (yy + zz));
	mat.fMap[X][Y] = static_cast<float>(xy - wz);
	mat.fMap[X][Z] = static_cast<float>(xz + wy);
    mat.fMap[Y][X] = static_cast<float>(xy + wz); 
	mat.fMap[Y][Y] = static_cast<float>(1.0 - (xx + zz)); 
	mat.fMap[Y][Z] = static_cast<float>(yz - wx);
    mat.fMap[Z][X] = static_cast<float>(xz - wy);
	mat.fMap[Z][Y] = static_cast<float>(yz + wx); 
	mat.fMap[Z][Z] = static_cast<float>(1.0 - (xx + yy));
    mat.fMap[W][X] = mat.fMap[W][Y] = mat.fMap[W][Z] = 
		mat.fMap[X][W] = mat.fMap[Y][W] = mat.fMap[Z][W] = 0.0; 
	mat.fMap[W][W] = 1.0;
    SetFromMatrix44(&mat, order);
}