409 lines
8.9 KiB
C++
409 lines
8.9 KiB
C++
/*
|
|
* Copyright 2008 Haiku Inc. All rights reserved.
|
|
* Distributed under the terms of the MIT License.
|
|
*
|
|
* Authors:
|
|
* Alexandre Deckner
|
|
*
|
|
*/
|
|
|
|
/*
|
|
*
|
|
* This is a refactored and stripped down version of bullet-2.66 src\LinearMath\btQuaternion.h
|
|
* The dependancies on base class btQuadWord have been removed for simplification.
|
|
* Added gl matrix conversion method.
|
|
*
|
|
*/
|
|
|
|
/*
|
|
Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
|
|
|
|
This software is provided 'as-is', without any express or implied warranty.
|
|
In no event will the authors be held liable for any damages arising from the use of this software.
|
|
Permission is granted to anyone to use this software for any purpose,
|
|
including commercial applications, and to alter it and redistribute it freely,
|
|
subject to the following restrictions:
|
|
|
|
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
|
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
|
3. This notice may not be removed or altered from any source distribution.
|
|
*/
|
|
|
|
|
|
#ifndef __QUATERNION_H__
|
|
#define __QUATERNION_H__
|
|
|
|
#include "Vector3.h"
|
|
|
|
class Quaternion {
|
|
protected:
|
|
float m_x;
|
|
float m_y;
|
|
float m_z;
|
|
float m_w;
|
|
public:
|
|
Quaternion() {}
|
|
|
|
Quaternion(const Quaternion& q)
|
|
{
|
|
*((Quaternion*)this) = q;
|
|
}
|
|
|
|
Quaternion(const float& x, const float& y, const float& z,const float& w)
|
|
{
|
|
m_x = x, m_y = y, m_z = z, m_w = w;
|
|
}
|
|
|
|
Quaternion(const Vector3& axis, const float& angle)
|
|
{
|
|
setRotation(axis, angle);
|
|
}
|
|
|
|
Quaternion(const float& yaw, const float& pitch, const float& roll)
|
|
{
|
|
setEuler(yaw, pitch, roll);
|
|
}
|
|
|
|
inline const float& x() const { return m_x; }
|
|
|
|
|
|
inline const float& y() const { return m_y; }
|
|
|
|
|
|
inline const float& z() const { return m_z; }
|
|
|
|
|
|
inline const float& w() const { return m_w; }
|
|
|
|
|
|
void setValue(const float& x, const float& y, const float& z)
|
|
{
|
|
m_x=x;
|
|
m_y=y;
|
|
m_z=z;
|
|
m_w = 0.f;
|
|
}
|
|
|
|
|
|
void setValue(const float& x, const float& y, const float& z,const float& w)
|
|
{
|
|
m_x=x;
|
|
m_y=y;
|
|
m_z=z;
|
|
m_w=w;
|
|
}
|
|
|
|
|
|
void setRotation(const Vector3& axis, const float& angle)
|
|
{
|
|
float d = axis.length();
|
|
assert(d != 0.0f);
|
|
float s = sin(angle * 0.5f) / d;
|
|
setValue(axis.x() * s, axis.y() * s, axis.z() * s,
|
|
cos(angle * 0.5f));
|
|
}
|
|
|
|
|
|
void setEuler(const float& yaw, const float& pitch, const float& roll)
|
|
{
|
|
float halfYaw = yaw * 0.5f;
|
|
float halfPitch = pitch * 0.5f;
|
|
float halfRoll = roll * 0.5f;
|
|
float cosYaw = cos(halfYaw);
|
|
float sinYaw = sin(halfYaw);
|
|
float cosPitch = cos(halfPitch);
|
|
float sinPitch = sin(halfPitch);
|
|
float cosRoll = cos(halfRoll);
|
|
float sinRoll = sin(halfRoll);
|
|
setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
|
|
cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
|
|
sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
|
|
cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
|
|
}
|
|
|
|
|
|
Quaternion& operator+=(const Quaternion& q)
|
|
{
|
|
m_x += q.x(); m_y += q.y(); m_z += q.z(); m_w += q.m_w;
|
|
return *this;
|
|
}
|
|
|
|
|
|
Quaternion& operator-=(const Quaternion& q)
|
|
{
|
|
m_x -= q.x(); m_y -= q.y(); m_z -= q.z(); m_w -= q.m_w;
|
|
return *this;
|
|
}
|
|
|
|
|
|
Quaternion& operator*=(const float& s)
|
|
{
|
|
m_x *= s; m_y *= s; m_z *= s; m_w *= s;
|
|
return *this;
|
|
}
|
|
|
|
|
|
Quaternion& operator*=(const Quaternion& q)
|
|
{
|
|
setValue(m_w * q.x() + m_x * q.m_w + m_y * q.z() - m_z * q.y(),
|
|
m_w * q.y() + m_y * q.m_w + m_z * q.x() - m_x * q.z(),
|
|
m_w * q.z() + m_z * q.m_w + m_x * q.y() - m_y * q.x(),
|
|
m_w * q.m_w - m_x * q.x() - m_y * q.y() - m_z * q.z());
|
|
return *this;
|
|
}
|
|
|
|
|
|
float dot(const Quaternion& q) const
|
|
{
|
|
return m_x * q.x() + m_y * q.y() + m_z * q.z() + m_w * q.m_w;
|
|
}
|
|
|
|
|
|
float length2() const
|
|
{
|
|
return dot(*this);
|
|
}
|
|
|
|
|
|
float length() const
|
|
{
|
|
return sqrt(length2());
|
|
}
|
|
|
|
|
|
Quaternion& normalize()
|
|
{
|
|
return *this /= length();
|
|
}
|
|
|
|
|
|
inline Quaternion
|
|
operator*(const float& s) const
|
|
{
|
|
return Quaternion(x() * s, y() * s, z() * s, m_w * s);
|
|
}
|
|
|
|
|
|
Quaternion operator/(const float& s) const
|
|
{
|
|
assert(s != 0.0f);
|
|
return *this * (1.0f / s);
|
|
}
|
|
|
|
|
|
Quaternion& operator/=(const float& s)
|
|
{
|
|
assert(s != 0.0f);
|
|
return *this *= 1.0f / s;
|
|
}
|
|
|
|
|
|
Quaternion normalized() const
|
|
{
|
|
return *this / length();
|
|
}
|
|
|
|
|
|
float angle(const Quaternion& q) const
|
|
{
|
|
float s = sqrt(length2() * q.length2());
|
|
assert(s != 0.0f);
|
|
return acos(dot(q) / s);
|
|
}
|
|
|
|
|
|
float getAngle() const
|
|
{
|
|
float s = 2.0f * acos(m_w);
|
|
return s;
|
|
}
|
|
|
|
|
|
Quaternion inverse() const
|
|
{
|
|
return Quaternion(m_x, m_y, m_z, -m_w);
|
|
}
|
|
|
|
|
|
inline Quaternion
|
|
operator+(const Quaternion& q2) const
|
|
{
|
|
const Quaternion& q1 = *this;
|
|
return Quaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_w + q2.m_w);
|
|
}
|
|
|
|
|
|
inline Quaternion
|
|
operator-(const Quaternion& q2) const
|
|
{
|
|
const Quaternion& q1 = *this;
|
|
return Quaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_w - q2.m_w);
|
|
}
|
|
|
|
|
|
inline Quaternion operator-() const
|
|
{
|
|
const Quaternion& q2 = *this;
|
|
return Quaternion( - q2.x(), - q2.y(), - q2.z(), - q2.m_w);
|
|
}
|
|
|
|
|
|
inline Quaternion farthest( const Quaternion& qd) const
|
|
{
|
|
Quaternion diff,sum;
|
|
diff = *this - qd;
|
|
sum = *this + qd;
|
|
if( diff.dot(diff) > sum.dot(sum) )
|
|
return qd;
|
|
return (-qd);
|
|
}
|
|
|
|
|
|
Quaternion slerp(const Quaternion& q, const float& t) const
|
|
{
|
|
float theta = angle(q);
|
|
if (theta != 0.0f)
|
|
{
|
|
float d = 1.0f / sin(theta);
|
|
float s0 = sin((1.0f - t) * theta);
|
|
float s1 = sin(t * theta);
|
|
return Quaternion((m_x * s0 + q.x() * s1) * d,
|
|
(m_y * s0 + q.y() * s1) * d,
|
|
(m_z * s0 + q.z() * s1) * d,
|
|
(m_w * s0 + q.m_w * s1) * d);
|
|
}
|
|
else
|
|
{
|
|
return *this;
|
|
}
|
|
}
|
|
|
|
|
|
void toOpenGLMatrix(float m[4][4]){
|
|
|
|
float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
|
|
|
|
// calculate coefficients
|
|
x2 = m_x + m_x; y2 = m_y + m_y;
|
|
z2 = m_z + m_z;
|
|
xx = m_x * x2; xy = m_x * y2; xz = m_x * z2;
|
|
yy = m_y * y2; yz = m_y * z2; zz = m_z * z2;
|
|
wx = m_w * x2; wy = m_w * y2; wz = m_w * z2;
|
|
|
|
|
|
m[0][0] = 1.0 - (yy + zz); m[1][0] = xy - wz;
|
|
m[2][0] = xz + wy; m[3][0] = 0.0;
|
|
|
|
m[0][1] = xy + wz; m[1][1] = 1.0 - (xx + zz);
|
|
m[2][1] = yz - wx; m[3][1] = 0.0;
|
|
|
|
|
|
m[0][2] = xz - wy; m[1][2] = yz + wx;
|
|
m[2][2] = 1.0 - (xx + yy); m[3][2] = 0.0;
|
|
|
|
|
|
m[0][3] = 0; m[1][3] = 0;
|
|
m[2][3] = 0; m[3][3] = 1;
|
|
}
|
|
};
|
|
|
|
|
|
inline Quaternion
|
|
operator-(const Quaternion& q)
|
|
{
|
|
return Quaternion(-q.x(), -q.y(), -q.z(), -q.w());
|
|
}
|
|
|
|
|
|
inline Quaternion
|
|
operator*(const Quaternion& q1, const Quaternion& q2) {
|
|
return Quaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(),
|
|
q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(),
|
|
q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(),
|
|
q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z());
|
|
}
|
|
|
|
|
|
inline Quaternion
|
|
operator*(const Quaternion& q, const Vector3& w)
|
|
{
|
|
return Quaternion( q.w() * w.x() + q.y() * w.z() - q.z() * w.y(),
|
|
q.w() * w.y() + q.z() * w.x() - q.x() * w.z(),
|
|
q.w() * w.z() + q.x() * w.y() - q.y() * w.x(),
|
|
-q.x() * w.x() - q.y() * w.y() - q.z() * w.z());
|
|
}
|
|
|
|
|
|
inline Quaternion
|
|
operator*(const Vector3& w, const Quaternion& q)
|
|
{
|
|
return Quaternion( w.x() * q.w() + w.y() * q.z() - w.z() * q.y(),
|
|
w.y() * q.w() + w.z() * q.x() - w.x() * q.z(),
|
|
w.z() * q.w() + w.x() * q.y() - w.y() * q.x(),
|
|
-w.x() * q.x() - w.y() * q.y() - w.z() * q.z());
|
|
}
|
|
|
|
|
|
inline float
|
|
dot(const Quaternion& q1, const Quaternion& q2)
|
|
{
|
|
return q1.dot(q2);
|
|
}
|
|
|
|
|
|
inline float
|
|
length(const Quaternion& q)
|
|
{
|
|
return q.length();
|
|
}
|
|
|
|
|
|
inline float
|
|
angle(const Quaternion& q1, const Quaternion& q2)
|
|
{
|
|
return q1.angle(q2);
|
|
}
|
|
|
|
|
|
inline Quaternion
|
|
inverse(const Quaternion& q)
|
|
{
|
|
return q.inverse();
|
|
}
|
|
|
|
|
|
inline Quaternion
|
|
slerp(const Quaternion& q1, const Quaternion& q2, const float& t)
|
|
{
|
|
return q1.slerp(q2, t);
|
|
}
|
|
|
|
|
|
inline Quaternion
|
|
shortestArcQuat(const Vector3& v0, const Vector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized
|
|
{
|
|
Vector3 c = v0.cross(v1);
|
|
float d = v0.dot(v1);
|
|
|
|
if (d < -1.0 + FLT_EPSILON)
|
|
return Quaternion(0.0f,1.0f,0.0f,0.0f); // just pick any vector
|
|
|
|
float s = sqrt((1.0f + d) * 2.0f);
|
|
float rs = 1.0f / s;
|
|
|
|
return Quaternion(c.x()*rs, c.y()*rs, c.z()*rs, s * 0.5f);
|
|
}
|
|
|
|
|
|
inline Quaternion
|
|
shortestArcQuatNormalize2(Vector3& v0,Vector3& v1)
|
|
{
|
|
v0.normalize();
|
|
v1.normalize();
|
|
return shortestArcQuat(v0,v1);
|
|
}
|
|
#endif
|
|
|
|
|
|
|