402 lines
12 KiB
C++
402 lines
12 KiB
C++
/*
|
|
Copyright (c) 2009 Christopher A. Taylor. All rights reserved.
|
|
|
|
Redistribution and use in source and binary forms, with or without
|
|
modification, are permitted provided that the following conditions are met:
|
|
|
|
* Redistributions of source code must retain the above copyright notice,
|
|
this list of conditions and the following disclaimer.
|
|
* Redistributions in binary form must reproduce the above copyright notice,
|
|
this list of conditions and the following disclaimer in the documentation
|
|
and/or other materials provided with the distribution.
|
|
* Neither the name of LibCat nor the names of its contributors may be used
|
|
to endorse or promote products derived from this software without
|
|
specific prior written permission.
|
|
|
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
|
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
|
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
|
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
|
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
|
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
|
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
|
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
|
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
POSSIBILITY OF SUCH DAMAGE.
|
|
*/
|
|
|
|
/*
|
|
Based on code from "Physics for Game Developers", David M. Bourg
|
|
|
|
slerp() code adapted from this website:
|
|
http://www.number-none.com/product/Understanding%20Slerp,%20Then%20Not%20Using%20It/index.html
|
|
*/
|
|
|
|
#ifndef CAT_QUATERNION_HPP
|
|
#define CAT_QUATERNION_HPP
|
|
|
|
#include <cat/gfx/Vector.hpp>
|
|
#include <cat/gfx/Matrix.hpp>
|
|
|
|
namespace cat {
|
|
|
|
|
|
// Generic quaternion class for linear algebra
|
|
template <typename Scalar, typename Double>
|
|
class Quaternion
|
|
{
|
|
protected:
|
|
// Backed by a 4D vector with elements arranged as <x,y,z,w>
|
|
Vector<4, Scalar, Double> _v;
|
|
|
|
public:
|
|
// Short-hand for the current quaternion type
|
|
typedef Quaternion<Scalar, Double> mytype;
|
|
|
|
// Short-hand for the vector type
|
|
typedef Vector<3, Scalar, Double> vectype;
|
|
|
|
// Uninitialized quaternion is not cleared
|
|
Quaternion() { }
|
|
|
|
// Copy constructor
|
|
Quaternion(const mytype &u)
|
|
: _v(u._v)
|
|
{
|
|
}
|
|
|
|
// Initialization constructor
|
|
Quaternion(const vectype &v, Scalar w)
|
|
: _v(v)
|
|
{
|
|
}
|
|
|
|
// Initialization constructor
|
|
Quaternion(Scalar x, Scalar y, Scalar z, Scalar w)
|
|
: _v(x, y, z, w)
|
|
{
|
|
}
|
|
|
|
// Assignment operator
|
|
mytype &operator=(const mytype &u)
|
|
{
|
|
_v.copy(u._v);
|
|
|
|
return *this;
|
|
}
|
|
|
|
// Convert from Euler angle representation
|
|
// Pre-condition: angles in radians (see Deg2Rad in Scalar.hpp)
|
|
void setFromEulerAngles(f32 xroll, f32 ypitch, f32 zyaw)
|
|
{
|
|
f64 croll = cos(0.5f * xroll);
|
|
f64 cpitch = cos(0.5f * ypitch);
|
|
f64 cyaw = cos(0.5f * zyaw);
|
|
f64 sroll = sin(0.5f * xroll);
|
|
f64 spitch = sin(0.5f * ypitch);
|
|
f64 syaw = sin(0.5f * zyaw);
|
|
|
|
f64 cyawcpitch = cyaw * cpitch;
|
|
f64 syawspitch = syaw * spitch;
|
|
f64 cyawspitch = cyaw * spitch;
|
|
f64 syawcpitch = syaw * cpitch;
|
|
|
|
_v(0) = static_cast<Scalar>( cyawcpitch * sroll - syawspitch * croll );
|
|
_v(1) = static_cast<Scalar>( cyawspitch * croll + syawcpitch * sroll );
|
|
_v(2) = static_cast<Scalar>( syawcpitch * croll - cyawspitch * sroll );
|
|
_v(3) = static_cast<Scalar>( cyawcpitch * croll + syawspitch * sroll );
|
|
|
|
_v.normalize();
|
|
}
|
|
|
|
// Convert from axis-angle representation
|
|
// Pre-condition: axis must be unit-length
|
|
void setFromAxisAngle(const vectype &axis, f32 angle)
|
|
{
|
|
angle *= 0.5f;
|
|
|
|
Scalar theta = static_cast<Scalar>( sin(angle) );
|
|
|
|
_v(0) = theta * axis(0);
|
|
_v(1) = theta * axis(1);
|
|
_v(2) = theta * axis(2);
|
|
_v(3) = static_cast<Scalar>( cos(angle) );
|
|
}
|
|
|
|
// Conjugate
|
|
mytype operator~() const
|
|
{
|
|
return mytype(-_v(0), -_v(1), -_v(2), _v(3));
|
|
}
|
|
|
|
// Conjugate in-place
|
|
mytype &conjugate() const
|
|
{
|
|
_v(0) = -_v(0);
|
|
_v(1) = -_v(1);
|
|
_v(2) = -_v(2);
|
|
|
|
return *this;
|
|
}
|
|
|
|
// Multiply by quaternion
|
|
mytype operator*(const mytype &u)
|
|
{
|
|
// Cache each of the elements since each is used 4 times
|
|
Double x1 = _v(0), x2 = u._v(0);
|
|
Double y1 = _v(1), y2 = u._v(1);
|
|
Double z1 = _v(2), z2 = u._v(2);
|
|
Double w1 = _v(3), w2 = u._v(3);
|
|
|
|
// Quaternion multiplication formula:
|
|
Scalar x3 = static_cast<Scalar>( w1*x2 + x1*w2 + y1*z2 - z1*y2 );
|
|
Scalar y3 = static_cast<Scalar>( w1*y2 - x1*z2 + y1*w2 + z1*x2 );
|
|
Scalar z3 = static_cast<Scalar>( w1*z2 + x1*y2 - y1*x2 + z1*w2 );
|
|
Scalar w3 = static_cast<Scalar>( w1*w2 - x1*x2 - y1*y2 - z1*z2 );
|
|
|
|
return mytype(x3, y3, z3, w3);
|
|
}
|
|
|
|
// Multiply by quaternion in-place: this = this * u
|
|
mytype &operator*=(const mytype &u)
|
|
{
|
|
// Cache each of the elements since each is used 4 times
|
|
Double x1 = _v(0), x2 = u._v(0);
|
|
Double y1 = _v(1), y2 = u._v(1);
|
|
Double z1 = _v(2), z2 = u._v(2);
|
|
Double w1 = _v(3), w2 = u._v(3);
|
|
|
|
// Quaternion multiplication formula:
|
|
_v(0) = static_cast<Scalar>( w1*x2 + x1*w2 + y1*z2 - z1*y2 );
|
|
_v(1) = static_cast<Scalar>( w1*y2 - x1*z2 + y1*w2 + z1*x2 );
|
|
_v(2) = static_cast<Scalar>( w1*z2 + x1*y2 - y1*x2 + z1*w2 );
|
|
_v(3) = static_cast<Scalar>( w1*w2 - x1*x2 - y1*y2 - z1*z2 );
|
|
|
|
return *this;
|
|
}
|
|
|
|
// Multiply by quaternion in-place: this = u * this
|
|
mytype &postMultiply(const mytype &u)
|
|
{
|
|
// Cache each of the elements since each is used 4 times
|
|
Double x2 = _v(0), x1 = u._v(0);
|
|
Double y2 = _v(1), y1 = u._v(1);
|
|
Double z2 = _v(2), z1 = u._v(2);
|
|
Double w2 = _v(3), w1 = u._v(3);
|
|
|
|
// Quaternion multiplication formula:
|
|
_v(0) = static_cast<Scalar>( w1*x2 + x1*w2 + y1*z2 - z1*y2 );
|
|
_v(1) = static_cast<Scalar>( w1*y2 - x1*z2 + y1*w2 + z1*x2 );
|
|
_v(2) = static_cast<Scalar>( w1*z2 + x1*y2 - y1*x2 + z1*w2 );
|
|
_v(3) = static_cast<Scalar>( w1*w2 - x1*x2 - y1*y2 - z1*z2 );
|
|
|
|
return *this;
|
|
}
|
|
|
|
// Rotate given vector in-place
|
|
void rotate(vectype &u)
|
|
{
|
|
// Implements the simple formula: (q1 * u2 * ~q1).getVector()
|
|
|
|
// Cache each of the elements since each is used 4 times
|
|
Double x1 = _v(0), x2 = u(0);
|
|
Double y1 = _v(1), y2 = u(1);
|
|
Double z1 = _v(2), z2 = u(2);
|
|
Double w1 = _v(3);
|
|
|
|
// Quaternion-vector multiplication formula (q3 = q1 * u2):
|
|
Double x3 = w1*x2 + y1*z2 - z1*y2;
|
|
Double y3 = w1*y2 - x1*z2 + z1*x2;
|
|
Double z3 = w1*z2 + x1*y2 - y1*x2;
|
|
Double w3 = -(x1*x2 + y1*y2 + z1*z2);
|
|
|
|
// Quaternion multiplication formula (q2' = q3 * ~q1):
|
|
u(0) = static_cast<Scalar>( w1*x3 - x1*w3 + y1*z3 - z1*y3 );
|
|
u(1) = static_cast<Scalar>( w1*y3 - x1*z3 - y1*w3 + z1*x3 );
|
|
u(2) = static_cast<Scalar>( w1*z3 + x1*y3 - y1*x3 - z1*w3 );
|
|
}
|
|
|
|
// NLERP: Very fast, non-constant velocity and torque-minimal
|
|
// Precondition: q1, q2 are unit length
|
|
friend void nlerp(const mytype &q1, const mytype &q2, Scalar t, mytype &result)
|
|
{
|
|
// Linearly interpolate and normalize result
|
|
// This formula is a little more work than "q1 + (q2 - q1) * t"
|
|
// but less likely to lose precision when it matters
|
|
|
|
// Cosine of phi, the angle between the two vectors
|
|
Double cos_phi = q1._v.dotProduct(q2._v);
|
|
|
|
// I have read this may try to rotate around the "long way" sometimes and to
|
|
// fix that we check if cos(phi) is negative and invert one of the inputs.
|
|
if (cos_phi < 0.0)
|
|
{
|
|
result._v = -q1._v;
|
|
}
|
|
else
|
|
{
|
|
result._v = q1._v;
|
|
}
|
|
|
|
// Simple linear interpolation
|
|
Scalar scale0 = static_cast<Scalar>(1) - t;
|
|
Scalar scale1 = t;
|
|
|
|
result._v *= scale0;
|
|
result._v += q2._v * scale1;
|
|
result._v.normalize();
|
|
}
|
|
|
|
// SLERP: Slower, constant velocity and torque-minimal
|
|
// Precondition: q1, q2 are unit length
|
|
friend void slerp(const mytype &q1, const mytype &q2, Scalar t, mytype &result)
|
|
{
|
|
// Cosine of phi, the angle between the two vectors
|
|
Double cos_phi = q1._v.dotProduct(q2._v);
|
|
|
|
// I have read this may try to rotate around the "long way" sometimes and to
|
|
// fix that we check if cos(phi) is negative and invert one of the inputs.
|
|
if (cos_phi < 0.0)
|
|
{
|
|
cos_phi = -cos_phi;
|
|
result._v = -q1._v;
|
|
}
|
|
else
|
|
{
|
|
result._v = q1._v;
|
|
}
|
|
|
|
// Default to simple linear interpolation
|
|
Scalar scale0 = static_cast<Scalar>(1) - t;
|
|
Scalar scale1 = t;
|
|
|
|
// If the distance is not small we need to do full slerp:
|
|
if (cos_phi < 0.9995)
|
|
{
|
|
// cos_phi is guaranteed to be within the domain of acos(), 0..1
|
|
Double phi = static_cast<Double>( acos(cos_phi) );
|
|
Double inv_sin_phi = static_cast<Double>(1) / sin(phi);
|
|
|
|
scale0 = static_cast<Scalar>( sin(scale0 * phi) * inv_sin_phi );
|
|
scale1 = static_cast<Scalar>( sin(scale1 * phi) * inv_sin_phi );
|
|
}
|
|
|
|
result._v *= scale0;
|
|
result._v += q2._v * scale1;
|
|
result._v.normalize();
|
|
}
|
|
|
|
// Get angle of rotation
|
|
Scalar getAngle()
|
|
{
|
|
return static_cast<Scalar>( 2 ) * static_cast<Scalar>( acos(_v(3)) );
|
|
}
|
|
|
|
// Get axis of rotation
|
|
vectype getAxis()
|
|
{
|
|
return vectype(_v(0), _v(1), _v(2)).normalize();
|
|
}
|
|
|
|
// Get matrix form of the rotation represented by this quaternion
|
|
void getMatrix4x4(Matrix<4, 4, Scalar> &result)
|
|
{
|
|
Double dx = _v(0);
|
|
Double dy = _v(1);
|
|
Double dz = _v(2);
|
|
Double dw = _v(3);
|
|
|
|
Double x2 = dx * dx;
|
|
Double y2 = dy * dy;
|
|
Double z2 = dz * dz;
|
|
Double xy = dx * dy;
|
|
Double yz = dy * dz;
|
|
Double zx = dz * dx;
|
|
Double xw = dx * dw;
|
|
Double yw = dy * dw;
|
|
Double zw = dz * dw;
|
|
|
|
const Double ONE = static_cast<Double>( 1 );
|
|
const Double TWO = static_cast<Double>( 2 );
|
|
|
|
// Result is written in OpenGL column-major matrix order:
|
|
result( 0) = static_cast<Scalar>( ONE - TWO * (y2 + z2) );
|
|
result( 1) = static_cast<Scalar>( TWO * (xy - zw) );
|
|
result( 2) = static_cast<Scalar>( TWO * (zx + yw) );
|
|
result( 3) = static_cast<Scalar>( 0 );
|
|
|
|
result( 4) = static_cast<Scalar>( TWO * (xy + zw) );
|
|
result( 5) = static_cast<Scalar>( ONE - TWO * (x2 + z2) );
|
|
result( 6) = static_cast<Scalar>( TWO * (yz - xw) );
|
|
result( 7) = static_cast<Scalar>( 0 );
|
|
|
|
result( 8) = static_cast<Scalar>( TWO * (zx - yw) );
|
|
result( 9) = static_cast<Scalar>( TWO * (yz + xw) );
|
|
result(10) = static_cast<Scalar>( ONE - TWO * (x2 + y2) );
|
|
result(11) = static_cast<Scalar>( 0 );
|
|
|
|
result(12) = static_cast<Scalar>( 0 );
|
|
result(13) = static_cast<Scalar>( 0 );
|
|
result(14) = static_cast<Scalar>( 0 );
|
|
result(15) = static_cast<Scalar>( ONE );
|
|
}
|
|
|
|
// Get Euler angles
|
|
vectype Quaternion::getEulerAngles()
|
|
{
|
|
Double dx = _v(0);
|
|
Double dy = _v(1);
|
|
Double dz = _v(2);
|
|
Double dw = _v(3);
|
|
|
|
Double x2 = dx * dx;
|
|
Double y2 = dy * dy;
|
|
Double z2 = dz * dz;
|
|
Double w2 = dw * dw;
|
|
Double xy = dx * dy;
|
|
Double yz = dy * dz;
|
|
Double xw = dx * dw;
|
|
Double yw = dy * dw;
|
|
Double zw = dz * dw;
|
|
|
|
const Double TWO = static_cast<Double>( 2 );
|
|
|
|
Double r11 = w2 + x2 - y2 - z2;
|
|
Double r21 = TWO * (xy + zw);
|
|
Double r31 = TWO * (xy - yw);
|
|
Double r32 = TWO * (yz + xw);
|
|
Double r33 = w2 - x2 - y2 + z2;
|
|
|
|
Double tmp = fabs(r31);
|
|
const Double LIMIT = static_cast<Double>( 0.999999 );
|
|
|
|
if (tmp > LIMIT)
|
|
{
|
|
Double xz = dx * dz;
|
|
|
|
Double r12 = TWO * (yz - zw);
|
|
Double r13 = TWO * (xz + yw);
|
|
|
|
return vectype(static_cast<Scalar>( 0 ),
|
|
static_cast<Scalar>( -CAT_HALF_PI_64 * r31 / tmp ),
|
|
static_cast<Scalar>( atan2(-r12, -r31 * r13) ));
|
|
}
|
|
|
|
return vectype(static_cast<Scalar>( atan2(r32, r33) ),
|
|
static_cast<Scalar>( asin(-r31) ),
|
|
static_cast<Scalar>( atan2(r21, r11) ));
|
|
}
|
|
};
|
|
|
|
|
|
// Short-hand for common usages:
|
|
|
|
typedef Quaternion<f32, f64> Quaternion4f;
|
|
typedef Quaternion<f64, f64> Quaternion4d;
|
|
|
|
|
|
} // namespace cat
|
|
|
|
#endif // CAT_QUATERNION_HPP
|