forked from 0ad/0ad
bb
157c6af18e
Avoid cases of filenames Update years in terms and other legal(ish) documents Don't update years in license headers, since change is not meaningful Will add linter rule in seperate commit Happy recompiling everyone! Original Patch By: Nescio Comment By: Gallaecio Differential Revision: D2620 This was SVN commit r27786.
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);
|
|
}
|