Loading...
Searching...
No Matches
quaternion.h
Go to the documentation of this file.
// Copyright (C) 2002-2012 Nikolaus Gebhardt
// This file is part of the "Irrlicht Engine".
// For conditions of distribution and use, see copyright notice in irrlicht.h
#ifndef __IRR_QUATERNION_H_INCLUDED__
#define __IRR_QUATERNION_H_INCLUDED__
#include "irrTypes.h"
#include "irrMath.h"
#include "matrix4.h"
#include "vector3d.h"
// Between Irrlicht 1.7 and Irrlicht 1.8 the quaternion-matrix conversions got fixed.
// This define disables all involved functions completely to allow finding all places
// where the wrong conversions had been in use.
#define IRR_TEST_BROKEN_QUATERNION_USE 0
namespace irr
{
namespace core
{
class quaternion
{
public:
quaternion() : X(0.0f), Y(0.0f), Z(0.0f), W(1.0f) {}
quaternion(f32 x, f32 y, f32 z, f32 w) : X(x), Y(y), Z(z), W(w) { }
quaternion(f32 x, f32 y, f32 z);
quaternion(const vector3df& vec);
#if !IRR_TEST_BROKEN_QUATERNION_USE
quaternion(const matrix4& mat);
#endif
bool operator==(const quaternion& other) const;
bool operator!=(const quaternion& other) const;
inline quaternion& operator=(const quaternion& other);
#if !IRR_TEST_BROKEN_QUATERNION_USE
inline quaternion& operator=(const matrix4& other);
#endif
quaternion operator+(const quaternion& other) const;
quaternion operator*(const quaternion& other) const;
quaternion operator*(f32 s) const;
quaternion& operator*=(f32 s);
vector3df operator*(const vector3df& v) const;
quaternion& operator*=(const quaternion& other);
inline f32 dotProduct(const quaternion& other) const;
inline quaternion& set(f32 x, f32 y, f32 z, f32 w);
inline quaternion& set(f32 x, f32 y, f32 z);
inline quaternion& set(const core::vector3df& vec);
inline quaternion& set(const core::quaternion& quat);
inline bool equals(const quaternion& other,
const f32 tolerance = ROUNDING_ERROR_f32 ) const;
inline quaternion& normalize();
#if !IRR_TEST_BROKEN_QUATERNION_USE
matrix4 getMatrix() const;
#endif
void getMatrix( matrix4 &dest, const core::vector3df &translation=core::vector3df() ) const;
void getMatrixCenter( matrix4 &dest, const core::vector3df &center, const core::vector3df &translation ) const;
inline void getMatrix_transposed( matrix4 &dest ) const;
quaternion& makeInverse();
quaternion& lerp(quaternion q1, quaternion q2, f32 time);
quaternion& slerp(quaternion q1, quaternion q2,
f32 time, f32 threshold=.05f);
quaternion& fromAngleAxis (f32 angle, const vector3df& axis);
void toAngleAxis (f32 &angle, core::vector3df& axis) const;
void toEuler(vector3df& euler) const;
quaternion& makeIdentity();
quaternion& rotationFromTo(const vector3df& from, const vector3df& to);
f32 X; // vectorial (imaginary) part
f32 Y;
f32 Z;
f32 W; // real part
};
// Constructor which converts euler angles to a quaternion
inline quaternion::quaternion(f32 x, f32 y, f32 z)
{
set(x,y,z);
}
// Constructor which converts euler angles to a quaternion
inline quaternion::quaternion(const vector3df& vec)
{
set(vec.X,vec.Y,vec.Z);
}
#if !IRR_TEST_BROKEN_QUATERNION_USE
// Constructor which converts a matrix to a quaternion
inline quaternion::quaternion(const matrix4& mat)
{
(*this) = mat;
}
#endif
// equal operator
inline bool quaternion::operator==(const quaternion& other) const
{
return ((X == other.X) &&
(Y == other.Y) &&
(Z == other.Z) &&
(W == other.W));
}
// inequality operator
inline bool quaternion::operator!=(const quaternion& other) const
{
return !(*this == other);
}
// assignment operator
inline quaternion& quaternion::operator=(const quaternion& other)
{
X = other.X;
Y = other.Y;
Z = other.Z;
W = other.W;
return *this;
}
#if !IRR_TEST_BROKEN_QUATERNION_USE
// matrix assignment operator
inline quaternion& quaternion::operator=(const matrix4& m)
{
const f32 diag = m[0] + m[5] + m[10] + 1;
if( diag > 0.0f )
{
const f32 scale = sqrtf(diag) * 2.0f; // get scale from diagonal
// TODO: speed this up
X = (m[6] - m[9]) / scale;
Y = (m[8] - m[2]) / scale;
Z = (m[1] - m[4]) / scale;
W = 0.25f * scale;
}
else
{
if (m[0]>m[5] && m[0]>m[10])
{
// 1st element of diag is greatest value
// find scale according to 1st element, and double it
const f32 scale = sqrtf(1.0f + m[0] - m[5] - m[10]) * 2.0f;
// TODO: speed this up
X = 0.25f * scale;
Y = (m[4] + m[1]) / scale;
Z = (m[2] + m[8]) / scale;
W = (m[6] - m[9]) / scale;
}
else if (m[5]>m[10])
{
// 2nd element of diag is greatest value
// find scale according to 2nd element, and double it
const f32 scale = sqrtf(1.0f + m[5] - m[0] - m[10]) * 2.0f;
// TODO: speed this up
X = (m[4] + m[1]) / scale;
Y = 0.25f * scale;
Z = (m[9] + m[6]) / scale;
W = (m[8] - m[2]) / scale;
}
else
{
// 3rd element of diag is greatest value
// find scale according to 3rd element, and double it
const f32 scale = sqrtf(1.0f + m[10] - m[0] - m[5]) * 2.0f;
// TODO: speed this up
X = (m[8] + m[2]) / scale;
Y = (m[9] + m[6]) / scale;
Z = 0.25f * scale;
W = (m[1] - m[4]) / scale;
}
}
return normalize();
}
#endif
// multiplication operator
inline quaternion quaternion::operator*(const quaternion& other) const
{
quaternion tmp;
tmp.W = (other.W * W) - (other.X * X) - (other.Y * Y) - (other.Z * Z);
tmp.X = (other.W * X) + (other.X * W) + (other.Y * Z) - (other.Z * Y);
tmp.Y = (other.W * Y) + (other.Y * W) + (other.Z * X) - (other.X * Z);
tmp.Z = (other.W * Z) + (other.Z * W) + (other.X * Y) - (other.Y * X);
return tmp;
}
// multiplication operator
inline quaternion quaternion::operator*(f32 s) const
{
return quaternion(s*X, s*Y, s*Z, s*W);
}
// multiplication operator
inline quaternion& quaternion::operator*=(f32 s)
{
X*=s;
Y*=s;
Z*=s;
W*=s;
return *this;
}
// multiplication operator
inline quaternion& quaternion::operator*=(const quaternion& other)
{
return (*this = other * (*this));
}
// add operator
inline quaternion quaternion::operator+(const quaternion& b) const
{
return quaternion(X+b.X, Y+b.Y, Z+b.Z, W+b.W);
}
#if !IRR_TEST_BROKEN_QUATERNION_USE
// Creates a matrix from this quaternion
inline matrix4 quaternion::getMatrix() const
{
core::matrix4 m;
getMatrix(m);
return m;
}
#endif
inline void quaternion::getMatrix(matrix4 &dest,
const core::vector3df &center) const
{
dest[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
dest[1] = 2.0f*X*Y + 2.0f*Z*W;
dest[2] = 2.0f*X*Z - 2.0f*Y*W;
dest[3] = 0.0f;
dest[4] = 2.0f*X*Y - 2.0f*Z*W;
dest[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
dest[6] = 2.0f*Z*Y + 2.0f*X*W;
dest[7] = 0.0f;
dest[8] = 2.0f*X*Z + 2.0f*Y*W;
dest[9] = 2.0f*Z*Y - 2.0f*X*W;
dest[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
dest[11] = 0.0f;
dest[12] = center.X;
dest[13] = center.Y;
dest[14] = center.Z;
dest[15] = 1.f;
dest.setDefinitelyIdentityMatrix ( false );
}
inline void quaternion::getMatrixCenter(matrix4 &dest,
const core::vector3df &center,
const core::vector3df &translation) const
{
dest[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
dest[1] = 2.0f*X*Y + 2.0f*Z*W;
dest[2] = 2.0f*X*Z - 2.0f*Y*W;
dest[3] = 0.0f;
dest[4] = 2.0f*X*Y - 2.0f*Z*W;
dest[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
dest[6] = 2.0f*Z*Y + 2.0f*X*W;
dest[7] = 0.0f;
dest[8] = 2.0f*X*Z + 2.0f*Y*W;
dest[9] = 2.0f*Z*Y - 2.0f*X*W;
dest[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
dest[11] = 0.0f;
dest.setRotationCenter ( center, translation );
}
// Creates a matrix from this quaternion
inline void quaternion::getMatrix_transposed(matrix4 &dest) const
{
dest[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
dest[4] = 2.0f*X*Y + 2.0f*Z*W;
dest[8] = 2.0f*X*Z - 2.0f*Y*W;
dest[12] = 0.0f;
dest[1] = 2.0f*X*Y - 2.0f*Z*W;
dest[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
dest[9] = 2.0f*Z*Y + 2.0f*X*W;
dest[13] = 0.0f;
dest[2] = 2.0f*X*Z + 2.0f*Y*W;
dest[6] = 2.0f*Z*Y - 2.0f*X*W;
dest[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
dest[14] = 0.0f;
dest[3] = 0.f;
dest[7] = 0.f;
dest[11] = 0.f;
dest[15] = 1.f;
dest.setDefinitelyIdentityMatrix(false);
}
// Inverts this quaternion
inline quaternion& quaternion::makeInverse()
{
X = -X; Y = -Y; Z = -Z;
return *this;
}
// sets new quaternion
inline quaternion& quaternion::set(f32 x, f32 y, f32 z, f32 w)
{
X = x;
Y = y;
Z = z;
W = w;
return *this;
}
// sets new quaternion based on euler angles
inline quaternion& quaternion::set(f32 x, f32 y, f32 z)
{
f64 angle;
angle = x * 0.5;
const f64 sr = sin(angle);
const f64 cr = cos(angle);
angle = y * 0.5;
const f64 sp = sin(angle);
const f64 cp = cos(angle);
angle = z * 0.5;
const f64 sy = sin(angle);
const f64 cy = cos(angle);
const f64 cpcy = cp * cy;
const f64 spcy = sp * cy;
const f64 cpsy = cp * sy;
const f64 spsy = sp * sy;
X = (f32)(sr * cpcy - cr * spsy);
Y = (f32)(cr * spcy + sr * cpsy);
Z = (f32)(cr * cpsy - sr * spcy);
W = (f32)(cr * cpcy + sr * spsy);
return normalize();
}
// sets new quaternion based on euler angles
inline quaternion& quaternion::set(const core::vector3df& vec)
{
return set(vec.X, vec.Y, vec.Z);
}
// sets new quaternion based on other quaternion
inline quaternion& quaternion::set(const core::quaternion& quat)
{
return (*this=quat);
}
inline bool quaternion::equals(const quaternion& other, const f32 tolerance) const
{
return core::equals(X, other.X, tolerance) &&
core::equals(Y, other.Y, tolerance) &&
core::equals(Z, other.Z, tolerance) &&
core::equals(W, other.W, tolerance);
}
// normalizes the quaternion
inline quaternion& quaternion::normalize()
{
const f32 n = X*X + Y*Y + Z*Z + W*W;
if (n == 1)
return *this;
//n = 1.0f / sqrtf(n);
return (*this *= reciprocal_squareroot ( n ));
}
// set this quaternion to the result of the linear interpolation between two quaternions
inline quaternion& quaternion::lerp(quaternion q1, quaternion q2, f32 time)
{
const f32 scale = 1.0f - time;
return (*this = (q1*scale) + (q2*time));
}
// set this quaternion to the result of the interpolation between two quaternions
inline quaternion& quaternion::slerp(quaternion q1, quaternion q2, f32 time, f32 threshold)
{
f32 angle = q1.dotProduct(q2);
// make sure we use the short rotation
if (angle < 0.0f)
{
q1 *= -1.0f;
angle *= -1.0f;
}
if (angle <= (1-threshold)) // spherical interpolation
{
const f32 theta = acosf(angle);
const f32 invsintheta = reciprocal(sinf(theta));
const f32 scale = sinf(theta * (1.0f-time)) * invsintheta;
const f32 invscale = sinf(theta * time) * invsintheta;
return (*this = (q1*scale) + (q2*invscale));
}
else // linear interploation
return lerp(q1,q2,time);
}
// calculates the dot product
inline f32 quaternion::dotProduct(const quaternion& q2) const
{
return (X * q2.X) + (Y * q2.Y) + (Z * q2.Z) + (W * q2.W);
}
inline quaternion& quaternion::fromAngleAxis(f32 angle, const vector3df& axis)
{
const f32 fHalfAngle = 0.5f*angle;
const f32 fSin = sinf(fHalfAngle);
W = cosf(fHalfAngle);
X = fSin*axis.X;
Y = fSin*axis.Y;
Z = fSin*axis.Z;
return *this;
}
inline void quaternion::toAngleAxis(f32 &angle, core::vector3df &axis) const
{
const f32 scale = sqrtf(X*X + Y*Y + Z*Z);
if (core::iszero(scale) || W > 1.0f || W < -1.0f)
{
angle = 0.0f;
axis.X = 0.0f;
axis.Y = 1.0f;
axis.Z = 0.0f;
}
else
{
const f32 invscale = reciprocal(scale);
angle = 2.0f * acosf(W);
axis.X = X * invscale;
axis.Y = Y * invscale;
axis.Z = Z * invscale;
}
}
inline void quaternion::toEuler(vector3df& euler) const
{
const f64 sqw = W*W;
const f64 sqx = X*X;
const f64 sqy = Y*Y;
const f64 sqz = Z*Z;
const f64 test = 2.0 * (Y*W - X*Z);
if (core::equals(test, 1.0, 0.000001))
{
// heading = rotation about z-axis
euler.Z = (f32) (-2.0*atan2(X, W));
// bank = rotation about x-axis
euler.X = 0;
// attitude = rotation about y-axis
euler.Y = (f32) (core::PI64/2.0);
}
else if (core::equals(test, -1.0, 0.000001))
{
// heading = rotation about z-axis
euler.Z = (f32) (2.0*atan2(X, W));
// bank = rotation about x-axis
euler.X = 0;
// attitude = rotation about y-axis
euler.Y = (f32) (core::PI64/-2.0);
}
else
{
// heading = rotation about z-axis
euler.Z = (f32) atan2(2.0 * (X*Y +Z*W),(sqx - sqy - sqz + sqw));
// bank = rotation about x-axis
euler.X = (f32) atan2(2.0 * (Y*Z +X*W),(-sqx - sqy + sqz + sqw));
// attitude = rotation about y-axis
euler.Y = (f32) asin( clamp(test, -1.0, 1.0) );
}
}
inline vector3df quaternion::operator* (const vector3df& v) const
{
// nVidia SDK implementation
vector3df uv, uuv;
vector3df qvec(X, Y, Z);
uv = qvec.crossProduct(v);
uuv = qvec.crossProduct(uv);
uv *= (2.0f * W);
uuv *= 2.0f;
return v + uv + uuv;
}
// set quaternion to identity
inline core::quaternion& quaternion::makeIdentity()
{
W = 1.f;
X = 0.f;
Y = 0.f;
Z = 0.f;
return *this;
}
inline core::quaternion& 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();
const f32 d = v0.dotProduct(v1);
if (d >= 1.0f) // If dot == 1, vectors are the same
{
return makeIdentity();
}
else if (d <= -1.0f) // exactly opposite
{
core::vector3df axis(1.0f, 0.f, 0.f);
axis = axis.crossProduct(v0);
if (axis.getLength()==0)
{
axis.set(0.f,1.f,0.f);
axis = axis.crossProduct(v0);
}
// same as fromAngleAxis(core::PI, axis).normalize();
return set(axis.X, axis.Y, axis.Z, 0).normalize();
}
const f32 s = sqrtf( (1+d)*2 ); // optimize inv_sqrt
const f32 invs = 1.f / s;
const vector3df c = v0.crossProduct(v1)*invs;
return set(c.X, c.Y, c.Z, s * 0.5f).normalize();
}
} // end namespace core
} // end namespace irr
#endif
irr::core::CMatrix4::setRotationCenter
void setRotationCenter(const core::vector3df &center, const core::vector3df &translate)
Builds a combined matrix which translates to a center before rotation and translates from origin afte...
Definition matrix4.h:2043
irr::core::CMatrix4::setDefinitelyIdentityMatrix
void setDefinitelyIdentityMatrix(bool isDefinitelyIdentityMatrix)
Sets if the matrix is definitely identity matrix.
Definition matrix4.h:2190
irr::core::quaternion
Quaternion class for representing rotations.
Definition quaternion.h:27
irr::core::quaternion::operator=
quaternion & operator=(const quaternion &other)
Assignment operator.
Definition quaternion.h:223
irr::core::quaternion::dotProduct
f32 dotProduct(const quaternion &other) const
Calculates the dot product.
Definition quaternion.h:556
irr::core::quaternion::getMatrix
matrix4 getMatrix() const
Creates a matrix from this quaternion.
Definition quaternion.h:338
irr::core::quaternion::makeInverse
quaternion & makeInverse()
Inverts this quaternion.
Definition quaternion.h:438
irr::core::quaternion::makeIdentity
quaternion & makeIdentity()
Set quaternion to identity.
Definition quaternion.h:649
irr::core::quaternion::equals
bool equals(const quaternion &other, const f32 tolerance=ROUNDING_ERROR_f32) const
returns if this quaternion equals the other one, taking floating point rounding errors into account
Definition quaternion.h:500
irr::core::quaternion::set
quaternion & set(f32 x, f32 y, f32 z, f32 w)
Sets new quaternion.
Definition quaternion.h:446
irr::core::quaternion::operator!=
bool operator!=(const quaternion &other) const
inequality operator
Definition quaternion.h:217
irr::core::quaternion::toEuler
void toEuler(vector3df &euler) const
Output this quaternion to an euler angle (radians).
Definition quaternion.h:596
irr::core::quaternion::X
f32 X
Quaternion elements.
Definition quaternion.h:179
irr::core::quaternion::operator==
bool operator==(const quaternion &other) const
Equalilty operator.
Definition quaternion.h:208
irr::core::quaternion::lerp
quaternion & lerp(quaternion q1, quaternion q2, f32 time)
Set this quaternion to the linear interpolation between two quaternions.
Definition quaternion.h:523
irr::core::quaternion::quaternion
quaternion(f32 x, f32 y, f32 z, f32 w)
Constructor.
Definition quaternion.h:34
irr::core::quaternion::Y
f32 Y
Definition quaternion.h:180
irr::core::quaternion::operator+
quaternion operator+(const quaternion &other) const
Add operator.
Definition quaternion.h:331
irr::core::quaternion::toAngleAxis
void toAngleAxis(f32 &angle, core::vector3df &axis) const
Fills an angle (radians) around an axis (unit vector).
Definition quaternion.h:575
irr::core::quaternion::Z
f32 Z
Definition quaternion.h:181
irr::core::quaternion::slerp
quaternion & slerp(quaternion q1, quaternion q2, f32 time, f32 threshold=.05f)
Set this quaternion to the result of the spherical interpolation between two quaternions.
Definition quaternion.h:531
irr::core::quaternion::operator*=
quaternion & operator*=(f32 s)
Multiplication operator with scalar.
Definition quaternion.h:315
irr::core::quaternion::rotationFromTo
quaternion & rotationFromTo(const vector3df &from, const vector3df &to)
Set quaternion to represent a rotation from one vector to another.
Definition quaternion.h:658
irr::core::quaternion::normalize
quaternion & normalize()
Normalizes the quaternion.
Definition quaternion.h:510
irr::core::quaternion::getMatrixCenter
void getMatrixCenter(matrix4 &dest, const core::vector3df &center, const core::vector3df &translation) const
Definition quaternion.h:388
irr::core::quaternion::fromAngleAxis
quaternion & fromAngleAxis(f32 angle, const vector3df &axis)
Create quaternion from rotation angle and rotation axis.
Definition quaternion.h:563
irr::core::quaternion::getMatrix_transposed
void getMatrix_transposed(matrix4 &dest) const
Creates a matrix from this quaternion.
Definition quaternion.h:411
irr::core::quaternion::W
f32 W
Definition quaternion.h:182
irr::core::quaternion::operator*
quaternion operator*(const quaternion &other) const
Multiplication operator.
Definition quaternion.h:294
irr::core::quaternion::quaternion
quaternion()
Default Constructor.
Definition quaternion.h:31
irr::core::vector3d::crossProduct
vector3d< T > crossProduct(const vector3d< T > &p) const
Calculates the cross product with another vector.
Definition vector3d.h:147
irr::core::vector3d::getLength
T getLength() const
Get length of the vector.
Definition vector3d.h:117
irr::core::vector3d::X
T X
X coordinate of the vector.
Definition vector3d.h:408
irr::core::vector3d::normalize
vector3d< T > & normalize()
Normalizes the vector.
Definition vector3d.h:168
irr::core::vector3d::set
vector3d< T > & set(const T nx, const T ny, const T nz)
Definition vector3d.h:113
irr::core::vector3d::Z
T Z
Z coordinate of the vector.
Definition vector3d.h:414
irr::core::vector3d::dotProduct
T dotProduct(const vector3d< T > &other) const
Get the dot product with another vector.
Definition vector3d.h:125
irr::core::vector3d::Y
T Y
Y coordinate of the vector.
Definition vector3d.h:411
irrMath.h
irrTypes.h
matrix4.h
irr::core::vector3df
vector3d< f32 > vector3df
Typedef for a f32 3d vector.
Definition vector3d.h:445
irr::core::PI64
const f64 PI64
Constant for 64bit PI.
Definition irrMath.h:68
irr::core::clamp
const T clamp(const T &value, const T &low, const T &high)
clamps a value between low and high
Definition irrMath.h:166
irr::core::matrix4
CMatrix4< f32 > matrix4
Typedef for f32 matrix.
Definition matrix4.h:2235
irr::core::ROUNDING_ERROR_f32
const f32 ROUNDING_ERROR_f32
Definition irrMath.h:49
irr::core::reciprocal
REALINLINE f32 reciprocal(const f32 f)
Definition irrMath.h:535
irr::core::equals
bool equals(const f64 a, const f64 b, const f64 tolerance=ROUNDING_ERROR_f64)
returns if a equals b, taking possible rounding errors into account
Definition irrMath.h:185
irr::core::reciprocal_squareroot
REALINLINE f64 reciprocal_squareroot(const f64 x)
Definition irrMath.h:497
irr::core::iszero
bool iszero(const f64 a, const f64 tolerance=ROUNDING_ERROR_f64)
returns if a equals zero, taking rounding errors into account
Definition irrMath.h:270
irr
Everything in the Irrlicht Engine can be found in this namespace.
Definition aabbox3d.h:13
irr::f32
float f32
32 bit floating point variable.
Definition irrTypes.h:104
irr::f64
double f64
64 bit floating point variable.
Definition irrTypes.h:108
vector3d.h
- Irrlicht Engine Documentation © 2003-2012 by Nikolaus Gebhardt. Generated on for Irrlicht 3D Engine by Doxygen 1.15.0