freeminer/irrlicht
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

stole lightfeather's quaternion::toEuler function by mm765 (with permission) as it deals with singularities. added a constant for pi/2.

Browse Source 
git-svn-id: http://svn.code.sf.net/p/irrlicht/code/trunk@795 dfc29bdd-3216-0410-991c-e03cc46cb475
This commit is contained in:
bitplane 2007-07-17 15:42:32 +00:00
parent 7d574cc99d
commit 4d782c1e39
2 changed files with 59 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
include/irrMath.h
View File

@ -41,6 +41,9 @@ namespace core
//! Constant for reciprocal of PI.
const f32 RECIPROCAL_PI = 1.0f/PI;
//! Constant for half of PI.
const f32 HALF_PI = PI/2.0f;
//! Constant for 64bit PI.
const f64 PI64 = 3.1415926535897932384626433832795028841971693993751;

66
include/quaternion.h
View File

@ -103,6 +103,9 @@ class quaternion
//! set quaternion to identity
void makeIdentity();
//! sets quaternion to represent a rotation from one angle to another
void rotationFromTo(const vector3df& from, const vector3df& to);
f32 X, Y, Z, W;
};
@ -492,19 +495,37 @@ inline void quaternion::toAngleAxis(f32 &angle, core::vector3df &axis) const
inline void quaternion::toEuler(vector3df& euler) const
{
double sqw = W*W;
double sqx = X*X;
double sqy = Y*Y;
double sqz = Z*Z;
/* new version after euclideanspace.com by Matthias Meyer
old version had a problem where the initial quaternion
would return -0 for Y value and NaN for rotation of 1.5708
around Y-axis when using fromangleaxis() */
// heading = rotation about z-axis
euler.Z = (f32) (atan2(2.0 * (X*Y +Z*W),(sqx - sqy - sqz + sqw)));
f64 testValue = X * Y + Z * W;
if(testValue > 0.499f) // north pole singularity
{
euler.Y = (f32) (2 * atan2(X,W));
euler.Z = (f32) (HALF_PI);
euler.X = 0.0f;
return;
}
if(testValue < -0.499f) // south pole singularity
{
euler.Y = (f32) (-2 * atan2(X,W));
euler.Z = (f32) (-HALF_PI);
euler.X = 0.0f;
return;
}
f64 sqx = X*X;
f64 sqy = Y*Y;
f64 sqz = Z*Z;
f64 sqw = W*W;
f64 unit = sqx + sqy + sqz + sqw;
// bank = rotation about x-axis
euler.X = (f32) (atan2(2.0 * (Y*Z +X*W),(-sqx - sqy + sqz + sqw)));
euler.Y = (f32) atan2( 2.0*Y*W - 2.0*X*Z, sqx - sqy - sqz + sqw );
euler.Z = (f32) asin ( 2.0 * testValue / unit );
euler.X = (f32) atan2( 2.0*X*W - 2.0*Y*Z, -sqx + sqy - sqz + sqw );
// attitude = rotation about y-axis
euler.Y = (f32) (asin( clamp(-2.0 * (X*Z - Y*W), -1.0, 1.0) ));
return;
}
inline vector3df quaternion::operator* (const vector3df& v) const
@ -530,6 +551,31 @@ inline void quaternion::makeIdentity()
Z = 0.f;
}
inline void quaternion::rotationFromTo(const vector3df& from, const vector3df& to)
{
// Based on Stan Melax's article in Game Programming Gems
// Copy, since cannot modify local
vector3df v0 = from;
vector3df v1 = to;
v0.normalize();
v1.normalize();
vector3df c = v0.crossProduct(v1);
f32 d = v0.dotProduct(v1);
if (d >= 1.0f) // If dot == 1, vectors are the same
{
*this=quaternion(0,0,0,1); //IDENTITY;
}
f32 s = sqrtf( (1+d)*2 ); // optimize inv_sqrt
f32 invs = 1 / s;
X = c.X * invs;
Y = c.Y * invs;
Z = c.Z * invs;
W = s * 0.5f;
}
} // end namespace core
} // end namespace irr