source/maths/Quaternion.cpp

   1 /* Copyright (C) 2009 Wildfire Games.
   2  * This file is part of 0 A.D.
   3  *
   4  * 0 A.D. is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 2 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * 0 A.D. is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with 0 A.D.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include "precompiled.h"
  19
  20 #include "Quaternion.h"
  21
  22 #include "MathUtil.h"
  23 #include "Matrix3D.h"
  24
  25 const float EPSILON=0.0001f;
  26
  27
  28 CQuaternion::CQuaternion() :
  29         m_W(1)
  30 {
  31 }
  32
  33 CQuaternion::CQuaternion(float x, float y, float z, float w) :
  34         m_V(x, y, z), m_W(w)
  35 {
  36 }
  37
  38 CQuaternion CQuaternion::operator + (const CQuaternion &quat) const
  39 {
  40         CQuaternion Temp;
  41         Temp.m_W = m_W + quat.m_W;
  42         Temp.m_V = m_V + quat.m_V;
  43         return Temp;
  44 }
  45
  46 CQuaternion &CQuaternion::operator += (const CQuaternion &quat)
  47 {
  48         *this = *this + quat;
  49         return *this;
  50 }
  51
  52 CQuaternion CQuaternion::operator - (const CQuaternion &quat) const
  53 {
  54         CQuaternion Temp;
  55         Temp.m_W = m_W - quat.m_W;
  56         Temp.m_V = m_V - quat.m_V;
  57         return Temp;
  58 }
  59
  60 CQuaternion &CQuaternion::operator -= (const CQuaternion &quat)
  61 {
  62         *this = *this - quat;
  63         return *this;
  64 }
  65
  66 CQuaternion CQuaternion::operator * (const CQuaternion &quat) const
  67 {
  68         CQuaternion Temp;
  69         Temp.m_W = (m_W * quat.m_W) - (m_V.Dot(quat.m_V));
  70         Temp.m_V = (m_V.Cross(quat.m_V)) + (quat.m_V * m_W) + (m_V * quat.m_W);
  71         return Temp;
  72 }
  73
  74 CQuaternion &CQuaternion::operator *= (const CQuaternion &quat)
  75 {
  76         *this = *this * quat;
  77         return *this;
  78 }
  79
  80 CQuaternion CQuaternion::operator * (float factor) const
  81 {
  82         CQuaternion Temp;
  83         Temp.m_W = m_W * factor;
  84         Temp.m_V = m_V * factor;
  85         return Temp;
  86 }
  87
  88
  89 float CQuaternion::Dot(const CQuaternion& quat) const
  90 {
  91         return
  92                 m_V.X * quat.m_V.X +
  93                 m_V.Y * quat.m_V.Y +
  94                 m_V.Z * quat.m_V.Z +
  95                 m_W   * quat.m_W;
  96 }
  97
  98 void CQuaternion::FromEulerAngles (float x, float y, float z)
  99 {
 100         float cr, cp, cy;
 101         float sr, sp, sy;
 102
 103         CQuaternion QRoll, QPitch, QYaw;
 104
 105         cr = cosf(x * 0.5f);
 106         cp = cosf(y * 0.5f);
 107         cy = cosf(z * 0.5f);
 108
 109         sr = sinf(x * 0.5f);
 110         sp = sinf(y * 0.5f);
 111         sy = sinf(z * 0.5f);
 112
 113         QRoll.m_V = CVector3D(sr, 0, 0);
 114         QRoll.m_W = cr;
 115
 116         QPitch.m_V = CVector3D(0, sp, 0);
 117         QPitch.m_W = cp;
 118
 119         QYaw.m_V = CVector3D(0, 0, sy);
 120         QYaw.m_W = cy;
 121
 122         (*this) = QYaw * QPitch * QRoll;
 123 }
 124 CVector3D CQuaternion::ToEulerAngles()
 125 {
 126     float heading, attitude, bank;
 127         float sqw = m_W * m_W;
 128     float sqx = m_V.X*m_V.X;
 129     float sqy = m_V.Y*m_V.Y;
 130     float sqz = m_V.Z*m_V.Z;
 131         float unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
 132         float test = m_V.X*m_V.Y + m_V.Z*m_W;
 133         if (test > (.5f-EPSILON)*unit)
 134         { // singularity at north pole
 135                 heading = 2 * atan2( m_V.X, m_W);
 136                 attitude = (float)M_PI/2;
 137                 bank = 0;
 138         }
 139         else if (test < (-.5f+EPSILON)*unit)
 140         { // singularity at south pole
 141                 heading = -2 * atan2(m_V.X, m_W);
 142                 attitude = -(float)M_PI/2;
 143                 bank = 0;
 144         }
 145         else
 146         {
 147                 heading = atan2(2.f * (m_V.X*m_V.Y + m_V.Z*m_W),(sqx - sqy - sqz + sqw));
 148             bank = atan2(2.f * (m_V.Y*m_V.Z + m_V.X*m_W),(-sqx - sqy + sqz + sqw));
 149                 attitude = asin(-2.f * (m_V.X*m_V.Z - m_V.Y*m_W));
 150         }
 151         return CVector3D(bank, attitude, heading);
 152 }
 153
 154 CMatrix3D CQuaternion::ToMatrix () const
 155 {
 156         CMatrix3D result;
 157         ToMatrix(result);
 158         return result;
 159 }
 160
 161 void CQuaternion::ToMatrix(CMatrix3D& result) const
 162 {
 163         float wx, wy, wz, xx, xy, xz, yy, yz, zz;
 164
 165         // calculate coefficients
 166         xx = m_V.X * m_V.X * 2.f;
 167         xy = m_V.X * m_V.Y * 2.f;
 168         xz = m_V.X * m_V.Z * 2.f;
 169
 170         yy = m_V.Y * m_V.Y * 2.f;
 171         yz = m_V.Y * m_V.Z * 2.f;
 172
 173         zz = m_V.Z * m_V.Z * 2.f;
 174
 175         wx = m_W * m_V.X * 2.f;
 176         wy = m_W * m_V.Y * 2.f;
 177         wz = m_W * m_V.Z * 2.f;
 178
 179         result._11 = 1.0f - (yy + zz);
 180         result._12 = xy - wz;
 181         result._13 = xz + wy;
 182         result._14 = 0;
 183
 184         result._21 = xy + wz;
 185         result._22 = 1.0f - (xx + zz);
 186         result._23 = yz - wx;
 187         result._24 = 0;
 188
 189         result._31 = xz - wy;
 190         result._32 = yz + wx;
 191         result._33 = 1.0f - (xx + yy);
 192         result._34 = 0;
 193
 194         result._41 = 0;
 195         result._42 = 0;
 196         result._43 = 0;
 197         result._44 = 1;
 198 }
 199
 200 void CQuaternion::Slerp(const CQuaternion& from, const CQuaternion& to, float ratio)
 201 {
 202         float to1[4];
 203         float omega, cosom, sinom, scale0, scale1;
 204
 205         // calc cosine
 206         cosom = from.Dot(to);
 207
 208
 209         // adjust signs (if necessary)
 210         if (cosom < 0.0)
 211         {
 212                 cosom = -cosom;
 213                 to1[0] = -to.m_V.X;
 214                 to1[1] = -to.m_V.Y;
 215                 to1[2] = -to.m_V.Z;
 216                 to1[3] = -to.m_W;
 217         }
 218         else
 219         {
 220                 to1[0] = to.m_V.X;
 221                 to1[1] = to.m_V.Y;
 222                 to1[2] = to.m_V.Z;
 223                 to1[3] = to.m_W;
 224         }
 225
 226         // calculate coefficients
 227         if ((1.0f - cosom) > EPSILON)
 228         {
 229                 // standard case (slerp)
 230                 omega = acosf(cosom);
 231                 sinom = sinf(omega);
 232                 scale0 = sinf((1.0f - ratio) * omega) / sinom;
 233                 scale1 = sinf(ratio * omega) / sinom;
 234         }
 235         else
 236         {
 237                 // "from" and "to" quaternions are very close
 238                 //  ... so we can do a linear interpolation
 239                 scale0 = 1.0f - ratio;
 240                 scale1 = ratio;
 241         }
 242
 243         // calculate final values
 244         m_V.X = scale0 * from.m_V.X + scale1 * to1[0];
 245         m_V.Y = scale0 * from.m_V.Y + scale1 * to1[1];
 246         m_V.Z = scale0 * from.m_V.Z + scale1 * to1[2];
 247         m_W       = scale0 * from.m_W + scale1 * to1[3];
 248 }
 249
 250 void CQuaternion::Nlerp(const CQuaternion& from, const CQuaternion& to, float ratio)
 251 {
 252         float c = from.Dot(to);
 253         if (c < 0.f)
 254                 *this = from - (to + from) * ratio;
 255         else
 256                 *this = from + (to - from) * ratio;
 257         Normalize();
 258 }
 259
 260 ///////////////////////////////////////////////////////////////////////////////////////////////
 261 // FromAxisAngle: create a quaternion from axis/angle representation of a rotation
 262 void CQuaternion::FromAxisAngle(const CVector3D& axis, float angle)
 263 {
 264         float sinHalfTheta=(float) sin(angle/2);
 265         float cosHalfTheta=(float) cos(angle/2);
 266
 267         m_V.X=axis.X*sinHalfTheta;
 268         m_V.Y=axis.Y*sinHalfTheta;
 269         m_V.Z=axis.Z*sinHalfTheta;
 270         m_W=cosHalfTheta;
 271 }
 272
 273 ///////////////////////////////////////////////////////////////////////////////////////////////
 274 // ToAxisAngle: convert the quaternion to axis/angle representation of a rotation
 275 void CQuaternion::ToAxisAngle(CVector3D& axis, float& angle)
 276 {
 277         CQuaternion q = *this;
 278         q.Normalize();
 279         angle = acosf(q.m_W) * 2.f;
 280         float sin_a = sqrtf(1.f - q.m_W * q.m_W);
 281         if (fabsf(sin_a) < 0.0005f) sin_a = 1.f;
 282         axis.X = q.m_V.X / sin_a;
 283         axis.Y = q.m_V.Y / sin_a;
 284         axis.Z = q.m_V.Z / sin_a;
 285 }
 286
 287
 288 ///////////////////////////////////////////////////////////////////////////////////////////////
 289 // Normalize: normalize this quaternion
 290 void CQuaternion::Normalize()
 291 {
 292         float lensqrd=SQR(m_V.X)+SQR(m_V.Y)+SQR(m_V.Z)+SQR(m_W);
 293         if (lensqrd>0) {
 294                 float invlen=1.0f/sqrtf(lensqrd);
 295                 m_V*=invlen;
 296                 m_W*=invlen;
 297         }
 298 }
 299
 300 ///////////////////////////////////////////////////////////////////////////////////////////////
 301
 302 CVector3D CQuaternion::Rotate(const CVector3D& vec) const
 303 {
 304         // v' = q * v * q^-1
 305         // (where v is the quat. with w=0, xyz=vec)
 306
 307         return (*this * CQuaternion(vec.X, vec.Y, vec.Z, 0.f) * GetInverse()).m_V;
 308 }
 309
 310 CQuaternion CQuaternion::GetInverse() const
 311 {
 312         // (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
 313         // Since we're only using quaternions for rotation, they should always have unit
 314         // length, so assume l=1
 315         return CQuaternion(-m_V.X, -m_V.Y, -m_V.Z, m_W);
 316 }