mirror of
https://github.com/glest/glest-source.git
synced 2025-09-25 23:19:07 +02:00
2180 lines
59 KiB
C++
2180 lines
59 KiB
C++
// ==============================================================
|
|
// This file is part of MegaGlest (www.glest.org)
|
|
//
|
|
// Mathlib.inl -- Copyright (c) 2005-2006 David Henry
|
|
// changed for use with MegaGlest: Copyright (C) 2011- by Mark Vejvoda//
|
|
// -------------------------------------------------------------------
|
|
// Portions Copyright (c) Dante Treglia II and Mark A. DeLoura, 2000.
|
|
// Portions Copyright (c) Fletcher Dunn and Ian Parberry, 2002.
|
|
// -------------------------------------------------------------------
|
|
//
|
|
// This code is licensed under the MIT license:
|
|
// http://www.opensource.org/licenses/mit-license.php
|
|
//
|
|
// Open Source Initiative OSI - The MIT License (MIT):Licensing
|
|
//
|
|
// The MIT License (MIT)
|
|
// Copyright (c) <year> <copyright holders>
|
|
//
|
|
// Permission is hereby granted, free of charge, to any person obtaining a
|
|
// copy of this software and associated documentation files (the "Software"),
|
|
// to deal in the Software without restriction, including without limitation
|
|
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
|
|
// and/or sell copies of the Software, and to permit persons to whom the
|
|
// Software is furnished to do so, subject to the following conditions:
|
|
//
|
|
// The above copyright notice and this permission notice shall be included in
|
|
// all copies or substantial portions of the Software.
|
|
//
|
|
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
|
|
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
|
// THE SOFTWARE.//
|
|
//
|
|
// Implementation of a math library to use with OpenGL.
|
|
//
|
|
// Provide vector, matrix and quaternion operations.
|
|
//
|
|
/////////////////////////////////////////////////////////////////////////////
|
|
|
|
#include <cassert>
|
|
|
|
/////////////////////////////////////////////////////////////////////////////
|
|
//
|
|
// Global functions
|
|
//
|
|
/////////////////////////////////////////////////////////////////////////////
|
|
// --------------------------------------------------------------------------
|
|
// wrapPi
|
|
//
|
|
// "Wrap" an angle in range -pi...pi by adding the correct multiple
|
|
// of 2 pi
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline Real
|
|
wrapPi (Real theta)
|
|
{
|
|
theta += kPi;
|
|
theta -= std::floor (theta * k1Over2Pi) * k2Pi;
|
|
theta -= kPi;
|
|
return theta;
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// safeAcos
|
|
//
|
|
// Same as acos(x), but if x is out of range, it is "clamped" to the nearest
|
|
// valid value. The value returned is in range 0...pi, the same as the
|
|
// standard C acos() function
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline Real
|
|
safeAcos (Real x)
|
|
{
|
|
// Check limit conditions
|
|
if (x <= -1.0)
|
|
return kPi;
|
|
|
|
if (x >= 1.0)
|
|
return 0.0;
|
|
|
|
// value is in the domain - use standard C function
|
|
return std::acos (x);
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// canonizeEulerAngles
|
|
//
|
|
// Set the Euler angle triple to its "canonical" value. This does not change
|
|
// the meaning of the Euler angles as a representation of Orientation in 3D,
|
|
// but if the angles are for other purposes such as angular velocities, etc,
|
|
// then the operation might not be valid.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline void
|
|
canonizeEulerAngles (Real &roll, Real &pitch, Real &yaw)
|
|
{
|
|
// First, wrap pitch in range -pi ... pi
|
|
pitch = wrapPi (pitch);
|
|
|
|
// Now, check for "the back side" of the matrix, pitch outside
|
|
// the canonical range of -pi/2 ... pi/2
|
|
if (pitch < -kPiOver2)
|
|
{
|
|
roll += kPi;
|
|
pitch = -kPi - pitch;
|
|
yaw += kPi;
|
|
}
|
|
else if (pitch > kPiOver2)
|
|
{
|
|
roll += kPi;
|
|
pitch = kPi - pitch;
|
|
yaw += kPi;
|
|
}
|
|
|
|
// OK, now check for the Gimbal lock case (within a slight
|
|
// tolerance)
|
|
|
|
if (std::fabs (pitch) > kPiOver2 - 1e-4)
|
|
{
|
|
// We are in gimbal lock. Assign all rotation
|
|
// about the vertical axis to heading
|
|
yaw += roll;
|
|
roll = 0.0;
|
|
}
|
|
else
|
|
{
|
|
// Not in gimbal lock. Wrap the bank angle in
|
|
// canonical range
|
|
roll = wrapPi (roll);
|
|
}
|
|
|
|
// Wrap heading in canonical range
|
|
yaw = wrapPi (yaw);
|
|
}
|
|
|
|
|
|
/////////////////////////////////////////////////////////////////////////////
|
|
//
|
|
// class Vector3<Real> implementation.
|
|
//
|
|
/////////////////////////////////////////////////////////////////////////////
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Vector3::isZero
|
|
//
|
|
// Return true if is zero vector.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline bool
|
|
Vector3<Real>::isZero ()
|
|
{
|
|
return (_x == 0.0) && (_y == 0.0) && (_z == 0.0);
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Vector3::normalize
|
|
//
|
|
// Set vector length to 1.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline void
|
|
Vector3<Real>::normalize()
|
|
{
|
|
Real magSq = (_x * _x) + (_y * _y) + (_z * _z);
|
|
|
|
if (magSq > 0.0)
|
|
{
|
|
// check for divide-by-zero
|
|
Real oneOverMag = 1.0 / std::sqrt (magSq);
|
|
_x *= oneOverMag;
|
|
_y *= oneOverMag;
|
|
_z *= oneOverMag;
|
|
}
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Vector3 operators
|
|
//
|
|
// Operator overloading for basic vector operations.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline bool
|
|
Vector3<Real>::operator== (const Vector3<Real> &v) const
|
|
{
|
|
return ((_x == v._x) && (_y == v._y) && (_z == v._z));
|
|
}
|
|
|
|
template <typename Real>
|
|
inline bool
|
|
Vector3<Real>::operator!= (const Vector3<Real> &v) const
|
|
{
|
|
return ((_x != v._x) || (_y != v._y) || (_z != v._z));
|
|
}
|
|
|
|
template <typename Real>
|
|
inline Vector3<Real>
|
|
Vector3<Real>::operator- () const
|
|
{
|
|
return Vector3<Real> (-_x, -_y, -_z);
|
|
}
|
|
|
|
template <typename Real>
|
|
inline Vector3<Real>
|
|
Vector3<Real>::operator+ (const Vector3<Real> &v) const
|
|
{
|
|
return Vector3<Real> (_x + v._x, _y + v._y, _z + v._z);
|
|
}
|
|
|
|
template <typename Real>
|
|
inline Vector3<Real>
|
|
Vector3<Real>::operator- (const Vector3<Real> &v) const
|
|
{
|
|
return Vector3<Real> (_x - v._x, _y - v._y, _z - v._z);
|
|
}
|
|
|
|
template <typename Real>
|
|
inline Vector3<Real>
|
|
Vector3<Real>::operator* (Real s) const
|
|
{
|
|
return Vector3<Real> (_x * s, _y * s, _z * s);
|
|
}
|
|
|
|
template <typename Real>
|
|
inline Vector3<Real>
|
|
Vector3<Real>::operator/ (Real s) const
|
|
{
|
|
Real oneOverS = 1.0 / s; // Note: no check for divide by zero
|
|
return Vector3<Real> (_x * oneOverS, _y * oneOverS, _z * oneOverS);
|
|
}
|
|
|
|
template <typename Real>
|
|
inline Vector3<Real> &
|
|
Vector3<Real>::operator+= (const Vector3<Real> &v)
|
|
{
|
|
_x += v._x; _y += v._y; _z += v._z;
|
|
return *this;
|
|
}
|
|
|
|
template <typename Real>
|
|
inline Vector3<Real> &
|
|
Vector3<Real>::operator-= (const Vector3<Real> &v)
|
|
{
|
|
_x -= v._x; _y -= v._y; _z -= v._z;
|
|
return *this;
|
|
}
|
|
|
|
template <typename Real>
|
|
inline Vector3<Real> &
|
|
Vector3<Real>::operator*= (Real s)
|
|
{
|
|
_x *= s; _y *= s; _z *= s;
|
|
return *this;
|
|
}
|
|
|
|
template <typename Real>
|
|
inline Vector3<Real> &
|
|
Vector3<Real>::operator/= (Real s)
|
|
{
|
|
Real oneOverS = 1.0 / s; // Note: no check for divide by zero!
|
|
_x *= oneOverS; _y *= oneOverS ; _z *= oneOverS;
|
|
return *this;
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
//
|
|
// Nonmember Vector3<Real> functions
|
|
//
|
|
// --------------------------------------------------------------------------
|
|
|
|
// Scalar on the left multiplication, for symmetry
|
|
template <typename Real>
|
|
inline Vector3<Real>
|
|
operator* (Real k, Vector3<Real> v)
|
|
{
|
|
return Vector3<Real> (k * v._x, k * v._y, k * v._z);
|
|
}
|
|
|
|
// Compute vector lenght
|
|
template <typename Real>
|
|
inline Real
|
|
VectorMag (const Vector3<Real> &v)
|
|
{
|
|
return std::sqrt ((v._x * v._x) + (v._y * v._y) + (v._z * v._z));
|
|
}
|
|
|
|
// Vector3 dot product
|
|
template <typename Real>
|
|
inline Real
|
|
DotProduct (const Vector3<Real> &a, const Vector3<Real> &b)
|
|
{
|
|
return ((a._x * b._x) + (a._y * b._y) + (a._z * b._z));
|
|
}
|
|
|
|
// Vector3 cross product
|
|
template <typename Real>
|
|
inline Vector3<Real>
|
|
CrossProduct (const Vector3<Real> &a, const Vector3<Real> &b)
|
|
{
|
|
return Vector3<Real> (
|
|
(a._y * b._z) - (a._z * b._y),
|
|
(a._z * b._x) - (a._x * b._z),
|
|
(a._x * b._y) - (a._y * b._x)
|
|
);
|
|
}
|
|
|
|
// Compute normal plane given three points
|
|
template <typename Real>
|
|
inline Vector3<Real>
|
|
ComputeNormal (const Vector3<Real> &p1, const Vector3<Real> &p2, const Vector3<Real> &p3)
|
|
{
|
|
Vector3<Real> vec1 (p1 - p2);
|
|
Vector3<Real> vec2 (p1 - p3);
|
|
|
|
Vector3<Real> result (CrossProduct (vec1, vec2));
|
|
result.normalize ();
|
|
|
|
return result;
|
|
}
|
|
|
|
// Compute distance between two points
|
|
template <typename Real>
|
|
inline Real
|
|
Distance (const Vector3<Real> &a, const Vector3<Real> &b)
|
|
{
|
|
Real dx = a._x - b._x;
|
|
Real dy = a._y - b._y;
|
|
Real dz = a._z - b._z;
|
|
return std::sqrt ((dx * dx) + (dy * dy) + (dz * dz));
|
|
}
|
|
|
|
// Compute squared distance between two points.
|
|
// Useful when comparing distances, since we don't need
|
|
// to square the result.
|
|
template <typename Real>
|
|
inline Real
|
|
DistanceSquared (const Vector3<Real> &a, const Vector3<Real> &b)
|
|
{
|
|
Real dx = a._x - b._x;
|
|
Real dy = a._y - b._y;
|
|
Real dz = a._z - b._z;
|
|
return ((dx * dx) + (dy * dy) + (dz * dz));
|
|
}
|
|
|
|
|
|
/////////////////////////////////////////////////////////////////////////////
|
|
//
|
|
// class Matrix4x4<Real> implementation.
|
|
//
|
|
// --------------------------------------------------------------------------
|
|
//
|
|
// MATRIX ORGANIZATION
|
|
//
|
|
// The purpose of this class is so that a user might perform transformations
|
|
// without fiddling with plus or minus signs or transposing the matrix
|
|
// until the output "looks right". But of course, the specifics of the
|
|
// internal representation is important. Not only for the implementation
|
|
// in this file to be correct, but occasionally direct access to the
|
|
// matrix variables is necessary, or beneficial for optimization. Thus,
|
|
// we document our matrix conventions here.
|
|
//
|
|
// Strict adherance to linear algebra rules dictates that the
|
|
// multiplication of a 4x4 matrix by a 3D vector is actually undefined.
|
|
// To circumvent this, we can consider the input and output vectors as
|
|
// having an assumed fourth coordinate of 1. Also, since the rightmost
|
|
// column is [ 0 0 0 1 ], we can simplificate calculations ignoring
|
|
// this last column. This is shown below:
|
|
//
|
|
// | m11 m12 m13 0 | | x | | x'|
|
|
// | m21 m22 m23 0 | | y | = | y'|
|
|
// | m31 m32 m33 0 | | z | | z'|
|
|
// | tx ty tz 1 | | 1 | | 1 |
|
|
//
|
|
// We use row vectors to represent the right, up and forward vectors
|
|
// in the 4x4 matrix. OpenGL uses column vectors, but the elements of
|
|
// an OpenGL matrix are ordered in columns so that m[i][j] is in row j
|
|
// and column i. This is the reverse of the standard C convention in
|
|
// which m[i][j] is in row i and column j. The matrix should be
|
|
// transposed before being sent to OpenGL.
|
|
//
|
|
// | m11 m21 m31 tx | | m0 m4 m8 m12 | | m0 m1 m2 m3 |
|
|
// | m12 m22 m32 ty | | m1 m5 m9 m13 | | m4 m5 m6 m7 |
|
|
// | m13 m23 m33 tz | | m2 m6 m10 m14 | | m8 m9 m10 m11 |
|
|
// | 0 0 0 tw | | m3 m7 m11 m15 | | m12 m13 m14 m15 |
|
|
//
|
|
// OpenGL style OpenGL matrix standard C
|
|
// arrangement convention
|
|
//
|
|
// Fortunately, accessing to the raw matrix data via the _m[] array gives
|
|
// us the transpose matrix; i.e. in OpenGL form, so that we can directly use
|
|
// it with glLoadMatrix{fd}() or glMultMatrix{fd}().
|
|
//
|
|
// Also, since the rightmost column (in standard C form) should always
|
|
// be [ 0 0 0 1 ], and sice these values (_h14, _h24, _h34 and _tw) are
|
|
// initialized in constructors, we don't need to modify them in our
|
|
// matrix operations, so we don't perform useless calculations...
|
|
//
|
|
// The right-hand rule is used to define "positive" rotation.
|
|
//
|
|
// +y +x'
|
|
// | |
|
|
// | |
|
|
// |______ +x +y' ______|
|
|
// / /
|
|
// / /
|
|
// +z +z'
|
|
//
|
|
// initial position Positive rotation of
|
|
// pi/2 around z-axis
|
|
//
|
|
/////////////////////////////////////////////////////////////////////////////
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Matrix4x4::identity
|
|
//
|
|
// Set matrix to identity.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline void
|
|
Matrix4x4<Real>::identity ()
|
|
{
|
|
_m11 = 1.0; _m21 = 0.0; _m31 = 0.0; _tx = 0.0;
|
|
_m12 = 0.0; _m22 = 1.0; _m32 = 0.0; _ty = 0.0;
|
|
_m13 = 0.0; _m23 = 0.0; _m33 = 1.0; _tz = 0.0;
|
|
_h14 = 0.0; _h24 = 0.0; _h34 = 0.0; _tw = 1.0;
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Matrix4x4::transpose
|
|
//
|
|
// Transpose the current matrix.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline void
|
|
Matrix4x4<Real>::transpose ()
|
|
{
|
|
*this = Transpose (*this);
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Matrix4x4::invert
|
|
//
|
|
// Invert the current matrix.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline void
|
|
Matrix4x4<Real>::invert ()
|
|
{
|
|
*this = Invert (*this);
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Matrix4x4::setTranslation
|
|
//
|
|
// Set the translation portion of the matrix.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline void
|
|
Matrix4x4<Real>::setTranslation (const Vector3<Real> &v)
|
|
{
|
|
_tx = v._x; _ty = v._y; _tz = v._z;
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Matrix4x4::transform
|
|
//
|
|
// Transform a point by the matrix.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline void
|
|
Matrix4x4<Real>::transform (Vector3<Real> &v) const
|
|
{
|
|
// Grind through the linear algebra.
|
|
v = Vector3<Real> (
|
|
(v._x * _m11) + (v._y * _m21) + (v._z * _m31) + _tx,
|
|
(v._x * _m12) + (v._y * _m22) + (v._z * _m32) + _ty,
|
|
(v._x * _m13) + (v._y * _m23) + (v._z * _m33) + _tz
|
|
);
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Matrix4x4::rotate
|
|
//
|
|
// Rotate a point by the 3x3 upper left portion of the matrix.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline void
|
|
Matrix4x4<Real>::rotate (Vector3<Real> &v) const
|
|
{
|
|
v = Vector3<Real> (
|
|
(v._x * _m11) + (v._y * _m21) + (v._z * _m31),
|
|
(v._x * _m12) + (v._y * _m22) + (v._z * _m32),
|
|
(v._x * _m13) + (v._y * _m23) + (v._z * _m33)
|
|
);
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Matrix4x4::inverseRotate
|
|
//
|
|
// Rotate a point by the inverse 3x3 upper left portion of the matrix.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline void
|
|
Matrix4x4<Real>::inverseRotate (Vector3<Real> &v) const
|
|
{
|
|
v = Vector3<Real> (
|
|
(v._x * _m11) + (v._y * _m12) + (v._z * _m13),
|
|
(v._x * _m21) + (v._y * _m22) + (v._z * _m23),
|
|
(v._x * _m31) + (v._y * _m32) + (v._z * _m33)
|
|
);
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Matrix4x4::inverseRotate
|
|
//
|
|
// Translate a point by the inverse matrix.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline void
|
|
Matrix4x4<Real>::inverseTranslate (Vector3<Real> &v) const
|
|
{
|
|
v._x -= _tx;
|
|
v._y -= _ty;
|
|
v._z -= _tz;
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Matrix4x4::fromQuaternion
|
|
//
|
|
// Convert a quaternion to a matrix.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline void
|
|
Matrix4x4<Real>::fromQuaternion (const Quaternion<Real> &q)
|
|
{
|
|
// Compute a few values to optimize common subexpressions
|
|
Real ww = 2.0 * q._w;
|
|
Real xx = 2.0 * q._x;
|
|
Real yy = 2.0 * q._y;
|
|
Real zz = 2.0 * q._z;
|
|
|
|
// Set the matrix elements. There is still a little more
|
|
// opportunity for optimization due to the many common
|
|
// subexpressions. We'll let the compiler handle that...
|
|
_m11 = 1.0 - (yy * q._y) - (zz * q._z);
|
|
_m12 = (xx * q._y) + (ww * q._z);
|
|
_m13 = (xx * q._z) - (ww * q._y);
|
|
|
|
_m21 = (xx * q._y) - (ww * q._z);
|
|
_m22 = 1.0 - (xx * q._x) - (zz * q._z);
|
|
_m23 = (yy * q._z) + (ww * q._x);
|
|
|
|
_m31 = (xx * q._z) + (ww * q._y);
|
|
_m32 = (yy * q._z) - (ww * q._x);
|
|
_m33 = 1.0 - (xx * q._x) - (yy * q._y);
|
|
|
|
// Reset the translation portion
|
|
_tx = _ty = _tz = 0.0;
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Matrix4x4::fromEulerAngles
|
|
//
|
|
// Setup a rotation matrix, given three X-Y-Z rotation angles. The
|
|
// rotations are performed first on x-axis, then y-axis and finaly z-axis.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline void
|
|
Matrix4x4<Real>::fromEulerAngles (Real x, Real y, Real z)
|
|
{
|
|
// Fetch sine and cosine of angles
|
|
Real cx = std::cos (x);
|
|
Real sx = std::sin (x);
|
|
Real cy = std::cos (y);
|
|
Real sy = std::sin (y);
|
|
Real cz = std::cos (z);
|
|
Real sz = std::sin (z);
|
|
|
|
Real sxsy = sx * sy;
|
|
Real cxsy = cx * sy;
|
|
|
|
// Fill in the matrix elements
|
|
_m11 = (cy * cz);
|
|
_m12 = (sxsy * cz) + (cx * sz);
|
|
_m13 = -(cxsy * cz) + (sx * sz);
|
|
|
|
_m21 = -(cy * sz);
|
|
_m22 = -(sxsy * sz) + (cx * cz);
|
|
_m23 = (cxsy * sz) + (sx * cz);
|
|
|
|
_m31 = (sy);
|
|
_m32 = -(sx * cy);
|
|
_m33 = (cx * cy);
|
|
|
|
// Reset the translation portion
|
|
_tx = _ty = _tz = 0.0;
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Matrix4x4::toEulerAngles
|
|
//
|
|
// Setup the euler angles in radians, given a rotation matrix. The rotation
|
|
// matrix could have been obtained from euler angles given the expression:
|
|
// M = X.Y.Z
|
|
// where X, Y and Z are rotation matrices about X, Y and Z axes.
|
|
// This is the "opposite" of the fromEulerAngles function.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline void
|
|
Matrix4x4<Real>::toEulerAngles (Real &x, Real &y, Real &z) const
|
|
{
|
|
// Compute Y-axis angle
|
|
y = std::asin (_m31);
|
|
|
|
// Compute cos and one over cos for optimization
|
|
Real cy = std::cos (y);
|
|
Real oneOverCosY = 1.0 / cy;
|
|
|
|
if (std::fabs (cy) > 0.001)
|
|
{
|
|
// No gimball lock
|
|
x = std::atan2 (-_m32 * oneOverCosY, _m33 * oneOverCosY);
|
|
z = std::atan2 (-_m21 * oneOverCosY, _m11 * oneOverCosY);
|
|
}
|
|
else
|
|
{
|
|
// Gimbal lock case
|
|
x = 0.0;
|
|
z = std::atan2 (_m12, _m22);
|
|
}
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Matrix4x4::rightVector
|
|
// Matrix4x4::upVector
|
|
// Matrix4x4::forwardVector
|
|
// Matrix4x4::translationVector
|
|
//
|
|
// Return a base vector from the matrix.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline Vector3<Real>
|
|
Matrix4x4<Real>::rightVector () const
|
|
{
|
|
return Vector3<Real> (_m11, _m12, _m13);
|
|
}
|
|
|
|
template <typename Real>
|
|
inline Vector3<Real>
|
|
Matrix4x4<Real>::upVector () const
|
|
{
|
|
return Vector3<Real> (_m21, _m22, _m23);
|
|
}
|
|
|
|
template <typename Real>
|
|
inline Vector3<Real>
|
|
Matrix4x4<Real>::forwardVector () const
|
|
{
|
|
return Vector3<Real> (_m31, _m32, _m33);
|
|
}
|
|
|
|
template <typename Real>
|
|
inline Vector3<Real>
|
|
Matrix4x4<Real>::translationVector () const
|
|
{
|
|
return Vector3<Real> (_tx, _ty, _tz);
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
//
|
|
// Nonmember Matrix4x4<Real> functions
|
|
//
|
|
// --------------------------------------------------------------------------
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Matrix4x4 * Matrix4x4
|
|
//
|
|
// Matrix concatenation.
|
|
//
|
|
// We also provide a *= operator, as per C convention.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline Matrix4x4<Real>
|
|
operator* (const Matrix4x4<Real> &a, const Matrix4x4<Real> &b)
|
|
{
|
|
Matrix4x4<Real> res;
|
|
|
|
// Compute the left 4x3 (linear transformation) portion
|
|
res._m11 = (a._m11 * b._m11) + (a._m21 * b._m12) + (a._m31 * b._m13);
|
|
res._m12 = (a._m12 * b._m11) + (a._m22 * b._m12) + (a._m32 * b._m13);
|
|
res._m13 = (a._m13 * b._m11) + (a._m23 * b._m12) + (a._m33 * b._m13);
|
|
|
|
res._m21 = (a._m11 * b._m21) + (a._m21 * b._m22) + (a._m31 * b._m23);
|
|
res._m22 = (a._m12 * b._m21) + (a._m22 * b._m22) + (a._m32 * b._m23);
|
|
res._m23 = (a._m13 * b._m21) + (a._m23 * b._m22) + (a._m33 * b._m23);
|
|
|
|
res._m31 = (a._m11 * b._m31) + (a._m21 * b._m32) + (a._m31 * b._m33);
|
|
res._m32 = (a._m12 * b._m31) + (a._m22 * b._m32) + (a._m32 * b._m33);
|
|
res._m33 = (a._m13 * b._m31) + (a._m23 * b._m32) + (a._m33 * b._m33);
|
|
|
|
// Compute the translation portion
|
|
res._tx = (a._m11 * b._tx) + (a._m21 * b._ty) + (a._m31 * b._tz) + a._tx;
|
|
res._ty = (a._m12 * b._tx) + (a._m22 * b._ty) + (a._m32 * b._tz) + a._ty;
|
|
res._tz = (a._m13 * b._tx) + (a._m23 * b._ty) + (a._m33 * b._tz) + a._tz;
|
|
|
|
return res;
|
|
}
|
|
|
|
template <typename Real>
|
|
inline Matrix4x4<Real> &
|
|
operator*= (Matrix4x4<Real> &a, const Matrix4x4<Real> &b)
|
|
{
|
|
a = a * b;
|
|
return a;
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Matrix4x4 * Vector3
|
|
//
|
|
// Transform a point by a matrix. This makes using the vector class look
|
|
// like it does with linear algebra notation on paper.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline Vector3<Real>
|
|
operator* (const Matrix4x4<Real> &m, const Vector3<Real> &p)
|
|
{
|
|
Vector3<Real> res (p);
|
|
m.transform (res);
|
|
return res;
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Transpose
|
|
//
|
|
// Return the transpose matrix.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline Matrix4x4<Real>
|
|
Transpose (const Matrix4x4<Real> &m)
|
|
{
|
|
Matrix4x4<Real> res;
|
|
|
|
res._m11 = m._m11; res._m21 = m._m12; res._m31 = m._m13; res._tx = m._h14;
|
|
res._m12 = m._m21; res._m22 = m._m22; res._m32 = m._m23; res._ty = m._h24;
|
|
res._m13 = m._m31; res._m23 = m._m32; res._m33 = m._m33; res._tz = m._h34;
|
|
res._h14 = m._tx; res._h24 = m._ty; res._h34 = m._tz; res._tw = m._tw;
|
|
|
|
return res;
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Determinant
|
|
//
|
|
// Compute the determinant of the 3x3 portion of the matrix.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline static Real
|
|
Determinant3x3 (const Matrix4x4<Real> &m)
|
|
{
|
|
return m._m11 * ((m._m22 * m._m33) - (m._m23 * m._m32))
|
|
+ m._m12 * ((m._m23 * m._m31) - (m._m21 * m._m33))
|
|
+ m._m13 * ((m._m21 * m._m32) - (m._m22 * m._m31));
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Invert
|
|
//
|
|
// Compute the inverse of a matrix. We use the classical adjoint divided
|
|
// by the determinant method.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline Matrix4x4<Real>
|
|
Invert (const Matrix4x4<Real> &m)
|
|
{
|
|
// Compute the determinant of the 3x3 portion
|
|
Real det = Determinant3x3 (m);
|
|
|
|
// If we're singular, then the determinant is zero and there's
|
|
// no inverse
|
|
assert (std::fabs (det) > 0.000001);
|
|
|
|
// Compute one over the determinant, so we divide once and
|
|
// can *multiply* per element
|
|
Real oneOverDet = 1.0 / det;
|
|
|
|
// Compute the 3x3 portion of the inverse, by
|
|
// dividing the adjoint by the determinant
|
|
Matrix4x4<Real> res;
|
|
|
|
res._m11 = ((m._m22 * m._m33) - (m._m23 * m._m32)) * oneOverDet;
|
|
res._m12 = ((m._m13 * m._m32) - (m._m12 * m._m33)) * oneOverDet;
|
|
res._m13 = ((m._m12 * m._m23) - (m._m13 * m._m22)) * oneOverDet;
|
|
|
|
res._m21 = ((m._m23 * m._m31) - (m._m21 * m._m33)) * oneOverDet;
|
|
res._m22 = ((m._m11 * m._m33) - (m._m13 * m._m31)) * oneOverDet;
|
|
res._m23 = ((m._m13 * m._m21) - (m._m11 * m._m23)) * oneOverDet;
|
|
|
|
res._m31 = ((m._m21 * m._m32) - (m._m22 * m._m31)) * oneOverDet;
|
|
res._m32 = ((m._m12 * m._m31) - (m._m11 * m._m32)) * oneOverDet;
|
|
res._m33 = ((m._m11 * m._m22) - (m._m12 * m._m21)) * oneOverDet;
|
|
|
|
// Compute the translation portion of the inverse
|
|
res._tx = -((m._tx * res._m11) + (m._ty * res._m21) + (m._tz * res._m31));
|
|
res._ty = -((m._tx * res._m12) + (m._ty * res._m22) + (m._tz * res._m32));
|
|
res._tz = -((m._tx * res._m13) + (m._ty * res._m23) + (m._tz * res._m33));
|
|
|
|
// Return it.
|
|
return res;
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// RotationMatrix
|
|
//
|
|
// Setup the matrix to perform a rotation about one of the three cardinal
|
|
// X-Y-Z axes.
|
|
//
|
|
// The axis of rotation is specified by the 1-based "axis" index.
|
|
//
|
|
// theta is the amount of rotation, in radians. The right-hand rule is
|
|
// used to define "positive" rotation.
|
|
//
|
|
// The translation portion is reset.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline Matrix4x4<Real>
|
|
RotationMatrix (Axis axis, Real theta)
|
|
{
|
|
Matrix4x4<Real> res;
|
|
|
|
// Get sin and cosine of rotation angle
|
|
Real s = std::sin (theta);
|
|
Real c = std::cos (theta);
|
|
|
|
// Check which axis they are rotating about
|
|
switch (axis)
|
|
{
|
|
case kXaxis: // Rotate about the x-axis
|
|
res._m11 = 1.0; res._m21 = 0.0; res._m31 = 0.0;
|
|
res._m12 = 0.0; res._m22 = c; res._m32 = -s;
|
|
res._m13 = 0.0; res._m23 = s; res._m33 = c;
|
|
break;
|
|
|
|
case kYaxis: // Rotate about the y-axis
|
|
res._m11 = c; res._m21 = 0.0; res._m31 = s;
|
|
res._m12 = 0.0; res._m22 = 1.0; res._m32 = 0.0;
|
|
res._m13 = -s; res._m23 = 0.0; res._m33 = c;
|
|
break;
|
|
|
|
case kZaxis: // Rotate about the z-axis
|
|
res._m11 = c; res._m21 = -s; res._m31 = 0.0;
|
|
res._m12 = s; res._m22 = c; res._m32 = 0.0;
|
|
res._m13 = 0.0; res._m23 = 0.0; res._m33 = 1.0;
|
|
break;
|
|
|
|
default:
|
|
// bogus axis index
|
|
assert (false);
|
|
}
|
|
|
|
// Reset the translation portion
|
|
res._tx = res._ty = res._tz = 0.0;
|
|
|
|
return res;
|
|
}
|
|
|
|
|
|
//---------------------------------------------------------------------------
|
|
// AxisRotationMatrix
|
|
//
|
|
// Setup the matrix to perform a rotation about an arbitrary axis.
|
|
// The axis of rotation must pass through the origin.
|
|
//
|
|
// axis defines the axis of rotation, and must be a unit vector.
|
|
//
|
|
// theta is the amount of rotation, in radians. The right-hand rule is
|
|
// used to define "positive" rotation.
|
|
//
|
|
// The translation portion is reset.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline Matrix4x4<Real>
|
|
RotationMatrix (const Vector3<Real> &axis, Real theta)
|
|
{
|
|
Matrix4x4<Real> res;
|
|
|
|
// Quick sanity check to make sure they passed in a unit vector
|
|
// to specify the axis
|
|
assert (std::fabs (DotProduct (axis, axis) - 1.0) < 0.001);
|
|
|
|
// Get sin and cosine of rotation angle
|
|
Real s = std::sin (theta);
|
|
Real c = std::cos (theta);
|
|
|
|
// Compute 1 - cos(theta) and some common subexpressions
|
|
Real a = 1.0 - c;
|
|
Real ax = a * axis._x;
|
|
Real ay = a * axis._y;
|
|
Real az = a * axis._z;
|
|
|
|
// Set the matrix elements. There is still a little more
|
|
// opportunity for optimization due to the many common
|
|
// subexpressions. We'll let the compiler handle that...
|
|
res._m11 = (ax * axis._x) + c;
|
|
res._m12 = (ax * axis._y) + (axis._z * s);
|
|
res._m13 = (ax * axis._z) - (axis._y * s);
|
|
|
|
res._m21 = (ay * axis._x) - (axis._z * s);
|
|
res._m22 = (ay * axis._y) + c;
|
|
res._m23 = (ay * axis._z) + (axis._x * s);
|
|
|
|
res._m31 = (az * axis._x) + (axis._y * s);
|
|
res._m32 = (az * axis._y) - (axis._x * s);
|
|
res._m33 = (az * axis._z) + c;
|
|
|
|
// Reset the translation portion
|
|
res._tx = res._ty = res._tz = 0.0;
|
|
|
|
return res;
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// TranslationMatrix
|
|
//
|
|
// Build a translation matrix given a translation vector.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline Matrix4x4<Real>
|
|
TranslationMatrix (Real x, Real y, Real z)
|
|
{
|
|
return TranslationMatrix (Vector3<Real> (x, y, z));
|
|
}
|
|
|
|
template <typename Real>
|
|
inline Matrix4x4<Real>
|
|
TranslationMatrix (const Vector3<Real> &v)
|
|
{
|
|
Matrix4x4<Real> res;
|
|
|
|
res._m11 = 1.0; res._m21 = 0.0; res._m31 = 0.0; res._tx = v._x;
|
|
res._m12 = 0.0; res._m22 = 1.0; res._m32 = 0.0; res._ty = v._y;
|
|
res._m13 = 0.0; res._m23 = 0.0; res._m33 = 1.0; res._tz = v._z;
|
|
|
|
return res;
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// ScaleMatrix
|
|
//
|
|
// Setup the matrix to perform scale on each axis. For uniform scale by k,
|
|
// use a vector of the form Vector3( k, k, k ).
|
|
//
|
|
// The translation portion is reset.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline Matrix4x4<Real>
|
|
ScaleMatrix (const Vector3<Real> &s)
|
|
{
|
|
Matrix4x4<Real> res;
|
|
|
|
// Set the matrix elements. Pretty straightforward
|
|
res._m11 = s._x; res._m21 = 0.0; res._m31 = 0.0;
|
|
res._m12 = 0.0; res._m22 = s._y; res._m32 = 0.0;
|
|
res._m13 = 0.0; res._m23 = 0.0; res._m33 = s._z;
|
|
|
|
// Reset the translation portion
|
|
res._tx = res._ty = res._tz = 0.0;
|
|
|
|
return res;
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// ScaleAlongAxisMatrix
|
|
//
|
|
// Setup the matrix to perform scale along an arbitrary axis.
|
|
//
|
|
// The axis is specified using a unit vector.
|
|
//
|
|
// The translation portion is reset.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline Matrix4x4<Real>
|
|
ScaleAlongAxisMatrix (const Vector3<Real> &axis, Real k)
|
|
{
|
|
Matrix4x4<Real> res;
|
|
|
|
// Quick sanity check to make sure they passed in a unit vector
|
|
// to specify the axis
|
|
assert (std::fabs (DotProduct (axis, axis) - 1.0) < 0.001);
|
|
|
|
// Compute k-1 and some common subexpressions
|
|
Real a = k - 1.0;
|
|
Real ax = a * axis._x;
|
|
Real ay = a * axis._y;
|
|
Real az = a * axis._z;
|
|
|
|
// Fill in the matrix elements. We'll do the common
|
|
// subexpression optimization ourselves here, since diagonally
|
|
// opposite matrix elements are equal
|
|
res._m11 = (ax * axis._x) + 1.0;
|
|
res._m22 = (ay * axis._y) + 1.0;
|
|
res._m32 = (az * axis._z) + 1.0;
|
|
|
|
res._m12 = res._m21 = (ax * axis._y);
|
|
res._m13 = res._m31 = (ax * axis._z);
|
|
res._m23 = res._m32 = (ay * axis._z);
|
|
|
|
// Reset the translation portion
|
|
res._tx = res._ty = res._tz = 0.0;
|
|
|
|
return res;
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// ShearMatrix
|
|
//
|
|
// Setup the matrix to perform a shear
|
|
//
|
|
// The type of shear is specified by the 1-based "axis" index. The effect
|
|
// of transforming a point by the matrix is described by the pseudocode
|
|
// below:
|
|
//
|
|
// xAxis => y += s * x, z += t * x
|
|
// yAxis => x += s * y, z += t * y
|
|
// zAxis => x += s * z, y += t * z
|
|
//
|
|
// The translation portion is reset.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline Matrix4x4<Real>
|
|
ShearMatrix (Axis axis, Real s, Real t)
|
|
{
|
|
Matrix4x4<Real> res;
|
|
|
|
// Check which type of shear they want
|
|
switch (axis)
|
|
{
|
|
case kXaxis: // Shear y and z using x
|
|
res._m11 = 1.0; res._m21 = 0.0; res._m31 = 0.0;
|
|
res._m12 = s; res._m22 = 1.0; res._m32 = 0.0;
|
|
res._m13 = t; res._m23 = 0.0; res._m33 = 1.0;
|
|
break;
|
|
|
|
case kYaxis: // Shear x and z using y
|
|
res._m11 = 1.0; res._m21 = s; res._m31 = 0.0;
|
|
res._m12 = 0.0; res._m22 = 1.0; res._m32 = 0.0;
|
|
res._m13 = 0.0; res._m23 = t; res._m33 = 1.0;
|
|
break;
|
|
|
|
case kZaxis: // Shear x and y using z
|
|
res._m11 = 1.0; res._m21 = 0.0; res._m31 = s;
|
|
res._m12 = 0.0; res._m22 = 1.0; res._m32 = t;
|
|
res._m13 = 0.0; res._m23 = 0.0; res._m33 = 1.0;
|
|
break;
|
|
|
|
default:
|
|
// bogus axis index
|
|
assert (false);
|
|
}
|
|
|
|
// Reset the translation portion
|
|
res._tx = res._ty = res._tz = 0.0;
|
|
|
|
return res;
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// ProjectionMatrix
|
|
//
|
|
// Setup the matrix to perform a projection onto a plane passing
|
|
// through the origin. The plane is perpendicular to the
|
|
// unit vector n.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline Matrix4x4<Real>
|
|
ProjectionMatrix (const Vector3<Real> &n)
|
|
{
|
|
Matrix4x4<Real> res;
|
|
|
|
// Quick sanity check to make sure they passed in a unit vector
|
|
// to specify the axis
|
|
assert (std::fabs (DotProduct (n, n) - 1.0) < 0.001);
|
|
|
|
// Fill in the matrix elements. We'll do the common
|
|
// subexpression optimization ourselves here, since diagonally
|
|
// opposite matrix elements are equal
|
|
res._m11 = 1.0 - (n._x * n._x);
|
|
res._m22 = 1.0 - (n._y * n._y);
|
|
res._m33 = 1.0 - (n._z * n._z);
|
|
|
|
res._m12 = res._m21 = -(n._x * n._y);
|
|
res._m13 = res._m31 = -(n._x * n._z);
|
|
res._m23 = res._m32 = -(n._y * n._z);
|
|
|
|
// Reset the translation portion
|
|
res._tx = res._ty = res._tz = 0.0;
|
|
|
|
return res;
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// ReflectionMatrix
|
|
//
|
|
// Setup the matrix to perform a reflection about a plane parallel
|
|
// to a cardinal plane.
|
|
//
|
|
// axis is a 1-based index which specifies the plane to project about:
|
|
//
|
|
// xAxis => reflect about the plane x=k
|
|
// yAxis => reflect about the plane y=k
|
|
// zAxis => reflect about the plane z=k
|
|
//
|
|
// The translation is set appropriately, since translation must occur if
|
|
// k != 0
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline Matrix4x4<Real>
|
|
ReflectionMatrix (Axis axis, Real k)
|
|
{
|
|
Matrix4x4<Real> res;
|
|
|
|
// Check which plane they want to reflect about
|
|
switch (axis)
|
|
{
|
|
case kXaxis: // Reflect about the plane x=k
|
|
res._m11 = -1.0; res._m21 = 0.0; res._m31 = 0.0; res._tx = 2.0 * k;
|
|
res._m12 = 0.0; res._m22 = 1.0; res._m32 = 0.0; res._ty = 0.0;
|
|
res._m13 = 0.0; res._m23 = 0.0; res._m33 = 1.0; res._tz = 0.0;
|
|
break;
|
|
|
|
case kYaxis: // Reflect about the plane y=k
|
|
res._m11 = 1.0; res._m21 = 0.0; res._m31 = 0.0; res._tx = 0.0;
|
|
res._m12 = 0.0; res._m22 = -1.0; res._m32 = 0.0; res._ty = 2.0 * k;
|
|
res._m13 = 0.0; res._m23 = 0.0; res._m33 = 1.0; res._tz = 0.0;
|
|
break;
|
|
|
|
case kZaxis: // Reflect about the plane z=k
|
|
res._m11 = 1.0; res._m21 = 0.0; res._m31 = 0.0; res._tx = 0.0;
|
|
res._m12 = 0.0; res._m22 = 1.0; res._m32 = 0.0; res._ty = 0.0;
|
|
res._m13 = 0.0; res._m23 = 0.0; res._m33 = -1.0; res._tz = 2.0 * k;
|
|
break;
|
|
|
|
default:
|
|
// bogus axis index
|
|
assert (false);
|
|
}
|
|
|
|
return res;
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// AxisReflectionMatrix
|
|
//
|
|
// Setup the matrix to perform a reflection about an arbitrary plane
|
|
// through the origin. The unit vector n is perpendicular to the plane.
|
|
//
|
|
// The translation portion is reset.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline Matrix4x4<Real>
|
|
AxisReflectionMatrix (const Vector3<Real> &n)
|
|
{
|
|
Matrix4x4<Real> res;
|
|
|
|
// Quick sanity check to make sure they passed in a unit vector
|
|
// to specify the axis
|
|
assert (std::fabs (DotProduct (n, n) - 1.0) < 0.001);
|
|
|
|
// Compute common subexpressions
|
|
Real ax = -2.0 * n._x;
|
|
Real ay = -2.0 * n._y;
|
|
Real az = -2.0 * n._z;
|
|
|
|
// Fill in the matrix elements. We'll do the common
|
|
// subexpression optimization ourselves here, since diagonally
|
|
// opposite matrix elements are equal
|
|
res._m11 = 1.0 + (ax * n._x);
|
|
res._m22 = 1.0 + (ay * n._y);
|
|
res._m32 = 1.0 + (az * n._z);
|
|
|
|
res._m12 = res._m21 = (ax * n._y);
|
|
res._m13 = res._m31 = (ax * n._z);
|
|
res._m23 = res._m32 = (ay * n._z);
|
|
|
|
// Reset the translation portion
|
|
res._tx = res._ty = res._tz = 0.00;
|
|
|
|
return res;
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// LookAtMatrix
|
|
//
|
|
// Setup the matrix to perform a "Look At" transformation like a first
|
|
// person camera.
|
|
// --------------------------------------------------------------------------
|
|
template <typename Real>
|
|
inline Matrix4x4<Real>
|
|
LookAtMatrix (const Vector3<Real> &camPos, const Vector3<Real> &target,
|
|
const Vector3<Real> &camUp)
|
|
{
|
|
Matrix4x4<Real> rot, trans;
|
|
|
|
Vector3<Real> forward (camPos - target);
|
|
forward.normalize ();
|
|
|
|
Vector3<Real> right (CrossProduct (camUp, forward));
|
|
Vector3<Real> up (CrossProduct (forward, right));
|
|
|
|
right.normalize ();
|
|
up.normalize ();
|
|
|
|
rot._m11 = right._x;
|
|
rot._m21 = right._y;
|
|
rot._m31 = right._z;
|
|
|
|
rot._m12 = up._x;
|
|
rot._m22 = up._y;
|
|
rot._m32 = up._z;
|
|
|
|
rot._m13 = forward._x;
|
|
rot._m23 = forward._y;
|
|
rot._m33 = forward._z;
|
|
|
|
rot._tx = 0.0;
|
|
rot._ty = 0.0;
|
|
rot._tz = 0.0;
|
|
|
|
trans = TranslationMatrix (-camPos);
|
|
|
|
return (rot * trans);
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// FrustumMatrix
|
|
//
|
|
// Setup a frustum matrix given the left, right, bottom, top, near, and far
|
|
// values for the frustum boundaries.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline Matrix4x4<Real>
|
|
FrustumMatrix (Real l, Real r, Real b, Real t, Real n, Real f)
|
|
{
|
|
assert (n >= 0.0);
|
|
assert (f >= 0.0);
|
|
|
|
Matrix4x4<Real> res;
|
|
|
|
Real width = r - l;
|
|
Real height = t - b;
|
|
Real depth = f - n;
|
|
|
|
res._m[0] = (2 * n) / width;
|
|
res._m[1] = 0.0;
|
|
res._m[2] = 0.0;
|
|
res._m[3] = 0.0;
|
|
|
|
res._m[4] = 0.0;
|
|
res._m[5] = (2 * n) / height;
|
|
res._m[6] = 0.0;
|
|
res._m[7] = 0.0;
|
|
|
|
res._m[8] = (r + l) / width;
|
|
res._m[9] = (t + b) / height;
|
|
res._m[10]= -(f + n) / depth;
|
|
res._m[11]= -1.0;
|
|
|
|
res._m[12]= 0.0;
|
|
res._m[13]= 0.0;
|
|
res._m[14]= -(2 * f * n) / depth;
|
|
res._m[15]= 0.0;
|
|
|
|
return res;
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// PerspectiveMatrix
|
|
//
|
|
// Setup a perspective matrix given the field-of-view in the Y direction
|
|
// in degrees, the aspect ratio of Y/X, and near and far plane distances.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline Matrix4x4<Real>
|
|
PerspectiveMatrix (Real fovY, Real aspect, Real n, Real f)
|
|
{
|
|
Matrix4x4<Real> res;
|
|
|
|
Real angle;
|
|
Real cot;
|
|
|
|
angle = fovY / 2.0;
|
|
angle = degToRad (angle);
|
|
|
|
cot = std::cos (angle) / std::sin (angle);
|
|
|
|
res._m[0] = cot / aspect;
|
|
res._m[1] = 0.0;
|
|
res._m[2] = 0.0;
|
|
res._m[3] = 0.0;
|
|
|
|
res._m[4] = 0.0;
|
|
res._m[5] = cot;
|
|
res._m[6] = 0.0;
|
|
res._m[7] = 0.0;
|
|
|
|
res._m[8] = 0.0;
|
|
res._m[9] = 0.0;
|
|
res._m[10]= -(f + n) / (f - n);
|
|
res._m[11]= -1.0;
|
|
|
|
res._m[12]= 0.0;
|
|
res._m[13]= 0.0;
|
|
res._m[14]= -(2 * f * n) / (f - n);
|
|
res._m[15]= 0.0;
|
|
|
|
return res;
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// OrthoMatrix
|
|
//
|
|
// Setup an orthographic Matrix4x4 given the left, right, bottom, top, near,
|
|
// and far values for the frustum boundaries.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline Matrix4x4<Real>
|
|
OrthoMatrix (Real l, Real r, Real b, Real t, Real n, Real f)
|
|
{
|
|
Matrix4x4<Real> res;
|
|
|
|
Real width = r - l;
|
|
Real height = t - b;
|
|
Real depth = f - n;
|
|
|
|
res._m[0] = 2.0 / width;
|
|
res._m[1] = 0.0;
|
|
res._m[2] = 0.0;
|
|
res._m[3] = 0.0;
|
|
|
|
res._m[4] = 0.0;
|
|
res._m[5] = 2.0 / height;
|
|
res._m[6] = 0.0;
|
|
res._m[7] = 0.0;
|
|
|
|
res._m[8] = 0.0;
|
|
res._m[9] = 0.0;
|
|
res._m[10]= -2.0 / depth;
|
|
res._m[11]= 0.0;
|
|
|
|
res._m[12]= -(r + l) / width;
|
|
res._m[13]= -(t + b) / height;
|
|
res._m[14]= -(f + n) / depth;
|
|
res._m[15]= 1.0;
|
|
|
|
return res;
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// OrthoNormalMatrix
|
|
//
|
|
// Setup an orientation matrix using 3 basis normalized vectors.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline Matrix4x4<Real>
|
|
OrthoNormalMatrix (const Vector3<Real> &xdir, const Vector3<Real> &ydir,
|
|
const Vector3<Real> &zdir)
|
|
{
|
|
Matrix4x4<Real> res;
|
|
|
|
res._m[0] = xdir._x; res._m[4] = ydir._x; res._m[8] = zdir._x; res._m[12] = 0.0;
|
|
res._m[1] = xdir._y; res._m[5] = ydir._y; res._m[9] = zdir._y; res._m[13] = 0.0;
|
|
res._m[2] = xdir._z; res._m[6] = ydir._z; res._m[10]= zdir._z; res._m[14] = 0.0;
|
|
res._m[3] = 0.0; res._m[7] = 0.0; res._m[11]= 0.0; res._m[15] = 1.0;
|
|
|
|
return res;
|
|
}
|
|
|
|
|
|
/////////////////////////////////////////////////////////////////////////////
|
|
//
|
|
// class Quaternion<Real> implementation.
|
|
//
|
|
/////////////////////////////////////////////////////////////////////////////
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Quaternion::identity
|
|
//
|
|
// Set to identity
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline void
|
|
Quaternion<Real>::identity ()
|
|
{
|
|
_w = 1.0; _x = _y = _z = 0.0;
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Quaternion::normalize
|
|
//
|
|
// "Normalize" a quaternion. Note that normally, quaternions
|
|
// are always normalized (within limits of numerical precision).
|
|
//
|
|
// This function is provided primarily to combat floating point "error
|
|
// creep", which can occur when many successive quaternion operations
|
|
// are applied.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline void
|
|
Quaternion<Real>::normalize ()
|
|
{
|
|
// Compute magnitude of the quaternion
|
|
Real mag = std::sqrt ((_w * _w) + (_x * _x) + (_y * _y) + (_z * _z));
|
|
|
|
// Check for bogus length, to protect against divide by zero
|
|
if (mag > 0.0)
|
|
{
|
|
// Normalize it
|
|
Real oneOverMag = 1.0 / mag;
|
|
|
|
_w *= oneOverMag;
|
|
_x *= oneOverMag;
|
|
_y *= oneOverMag;
|
|
_z *= oneOverMag;
|
|
}
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Quaternion<Real>::computeW
|
|
//
|
|
// Compute the W component of a unit quaternion given its x,y,z components.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline void
|
|
Quaternion<Real>::computeW ()
|
|
{
|
|
Real t = 1.0 - (_x * _x) - (_y * _y) - (_z * _z);
|
|
|
|
if (t < 0.0)
|
|
_w = 0.0;
|
|
else
|
|
_w = -std::sqrt (t);
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Quaternion<Real>::rotate
|
|
//
|
|
// Rotate a point by quaternion. v' = q.p.q*, where p = <0, v>.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline void
|
|
Quaternion<Real>::rotate (Vector3<Real> &v) const
|
|
{
|
|
Quaternion<Real> qf = *this * v * ~(*this);
|
|
v._x = qf._x;
|
|
v._y = qf._y;
|
|
v._z = qf._z;
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Quaternion::fromMatrix
|
|
//
|
|
// Setup the quaternion to perform a rotation, given the angular displacement
|
|
// in matrix form.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline void
|
|
Quaternion<Real>::fromMatrix (const Matrix4x4<Real> &m)
|
|
{
|
|
Real trace = m._m11 + m._m22 + m._m33 + 1.0;
|
|
|
|
if (trace > 0.0001)
|
|
{
|
|
Real s = 0.5 / std::sqrt (trace);
|
|
_w = 0.25 / s;
|
|
_x = (m._m23 - m._m32) * s;
|
|
_y = (m._m31 - m._m13) * s;
|
|
_z = (m._m12 - m._m21) * s;
|
|
}
|
|
else
|
|
{
|
|
if ((m._m11 > m._m22) && (m._m11 > m._m33))
|
|
{
|
|
Real s = 0.5 / std::sqrt (1.0 + m._m11 - m._m22 - m._m33);
|
|
_x = 0.25 / s;
|
|
_y = (m._m21 + m._m12) * s;
|
|
_z = (m._m31 + m._m13) * s;
|
|
_w = (m._m32 - m._m23) * s;
|
|
}
|
|
else if (m._m22 > m._m33)
|
|
{
|
|
Real s = 0.5 / std::sqrt (1.0 + m._m22 - m._m11 - m._m33);
|
|
_x = (m._m21 + m._m12) * s;
|
|
_y = 0.25 / s;
|
|
_z = (m._m32 + m._m23) * s;
|
|
_w = (m._m31 - m._m13) * s;
|
|
}
|
|
else
|
|
{
|
|
Real s = 0.5 / std::sqrt (1.0 + m._m33 - m._m11 - m._m22);
|
|
_x = (m._m31 + m._m13) * s;
|
|
_y = (m._m32 + m._m23) * s;
|
|
_z = 0.25 / s;
|
|
_w = (m._m21 - m._m12) * s;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Quaternion::fromEulerAngles
|
|
//
|
|
// Setup the quaternion to perform an object->inertial rotation, given the
|
|
// orientation in XYZ-Euler angles format. x,y,z parameters must be in
|
|
// radians.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline void
|
|
Quaternion<Real>::fromEulerAngles (Real x, Real y, Real z)
|
|
{
|
|
// Compute sine and cosine of the half angles
|
|
Real sr = std::sin (x * 0.5);
|
|
Real cr = std::cos (x * 0.5);
|
|
Real sp = std::sin (y * 0.5);
|
|
Real cp = std::cos (y * 0.5);
|
|
Real sy = std::sin (z * 0.5);
|
|
Real cy = std::cos (z * 0.5);
|
|
|
|
// Compute values
|
|
_w = (cy * cp * cr) + (sy * sp * sr);
|
|
_x = -(sy * sp * cr) + (cy * cp * sr);
|
|
_y = (cy * sp * cr) + (sy * cp * sr);
|
|
_z = -(cy * sp * sr) + (sy * cp * cr);
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Quaternion::toEulerAngles
|
|
//
|
|
// Setup the Euler angles, given an object->inertial rotation quaternion.
|
|
// Returned x,y,z are in radians.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline void
|
|
Quaternion<Real>::toEulerAngles (Real &x, Real &y, Real &z) const
|
|
{
|
|
// Compute Y-axis angle
|
|
y = std::asin (2.0 * ((_x * _z) + (_w * _y)));
|
|
|
|
// Compute cos and one over cos for optimization
|
|
Real cy = std::cos (y);
|
|
Real oneOverCosY = 1.0 / cy;
|
|
|
|
if (std::fabs (cy) > 0.001)
|
|
{
|
|
// No gimball lock
|
|
x = std::atan2 (2.0 * ((_w * _x) - (_y * _z)) * oneOverCosY,
|
|
(1.0 - 2.0 * (_x*_x + _y*_y)) * oneOverCosY);
|
|
z = std::atan2 (2.0 * ((_w * _z) - (_x * _y)) * oneOverCosY,
|
|
(1.0 - 2.0 * (_y*_y + _z*_z)) * oneOverCosY);
|
|
}
|
|
else
|
|
{
|
|
// Gimbal lock case
|
|
x = 0.0;
|
|
z = std::atan2 (2.0 * ((_x * _y) + (_w * _z)),
|
|
1.0 - 2.0 * (_x*_x + _z*_z));
|
|
}
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Quaternion::getRotationAngle
|
|
//
|
|
// Return the rotation angle theta (in radians).
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline Real
|
|
Quaternion<Real>::rotationAngle () const
|
|
{
|
|
// Compute the half angle. Remember that w = cos(theta / 2)
|
|
Real thetaOver2 = safeAcos (_w);
|
|
|
|
// Return the rotation angle
|
|
return thetaOver2 * 2.0;
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Quaternion::getRotationAxis
|
|
//
|
|
// Return the rotation axis.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline Vector3<Real>
|
|
Quaternion<Real>::rotationAxis () const
|
|
{
|
|
// Compute sin^2(theta/2). Remember that w = cos(theta/2),
|
|
// and sin^2(x) + cos^2(x) = 1
|
|
Real sinThetaOver2Sq = 1.0 - (_w * _w);
|
|
|
|
// Protect against numerical imprecision
|
|
if (sinThetaOver2Sq <= 0.0)
|
|
{
|
|
// Identity quaternion, or numerical imprecision. Just
|
|
// return any valid vector, since it doesn't matter
|
|
|
|
return Vector3<Real> (1.0, 0.0, 0.0);
|
|
}
|
|
|
|
// Compute 1 / sin(theta/2)
|
|
Real oneOverSinThetaOver2 = 1.0 / std::sqrt (sinThetaOver2Sq);
|
|
|
|
// Return axis of rotation
|
|
return Vector3<Real> (
|
|
_x * oneOverSinThetaOver2,
|
|
_y * oneOverSinThetaOver2,
|
|
_z * oneOverSinThetaOver2
|
|
);
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Quaternion operators
|
|
//
|
|
// Operator overloading for basic quaternion operations.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline Quaternion<Real>
|
|
Quaternion<Real>::operator+ (const Quaternion<Real> &q) const
|
|
{
|
|
return Quaternion<Real> (_w + q._w, _x + q._x, _y + q._y, _z + q._z);
|
|
}
|
|
|
|
template <typename Real>
|
|
inline Quaternion<Real> &
|
|
Quaternion<Real>::operator+= (const Quaternion<Real> &q)
|
|
{
|
|
_w += q._w; _x += q._x; _y += q._y; _z += q._z;
|
|
return *this;
|
|
}
|
|
|
|
template <typename Real>
|
|
inline Quaternion<Real>
|
|
Quaternion<Real>::operator- (const Quaternion<Real> &q) const
|
|
{
|
|
return Quaternion<Real> (_w - q._w, _x - q._x, _y - q._y, _z - q._z);
|
|
}
|
|
|
|
template <typename Real>
|
|
inline Quaternion<Real> &
|
|
Quaternion<Real>::operator-= (const Quaternion<Real> &q)
|
|
{
|
|
_w -= q._w; _x -= q._x; _y -= q._y; _z -= q._z;
|
|
return *this;
|
|
}
|
|
|
|
// Quaternion multiplication. The order of multiplication, from left
|
|
// to right, corresponds to the order of concatenated rotations.
|
|
// NOTE: Quaternion multiplication is NOT commutative, p * q != q * p
|
|
template <typename Real>
|
|
inline Quaternion<Real>
|
|
Quaternion<Real>::operator* (const Quaternion<Real> &q) const
|
|
{
|
|
// We use the Grassman product formula:
|
|
// pq = <w1w2 - dot(v1, v2), w1v2 + w2v1 + cross(v1, v2)>
|
|
return Quaternion<Real> (
|
|
(_w * q._w) - (_x * q._x) - (_y * q._y) - (_z * q._z),
|
|
(_x * q._w) + (_w * q._x) + (_y * q._z) - (_z * q._y),
|
|
(_y * q._w) + (_w * q._y) + (_z * q._x) - (_x * q._z),
|
|
(_z * q._w) + (_w * q._z) + (_x * q._y) - (_y * q._x)
|
|
);
|
|
}
|
|
|
|
template <typename Real>
|
|
inline Quaternion<Real> &
|
|
Quaternion<Real>::operator*= (const Quaternion<Real> &q)
|
|
{
|
|
*this = *this * q;
|
|
return *this;
|
|
}
|
|
|
|
template <typename Real>
|
|
inline Quaternion<Real>
|
|
Quaternion<Real>::operator* (const Vector3<Real> &v) const
|
|
{
|
|
// q * v = q * p where p = <0,v>
|
|
// Thus, we can simplify the operations.
|
|
return Quaternion<Real> (
|
|
- (_x * v._x) - (_y * v._y) - (_z * v._z),
|
|
(_w * v._x) + (_y * v._z) - (_z * v._y),
|
|
(_w * v._y) + (_z * v._x) - (_x * v._z),
|
|
(_w * v._z) + (_x * v._y) - (_y * v._x)
|
|
);
|
|
}
|
|
|
|
template <typename Real>
|
|
inline Quaternion<Real> &
|
|
Quaternion<Real>::operator*= (const Vector3<Real> &v)
|
|
{
|
|
*this = *this * v;
|
|
return *this;
|
|
}
|
|
|
|
template <typename Real>
|
|
inline Quaternion<Real>
|
|
Quaternion<Real>::operator* (Real k) const
|
|
{
|
|
return Quaternion<Real> (_w * k, _x * k, _y * k, _z * k);
|
|
}
|
|
|
|
template <typename Real>
|
|
inline Quaternion<Real> &
|
|
Quaternion<Real>::operator*= (Real k)
|
|
{
|
|
_w *= k; _x *= k; _y *= k; _z *= k;
|
|
return *this;
|
|
}
|
|
|
|
template <typename Real>
|
|
inline Quaternion<Real>
|
|
Quaternion<Real>::operator/ (Real k) const
|
|
{
|
|
Real oneOverK = 1.0 / k;
|
|
return Quaternion<Real> (_w * oneOverK, _x * oneOverK, _y * oneOverK, _z * oneOverK);
|
|
}
|
|
|
|
template <typename Real>
|
|
inline Quaternion<Real> &
|
|
Quaternion<Real>::operator/= (Real k)
|
|
{
|
|
Real oneOverK = 1.0 / k;
|
|
_w *= oneOverK; _x *= oneOverK; _y *= oneOverK; _z *= oneOverK;
|
|
return *this;
|
|
}
|
|
|
|
// Quaternion conjugate
|
|
template <typename Real>
|
|
inline Quaternion<Real>
|
|
Quaternion<Real>::operator~ () const
|
|
{
|
|
return Quaternion<Real> (_w, -_x, -_y, -_z);
|
|
}
|
|
|
|
|
|
// Quaternion negation
|
|
template <typename Real>
|
|
inline Quaternion<Real>
|
|
Quaternion<Real>::operator- () const
|
|
{
|
|
return Quaternion<Real> (-_w, -_x, -_y, -_z);
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
//
|
|
// Nonmember Quaternion functions
|
|
//
|
|
// --------------------------------------------------------------------------
|
|
|
|
// Scalar on left multiplication
|
|
template <typename Real>
|
|
inline Quaternion<Real>
|
|
operator* (Real k, const Quaternion<Real> &q)
|
|
{
|
|
return Quaternion<Real> (q._w * k, q._x * k, q._y * k, q._z * k);
|
|
}
|
|
|
|
// Quaternion dot product
|
|
template <typename Real>
|
|
inline Real
|
|
DotProduct (const Quaternion<Real> &a, const Quaternion<Real> &b)
|
|
{
|
|
return ((a._w * b._w) + (a._x * b._x) + (a._y * b._y) + (a._z * b._z));
|
|
}
|
|
|
|
// Compute the quaternion conjugate. This is the quaternian
|
|
// with the opposite rotation as the original quaternion.
|
|
template <typename Real>
|
|
inline Quaternion<Real>
|
|
Conjugate (const Quaternion<Real> &q)
|
|
{
|
|
return Quaternion<Real> (q._w, -q._x, -q._y, -q._z);
|
|
}
|
|
|
|
|
|
// Compute the inverse quaternion (for unit quaternion only).
|
|
template <typename Real>
|
|
inline Quaternion<Real>
|
|
Inverse (const Quaternion<Real> &q)
|
|
{
|
|
// Assume this is a unit quaternion! No check for this!
|
|
Quaternion<Real> res (q._w, -q._x, -q._y, -q._z);
|
|
res.normalize ();
|
|
return res;
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// RotationQuaternion
|
|
//
|
|
// Setup the quaternion to rotate about the specified axis. theta must
|
|
// be in radians.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline Quaternion<Real>
|
|
RotationQuaternion (Axis axis, Real theta)
|
|
{
|
|
Quaternion<Real> res;
|
|
|
|
// Compute the half angle
|
|
Real thetaOver2 = theta * 0.5;
|
|
|
|
// Set the values
|
|
switch (axis)
|
|
{
|
|
case kXaxis:
|
|
res._w = std::cos (thetaOver2);
|
|
res._x = std::sin (thetaOver2);
|
|
res._y = 0.0;
|
|
res._z = 0.0;
|
|
break;
|
|
|
|
case kYaxis:
|
|
res._w = std::cos (thetaOver2);
|
|
res._x = 0.0;
|
|
res._y = std::sin (thetaOver2);
|
|
res._z = 0.0;
|
|
break;
|
|
|
|
case kZaxis:
|
|
res._w = std::cos (thetaOver2);
|
|
res._x = 0.0;
|
|
res._y = 0.0;
|
|
res._z = std::sin (thetaOver2);
|
|
break;
|
|
|
|
default:
|
|
// Bad axis
|
|
assert (false);
|
|
}
|
|
|
|
return res;
|
|
}
|
|
|
|
template <typename Real>
|
|
inline Quaternion<Real>
|
|
RotationQuaternion (const Vector3<Real> &axis, Real theta)
|
|
{
|
|
Quaternion<Real> res;
|
|
|
|
// The axis of rotation must be normalized
|
|
assert (std::fabs (DotProduct (axis, axis) - 1.0) < 0.001);
|
|
|
|
// Compute the half angle and its sin
|
|
Real thetaOver2 = theta * 0.5;
|
|
Real sinThetaOver2 = std::sin (thetaOver2);
|
|
|
|
// Set the values
|
|
res._w = std::cos (thetaOver2);
|
|
res._x = axis._x * sinThetaOver2;
|
|
res._y = axis._y * sinThetaOver2;
|
|
res._z = axis._z * sinThetaOver2;
|
|
|
|
return res;
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Log
|
|
//
|
|
// Unit quaternion logarithm. log(q) = log(cos(theta) + n*sin(theta))
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline Quaternion<Real>
|
|
Log (const Quaternion<Real> &q)
|
|
{
|
|
Quaternion<Real> res;
|
|
res._w = 0.0;
|
|
|
|
if (std::fabs (q._w) < 1.0)
|
|
{
|
|
Real theta = std::acos (q._w);
|
|
Real sin_theta = std::sin (theta);
|
|
|
|
if (std::fabs (sin_theta) > 0.00001)
|
|
{
|
|
Real thetaOverSinTheta = theta / sin_theta;
|
|
res._x = q._x * thetaOverSinTheta;
|
|
res._y = q._y * thetaOverSinTheta;
|
|
res._z = q._z * thetaOverSinTheta;
|
|
return res;
|
|
}
|
|
}
|
|
|
|
res._x = q._x;
|
|
res._y = q._y;
|
|
res._z = q._z;
|
|
|
|
return res;
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Exp
|
|
//
|
|
// Quaternion exponential.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline Quaternion<Real>
|
|
Exp (const Quaternion<Real> &q)
|
|
{
|
|
Real theta = std::sqrt (DotProduct (q, q));
|
|
Real sin_theta = std::sin (theta);
|
|
|
|
Quaternion<Real> res;
|
|
res._w = std::cos (theta);
|
|
|
|
if (std::fabs (sin_theta) > 0.00001)
|
|
{
|
|
Real sinThetaOverTheta = sin_theta / theta;
|
|
res._x = q._x * sinThetaOverTheta;
|
|
res._y = q._y * sinThetaOverTheta;
|
|
res._z = q._z * sinThetaOverTheta;
|
|
}
|
|
else
|
|
{
|
|
res._x = q._x;
|
|
res._y = q._y;
|
|
res._z = q._z;
|
|
}
|
|
|
|
return res;
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Pow
|
|
//
|
|
// Quaternion exponentiation.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline Quaternion<Real>
|
|
Pow (const Quaternion<Real> &q, Real exponent)
|
|
{
|
|
// Check for the case of an identity quaternion.
|
|
// This will protect against divide by zero
|
|
if (std::fabs (q._w) > 0.9999)
|
|
return q;
|
|
|
|
// Extract the half angle alpha (alpha = theta/2)
|
|
Real alpha = std::acos (q._w);
|
|
|
|
// Compute new alpha value
|
|
Real newAlpha = alpha * exponent;
|
|
|
|
// Compute new quaternion
|
|
Vector3<Real> n (q._x, q._y, q._z);
|
|
n *= std::sin (newAlpha) / std::sin (alpha);
|
|
|
|
return Quaternion<Real> (std::cos (newAlpha), n);
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Slerp
|
|
//
|
|
// Spherical linear interpolation.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline Quaternion<Real>
|
|
Slerp (const Quaternion<Real> &q0, const Quaternion<Real> &q1, Real t)
|
|
{
|
|
// Check for out-of range parameter and return edge points if so
|
|
if (t <= 0.0) return q0;
|
|
if (t >= 1.0) return q1;
|
|
|
|
// Compute "cosine of angle between quaternions" using dot product
|
|
Real cosOmega = DotProduct (q0, q1);
|
|
|
|
// If negative dot, use -q1. Two quaternions q and -q
|
|
// represent the same rotation, but may produce
|
|
// different slerp. We chose q or -q to rotate using
|
|
// the acute angle.
|
|
Real q1w = q1._w;
|
|
Real q1x = q1._x;
|
|
Real q1y = q1._y;
|
|
Real q1z = q1._z;
|
|
|
|
if (cosOmega < 0.0)
|
|
{
|
|
q1w = -q1w;
|
|
q1x = -q1x;
|
|
q1y = -q1y;
|
|
q1z = -q1z;
|
|
cosOmega = -cosOmega;
|
|
}
|
|
|
|
// We should have two unit quaternions, so dot should be <= 1.0
|
|
assert (cosOmega < 1.1);
|
|
|
|
// Compute interpolation fraction, checking for quaternions
|
|
// almost exactly the same
|
|
Real k0, k1;
|
|
|
|
if (cosOmega > 0.9999)
|
|
{
|
|
// Very close - just use linear interpolation,
|
|
// which will protect againt a divide by zero
|
|
|
|
k0 = 1.0 - t;
|
|
k1 = t;
|
|
}
|
|
else
|
|
{
|
|
// Compute the sin of the angle using the
|
|
// trig identity sin^2(omega) + cos^2(omega) = 1
|
|
Real sinOmega = std::sqrt (1.0 - (cosOmega * cosOmega));
|
|
|
|
// Compute the angle from its sin and cosine
|
|
Real omega = std::atan2 (sinOmega, cosOmega);
|
|
|
|
// Compute inverse of denominator, so we only have
|
|
// to divide once
|
|
Real oneOverSinOmega = 1.0 / sinOmega;
|
|
|
|
// Compute interpolation parameters
|
|
k0 = std::sin ((1.0 - t) * omega) * oneOverSinOmega;
|
|
k1 = std::sin (t * omega) * oneOverSinOmega;
|
|
}
|
|
|
|
// Interpolate and return new quaternion
|
|
return Quaternion<Real> (
|
|
(k0 * q0._w) + (k1 * q1w),
|
|
(k0 * q0._x) + (k1 * q1x),
|
|
(k0 * q0._y) + (k1 * q1y),
|
|
(k0 * q0._z) + (k1 * q1z)
|
|
);
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Squad
|
|
//
|
|
// Spherical cubic interpolation.
|
|
// squad = slerp (slerp (q0, q1, t), slerp (qa, qb, t), 2t(1 - t)).
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline Quaternion<Real>
|
|
Squad (const Quaternion<Real> &q0, const Quaternion<Real> &qa,
|
|
const Quaternion<Real> &qb, const Quaternion<Real> &q1, Real t)
|
|
{
|
|
Real slerp_t = 2.0 * t * (1.0 - t);
|
|
|
|
Quaternion<Real> slerp_q0 = Slerp (q0, q1, t);
|
|
Quaternion<Real> slerp_q1 = Slerp (qa, qb, t);
|
|
|
|
return Slerp (slerp_q0, slerp_q1, slerp_t);
|
|
}
|
|
|
|
|
|
// --------------------------------------------------------------------------
|
|
// Intermediate
|
|
//
|
|
// Compute intermediate quaternions for building spline segments.
|
|
// --------------------------------------------------------------------------
|
|
|
|
template <typename Real>
|
|
inline void
|
|
Intermediate (const Quaternion<Real> &qprev, const Quaternion<Real> &qcurr,
|
|
const Quaternion<Real> &qnext, Quaternion<Real> &qa, Quaternion<Real> &qb)
|
|
{
|
|
// We should have unit quaternions
|
|
assert (DotProduct (qprev, qprev) <= 1.0001);
|
|
assert (DotProduct (qcurr, qcurr) <= 1.0001);
|
|
|
|
Quaternion<Real> inv_prev = Conjugate (qprev);
|
|
Quaternion<Real> inv_curr = Conjugate (qcurr);
|
|
|
|
Quaternion<Real> p0 = inv_prev * qcurr;
|
|
Quaternion<Real> p1 = inv_curr * qnext;
|
|
|
|
Quaternion<Real> arg = (Log (p0) - Log (p1)) * 0.25;
|
|
|
|
qa = qcurr * Exp ( arg);
|
|
qb = qcurr * Exp (-arg);
|
|
}
|