/*==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);
}