LCOV - code coverage report
Current view: top level - source/maths - Quaternion.cpp (source / functions) Hit Total Coverage
Test: 0 A.D. test coverage report Lines: 59 167 35.3 %
Date: 2023-01-19 00:18:29 Functions: 8 23 34.8 %

          Line data    Source code
       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          27 : CQuaternion::CQuaternion() :
      29          27 :     m_W(1)
      30             : {
      31          27 : }
      32             : 
      33           4 : CQuaternion::CQuaternion(float x, float y, float z, float w) :
      34           4 :     m_V(x, y, z), m_W(w)
      35             : {
      36           4 : }
      37             : 
      38           0 : CQuaternion CQuaternion::operator + (const CQuaternion &quat) const
      39             : {
      40           0 :     CQuaternion Temp;
      41           0 :     Temp.m_W = m_W + quat.m_W;
      42           0 :     Temp.m_V = m_V + quat.m_V;
      43           0 :     return Temp;
      44             : }
      45             : 
      46           0 : CQuaternion &CQuaternion::operator += (const CQuaternion &quat)
      47             : {
      48           0 :     *this = *this + quat;
      49           0 :     return *this;
      50             : }
      51             : 
      52           0 : CQuaternion CQuaternion::operator - (const CQuaternion &quat) const
      53             : {
      54           0 :     CQuaternion Temp;
      55           0 :     Temp.m_W = m_W - quat.m_W;
      56           0 :     Temp.m_V = m_V - quat.m_V;
      57           0 :     return Temp;
      58             : }
      59             : 
      60           0 : CQuaternion &CQuaternion::operator -= (const CQuaternion &quat)
      61             : {
      62           0 :     *this = *this - quat;
      63           0 :     return *this;
      64             : }
      65             : 
      66           8 : CQuaternion CQuaternion::operator * (const CQuaternion &quat) const
      67             : {
      68           8 :     CQuaternion Temp;
      69           8 :     Temp.m_W = (m_W * quat.m_W) - (m_V.Dot(quat.m_V));
      70           8 :     Temp.m_V = (m_V.Cross(quat.m_V)) + (quat.m_V * m_W) + (m_V * quat.m_W);
      71           8 :     return Temp;
      72             : }
      73             : 
      74           0 : CQuaternion &CQuaternion::operator *= (const CQuaternion &quat)
      75             : {
      76           0 :     *this = *this * quat;
      77           0 :     return *this;
      78             : }
      79             : 
      80           0 : CQuaternion CQuaternion::operator * (float factor) const
      81             : {
      82           0 :     CQuaternion Temp;
      83           0 :     Temp.m_W = m_W * factor;
      84           0 :     Temp.m_V = m_V * factor;
      85           0 :     return Temp;
      86             : }
      87             : 
      88             : 
      89           0 : float CQuaternion::Dot(const CQuaternion& quat) const
      90             : {
      91             :     return
      92           0 :         m_V.X * quat.m_V.X +
      93           0 :         m_V.Y * quat.m_V.Y +
      94           0 :         m_V.Z * quat.m_V.Z +
      95           0 :         m_W   * quat.m_W;
      96             : }
      97             : 
      98           4 : void CQuaternion::FromEulerAngles (float x, float y, float z)
      99             : {
     100             :     float cr, cp, cy;
     101             :     float sr, sp, sy;
     102             : 
     103           4 :     CQuaternion QRoll, QPitch, QYaw;
     104             : 
     105           4 :     cr = cosf(x * 0.5f);
     106           4 :     cp = cosf(y * 0.5f);
     107           4 :     cy = cosf(z * 0.5f);
     108             : 
     109           4 :     sr = sinf(x * 0.5f);
     110           4 :     sp = sinf(y * 0.5f);
     111           4 :     sy = sinf(z * 0.5f);
     112             : 
     113           4 :     QRoll.m_V = CVector3D(sr, 0, 0);
     114           4 :     QRoll.m_W = cr;
     115             : 
     116           4 :     QPitch.m_V = CVector3D(0, sp, 0);
     117           4 :     QPitch.m_W = cp;
     118             : 
     119           4 :     QYaw.m_V = CVector3D(0, 0, sy);
     120           4 :     QYaw.m_W = cy;
     121             : 
     122           4 :     (*this) = QYaw * QPitch * QRoll;
     123           4 : }
     124           0 : CVector3D CQuaternion::ToEulerAngles()
     125             : {
     126             :     float heading, attitude, bank;
     127           0 :     float sqw = m_W * m_W;
     128           0 :     float sqx = m_V.X*m_V.X;
     129           0 :     float sqy = m_V.Y*m_V.Y;
     130           0 :     float sqz = m_V.Z*m_V.Z;
     131           0 :     float unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
     132           0 :     float test = m_V.X*m_V.Y + m_V.Z*m_W;
     133           0 :     if (test > (.5f-EPSILON)*unit)
     134             :     { // singularity at north pole
     135           0 :         heading = 2 * atan2( m_V.X, m_W);
     136           0 :         attitude = (float)M_PI/2;
     137           0 :         bank = 0;
     138             :     }
     139           0 :     else if (test < (-.5f+EPSILON)*unit)
     140             :     { // singularity at south pole
     141           0 :         heading = -2 * atan2(m_V.X, m_W);
     142           0 :         attitude = -(float)M_PI/2;
     143           0 :         bank = 0;
     144             :     }
     145             :     else
     146             :     {
     147           0 :         heading = atan2(2.f * (m_V.X*m_V.Y + m_V.Z*m_W),(sqx - sqy - sqz + sqw));
     148           0 :         bank = atan2(2.f * (m_V.Y*m_V.Z + m_V.X*m_W),(-sqx - sqy + sqz + sqw));
     149           0 :         attitude = asin(-2.f * (m_V.X*m_V.Z - m_V.Y*m_W));
     150             :     }
     151           0 :     return CVector3D(bank, attitude, heading);
     152             : }
     153             : 
     154           3 : CMatrix3D CQuaternion::ToMatrix () const
     155             : {
     156           3 :     CMatrix3D result;
     157           3 :     ToMatrix(result);
     158           3 :     return result;
     159             : }
     160             : 
     161           7 : void CQuaternion::ToMatrix(CMatrix3D& result) const
     162             : {
     163             :     float wx, wy, wz, xx, xy, xz, yy, yz, zz;
     164             : 
     165             :     // calculate coefficients
     166           7 :     xx = m_V.X * m_V.X * 2.f;
     167           7 :     xy = m_V.X * m_V.Y * 2.f;
     168           7 :     xz = m_V.X * m_V.Z * 2.f;
     169             : 
     170           7 :     yy = m_V.Y * m_V.Y * 2.f;
     171           7 :     yz = m_V.Y * m_V.Z * 2.f;
     172             : 
     173           7 :     zz = m_V.Z * m_V.Z * 2.f;
     174             : 
     175           7 :     wx = m_W * m_V.X * 2.f;
     176           7 :     wy = m_W * m_V.Y * 2.f;
     177           7 :     wz = m_W * m_V.Z * 2.f;
     178             : 
     179           7 :     result._11 = 1.0f - (yy + zz);
     180           7 :     result._12 = xy - wz;
     181           7 :     result._13 = xz + wy;
     182           7 :     result._14 = 0;
     183             : 
     184           7 :     result._21 = xy + wz;
     185           7 :     result._22 = 1.0f - (xx + zz);
     186           7 :     result._23 = yz - wx;
     187           7 :     result._24 = 0;
     188             : 
     189           7 :     result._31 = xz - wy;
     190           7 :     result._32 = yz + wx;
     191           7 :     result._33 = 1.0f - (xx + yy);
     192           7 :     result._34 = 0;
     193             : 
     194           7 :     result._41 = 0;
     195           7 :     result._42 = 0;
     196           7 :     result._43 = 0;
     197           7 :     result._44 = 1;
     198           7 : }
     199             : 
     200           0 : 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           0 :     cosom = from.Dot(to);
     207             : 
     208             : 
     209             :     // adjust signs (if necessary)
     210           0 :     if (cosom < 0.0)
     211             :     {
     212           0 :         cosom = -cosom;
     213           0 :         to1[0] = -to.m_V.X;
     214           0 :         to1[1] = -to.m_V.Y;
     215           0 :         to1[2] = -to.m_V.Z;
     216           0 :         to1[3] = -to.m_W;
     217             :     }
     218             :     else
     219             :     {
     220           0 :         to1[0] = to.m_V.X;
     221           0 :         to1[1] = to.m_V.Y;
     222           0 :         to1[2] = to.m_V.Z;
     223           0 :         to1[3] = to.m_W;
     224             :     }
     225             : 
     226             :     // calculate coefficients
     227           0 :     if ((1.0f - cosom) > EPSILON)
     228             :     {
     229             :         // standard case (slerp)
     230           0 :         omega = acosf(cosom);
     231           0 :         sinom = sinf(omega);
     232           0 :         scale0 = sinf((1.0f - ratio) * omega) / sinom;
     233           0 :         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           0 :         scale0 = 1.0f - ratio;
     240           0 :         scale1 = ratio;
     241             :     }
     242             : 
     243             :     // calculate final values
     244           0 :     m_V.X = scale0 * from.m_V.X + scale1 * to1[0];
     245           0 :     m_V.Y = scale0 * from.m_V.Y + scale1 * to1[1];
     246           0 :     m_V.Z = scale0 * from.m_V.Z + scale1 * to1[2];
     247           0 :     m_W   = scale0 * from.m_W + scale1 * to1[3];
     248           0 : }
     249             : 
     250           0 : void CQuaternion::Nlerp(const CQuaternion& from, const CQuaternion& to, float ratio)
     251             : {
     252           0 :     float c = from.Dot(to);
     253           0 :     if (c < 0.f)
     254           0 :         *this = from - (to + from) * ratio;
     255             :     else
     256           0 :         *this = from + (to - from) * ratio;
     257           0 :     Normalize();
     258           0 : }
     259             : 
     260             : ///////////////////////////////////////////////////////////////////////////////////////////////
     261             : // FromAxisAngle: create a quaternion from axis/angle representation of a rotation
     262           0 : void CQuaternion::FromAxisAngle(const CVector3D& axis, float angle)
     263             : {
     264           0 :     float sinHalfTheta=(float) sin(angle/2);
     265           0 :     float cosHalfTheta=(float) cos(angle/2);
     266             : 
     267           0 :     m_V.X=axis.X*sinHalfTheta;
     268           0 :     m_V.Y=axis.Y*sinHalfTheta;
     269           0 :     m_V.Z=axis.Z*sinHalfTheta;
     270           0 :     m_W=cosHalfTheta;
     271           0 : }
     272             : 
     273             : ///////////////////////////////////////////////////////////////////////////////////////////////
     274             : // ToAxisAngle: convert the quaternion to axis/angle representation of a rotation
     275           0 : void CQuaternion::ToAxisAngle(CVector3D& axis, float& angle)
     276             : {
     277           0 :     CQuaternion q = *this;
     278           0 :     q.Normalize();
     279           0 :     angle = acosf(q.m_W) * 2.f;
     280           0 :     float sin_a = sqrtf(1.f - q.m_W * q.m_W);
     281           0 :     if (fabsf(sin_a) < 0.0005f) sin_a = 1.f;
     282           0 :     axis.X = q.m_V.X / sin_a;
     283           0 :     axis.Y = q.m_V.Y / sin_a;
     284           0 :     axis.Z = q.m_V.Z / sin_a;
     285           0 : }
     286             : 
     287             : 
     288             : ///////////////////////////////////////////////////////////////////////////////////////////////
     289             : // Normalize: normalize this quaternion
     290           0 : void CQuaternion::Normalize()
     291             : {
     292           0 :     float lensqrd=SQR(m_V.X)+SQR(m_V.Y)+SQR(m_V.Z)+SQR(m_W);
     293           0 :     if (lensqrd>0) {
     294           0 :         float invlen=1.0f/sqrtf(lensqrd);
     295           0 :         m_V*=invlen;
     296           0 :         m_W*=invlen;
     297             :     }
     298           0 : }
     299             : 
     300             : ///////////////////////////////////////////////////////////////////////////////////////////////
     301             : 
     302           0 : 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           0 :     return (*this * CQuaternion(vec.X, vec.Y, vec.Z, 0.f) * GetInverse()).m_V;
     308             : }
     309             : 
     310           0 : 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           0 :     return CQuaternion(-m_V.X, -m_V.Y, -m_V.Z, m_W);
     316           3 : }

Generated by: LCOV version 1.13