wraitii
f6348b9617
This cleans up many un-necessary header includes, either simply providing nothing or forward declarations in their place. No major compilation time change here, though this does reduce depencies in some headers. Also fix up old MacOS STL-include fixes that are no longer relevant. Differential Revision: https://code.wildfiregames.com/D3128 This was SVN commit r24227.
317 lines
7.2 KiB
C++
317 lines
7.2 KiB
C++
/* Copyright (C) 2009 Wildfire Games.
|
|
* This file is part of 0 A.D.
|
|
*
|
|
* 0 A.D. is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation, either version 2 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* 0 A.D. is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with 0 A.D. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include "precompiled.h"
|
|
|
|
#include "Quaternion.h"
|
|
|
|
#include "MathUtil.h"
|
|
#include "Matrix3D.h"
|
|
|
|
const float EPSILON=0.0001f;
|
|
|
|
|
|
CQuaternion::CQuaternion() :
|
|
m_W(1)
|
|
{
|
|
}
|
|
|
|
CQuaternion::CQuaternion(float x, float y, float z, float w) :
|
|
m_V(x, y, z), m_W(w)
|
|
{
|
|
}
|
|
|
|
CQuaternion CQuaternion::operator + (const CQuaternion &quat) const
|
|
{
|
|
CQuaternion Temp;
|
|
Temp.m_W = m_W + quat.m_W;
|
|
Temp.m_V = m_V + quat.m_V;
|
|
return Temp;
|
|
}
|
|
|
|
CQuaternion &CQuaternion::operator += (const CQuaternion &quat)
|
|
{
|
|
*this = *this + quat;
|
|
return *this;
|
|
}
|
|
|
|
CQuaternion CQuaternion::operator - (const CQuaternion &quat) const
|
|
{
|
|
CQuaternion Temp;
|
|
Temp.m_W = m_W - quat.m_W;
|
|
Temp.m_V = m_V - quat.m_V;
|
|
return Temp;
|
|
}
|
|
|
|
CQuaternion &CQuaternion::operator -= (const CQuaternion &quat)
|
|
{
|
|
*this = *this - quat;
|
|
return *this;
|
|
}
|
|
|
|
CQuaternion CQuaternion::operator * (const CQuaternion &quat) const
|
|
{
|
|
CQuaternion Temp;
|
|
Temp.m_W = (m_W * quat.m_W) - (m_V.Dot(quat.m_V));
|
|
Temp.m_V = (m_V.Cross(quat.m_V)) + (quat.m_V * m_W) + (m_V * quat.m_W);
|
|
return Temp;
|
|
}
|
|
|
|
CQuaternion &CQuaternion::operator *= (const CQuaternion &quat)
|
|
{
|
|
*this = *this * quat;
|
|
return *this;
|
|
}
|
|
|
|
CQuaternion CQuaternion::operator * (float factor) const
|
|
{
|
|
CQuaternion Temp;
|
|
Temp.m_W = m_W * factor;
|
|
Temp.m_V = m_V * factor;
|
|
return Temp;
|
|
}
|
|
|
|
|
|
float CQuaternion::Dot(const CQuaternion& quat) const
|
|
{
|
|
return
|
|
m_V.X * quat.m_V.X +
|
|
m_V.Y * quat.m_V.Y +
|
|
m_V.Z * quat.m_V.Z +
|
|
m_W * quat.m_W;
|
|
}
|
|
|
|
void CQuaternion::FromEulerAngles (float x, float y, float z)
|
|
{
|
|
float cr, cp, cy;
|
|
float sr, sp, sy;
|
|
|
|
CQuaternion QRoll, QPitch, QYaw;
|
|
|
|
cr = cosf(x * 0.5f);
|
|
cp = cosf(y * 0.5f);
|
|
cy = cosf(z * 0.5f);
|
|
|
|
sr = sinf(x * 0.5f);
|
|
sp = sinf(y * 0.5f);
|
|
sy = sinf(z * 0.5f);
|
|
|
|
QRoll.m_V = CVector3D(sr, 0, 0);
|
|
QRoll.m_W = cr;
|
|
|
|
QPitch.m_V = CVector3D(0, sp, 0);
|
|
QPitch.m_W = cp;
|
|
|
|
QYaw.m_V = CVector3D(0, 0, sy);
|
|
QYaw.m_W = cy;
|
|
|
|
(*this) = QYaw * QPitch * QRoll;
|
|
}
|
|
CVector3D CQuaternion::ToEulerAngles()
|
|
{
|
|
float heading, attitude, bank;
|
|
float sqw = m_W * m_W;
|
|
float sqx = m_V.X*m_V.X;
|
|
float sqy = m_V.Y*m_V.Y;
|
|
float sqz = m_V.Z*m_V.Z;
|
|
float unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
|
|
float test = m_V.X*m_V.Y + m_V.Z*m_W;
|
|
if (test > (.5f-EPSILON)*unit)
|
|
{ // singularity at north pole
|
|
heading = 2 * atan2( m_V.X, m_W);
|
|
attitude = (float)M_PI/2;
|
|
bank = 0;
|
|
}
|
|
else if (test < (-.5f+EPSILON)*unit)
|
|
{ // singularity at south pole
|
|
heading = -2 * atan2(m_V.X, m_W);
|
|
attitude = -(float)M_PI/2;
|
|
bank = 0;
|
|
}
|
|
else
|
|
{
|
|
heading = atan2(2.f * (m_V.X*m_V.Y + m_V.Z*m_W),(sqx - sqy - sqz + sqw));
|
|
bank = atan2(2.f * (m_V.Y*m_V.Z + m_V.X*m_W),(-sqx - sqy + sqz + sqw));
|
|
attitude = asin(-2.f * (m_V.X*m_V.Z - m_V.Y*m_W));
|
|
}
|
|
return CVector3D(bank, attitude, heading);
|
|
}
|
|
|
|
CMatrix3D CQuaternion::ToMatrix () const
|
|
{
|
|
CMatrix3D result;
|
|
ToMatrix(result);
|
|
return result;
|
|
}
|
|
|
|
void CQuaternion::ToMatrix(CMatrix3D& result) const
|
|
{
|
|
float wx, wy, wz, xx, xy, xz, yy, yz, zz;
|
|
|
|
// calculate coefficients
|
|
xx = m_V.X * m_V.X * 2.f;
|
|
xy = m_V.X * m_V.Y * 2.f;
|
|
xz = m_V.X * m_V.Z * 2.f;
|
|
|
|
yy = m_V.Y * m_V.Y * 2.f;
|
|
yz = m_V.Y * m_V.Z * 2.f;
|
|
|
|
zz = m_V.Z * m_V.Z * 2.f;
|
|
|
|
wx = m_W * m_V.X * 2.f;
|
|
wy = m_W * m_V.Y * 2.f;
|
|
wz = m_W * m_V.Z * 2.f;
|
|
|
|
result._11 = 1.0f - (yy + zz);
|
|
result._12 = xy - wz;
|
|
result._13 = xz + wy;
|
|
result._14 = 0;
|
|
|
|
result._21 = xy + wz;
|
|
result._22 = 1.0f - (xx + zz);
|
|
result._23 = yz - wx;
|
|
result._24 = 0;
|
|
|
|
result._31 = xz - wy;
|
|
result._32 = yz + wx;
|
|
result._33 = 1.0f - (xx + yy);
|
|
result._34 = 0;
|
|
|
|
result._41 = 0;
|
|
result._42 = 0;
|
|
result._43 = 0;
|
|
result._44 = 1;
|
|
}
|
|
|
|
void CQuaternion::Slerp(const CQuaternion& from, const CQuaternion& to, float ratio)
|
|
{
|
|
float to1[4];
|
|
float omega, cosom, sinom, scale0, scale1;
|
|
|
|
// calc cosine
|
|
cosom = from.Dot(to);
|
|
|
|
|
|
// adjust signs (if necessary)
|
|
if (cosom < 0.0)
|
|
{
|
|
cosom = -cosom;
|
|
to1[0] = -to.m_V.X;
|
|
to1[1] = -to.m_V.Y;
|
|
to1[2] = -to.m_V.Z;
|
|
to1[3] = -to.m_W;
|
|
}
|
|
else
|
|
{
|
|
to1[0] = to.m_V.X;
|
|
to1[1] = to.m_V.Y;
|
|
to1[2] = to.m_V.Z;
|
|
to1[3] = to.m_W;
|
|
}
|
|
|
|
// calculate coefficients
|
|
if ((1.0f - cosom) > EPSILON)
|
|
{
|
|
// standard case (slerp)
|
|
omega = acosf(cosom);
|
|
sinom = sinf(omega);
|
|
scale0 = sinf((1.0f - ratio) * omega) / sinom;
|
|
scale1 = sinf(ratio * omega) / sinom;
|
|
}
|
|
else
|
|
{
|
|
// "from" and "to" quaternions are very close
|
|
// ... so we can do a linear interpolation
|
|
scale0 = 1.0f - ratio;
|
|
scale1 = ratio;
|
|
}
|
|
|
|
// calculate final values
|
|
m_V.X = scale0 * from.m_V.X + scale1 * to1[0];
|
|
m_V.Y = scale0 * from.m_V.Y + scale1 * to1[1];
|
|
m_V.Z = scale0 * from.m_V.Z + scale1 * to1[2];
|
|
m_W = scale0 * from.m_W + scale1 * to1[3];
|
|
}
|
|
|
|
void CQuaternion::Nlerp(const CQuaternion& from, const CQuaternion& to, float ratio)
|
|
{
|
|
float c = from.Dot(to);
|
|
if (c < 0.f)
|
|
*this = from - (to + from) * ratio;
|
|
else
|
|
*this = from + (to - from) * ratio;
|
|
Normalize();
|
|
}
|
|
|
|
///////////////////////////////////////////////////////////////////////////////////////////////
|
|
// FromAxisAngle: create a quaternion from axis/angle representation of a rotation
|
|
void CQuaternion::FromAxisAngle(const CVector3D& axis, float angle)
|
|
{
|
|
float sinHalfTheta=(float) sin(angle/2);
|
|
float cosHalfTheta=(float) cos(angle/2);
|
|
|
|
m_V.X=axis.X*sinHalfTheta;
|
|
m_V.Y=axis.Y*sinHalfTheta;
|
|
m_V.Z=axis.Z*sinHalfTheta;
|
|
m_W=cosHalfTheta;
|
|
}
|
|
|
|
///////////////////////////////////////////////////////////////////////////////////////////////
|
|
// ToAxisAngle: convert the quaternion to axis/angle representation of a rotation
|
|
void CQuaternion::ToAxisAngle(CVector3D& axis, float& angle)
|
|
{
|
|
CQuaternion q = *this;
|
|
q.Normalize();
|
|
angle = acosf(q.m_W) * 2.f;
|
|
float sin_a = sqrtf(1.f - q.m_W * q.m_W);
|
|
if (fabsf(sin_a) < 0.0005f) sin_a = 1.f;
|
|
axis.X = q.m_V.X / sin_a;
|
|
axis.Y = q.m_V.Y / sin_a;
|
|
axis.Z = q.m_V.Z / sin_a;
|
|
}
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////////////////////
|
|
// Normalize: normalize this quaternion
|
|
void CQuaternion::Normalize()
|
|
{
|
|
float lensqrd=SQR(m_V.X)+SQR(m_V.Y)+SQR(m_V.Z)+SQR(m_W);
|
|
if (lensqrd>0) {
|
|
float invlen=1.0f/sqrtf(lensqrd);
|
|
m_V*=invlen;
|
|
m_W*=invlen;
|
|
}
|
|
}
|
|
|
|
///////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
|
CVector3D CQuaternion::Rotate(const CVector3D& vec) const
|
|
{
|
|
// v' = q * v * q^-1
|
|
// (where v is the quat. with w=0, xyz=vec)
|
|
|
|
return (*this * CQuaternion(vec.X, vec.Y, vec.Z, 0.f) * GetInverse()).m_V;
|
|
}
|
|
|
|
CQuaternion CQuaternion::GetInverse() const
|
|
{
|
|
// (x,y,z,w)^-1 = (-x/l^2, -y/l^2, -z/l^2, w/l^2) where l^2=x^2+y^2+z^2+w^2
|
|
// Since we're only using quaternions for rotation, they should always have unit
|
|
// length, so assume l=1
|
|
return CQuaternion(-m_V.X, -m_V.Y, -m_V.Z, m_W);
|
|
}
|