You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
212 lines
6.8 KiB
212 lines
6.8 KiB
/*==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); |
|
}
|
|
|