1 /* Copyright (C) 2009 Wildfire Games.
2 * This file is part of 0 A.D.
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.
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.
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/>.
18 #include "precompiled.h"
20 #include "Quaternion.h"
25 const float EPSILON
=0.0001f
;
28 CQuaternion::CQuaternion() :
33 CQuaternion::CQuaternion(float x
, float y
, float z
, float w
) :
38 CQuaternion
CQuaternion::operator + (const CQuaternion
&quat
) const
41 Temp
.m_W
= m_W
+ quat
.m_W
;
42 Temp
.m_V
= m_V
+ quat
.m_V
;
46 CQuaternion
&CQuaternion::operator += (const CQuaternion
&quat
)
52 CQuaternion
CQuaternion::operator - (const CQuaternion
&quat
) const
55 Temp
.m_W
= m_W
- quat
.m_W
;
56 Temp
.m_V
= m_V
- quat
.m_V
;
60 CQuaternion
&CQuaternion::operator -= (const CQuaternion
&quat
)
66 CQuaternion
CQuaternion::operator * (const CQuaternion
&quat
) const
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
);
74 CQuaternion
&CQuaternion::operator *= (const CQuaternion
&quat
)
80 CQuaternion
CQuaternion::operator * (float factor
) const
83 Temp
.m_W
= m_W
* factor
;
84 Temp
.m_V
= m_V
* factor
;
89 float CQuaternion::Dot(const CQuaternion
& quat
) const
98 void CQuaternion::FromEulerAngles (float x
, float y
, float z
)
103 CQuaternion QRoll
, QPitch
, QYaw
;
113 QRoll
.m_V
= CVector3D(sr
, 0, 0);
116 QPitch
.m_V
= CVector3D(0, sp
, 0);
119 QYaw
.m_V
= CVector3D(0, 0, sy
);
122 (*this) = QYaw
* QPitch
* QRoll
;
124 CVector3D
CQuaternion::ToEulerAngles()
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;
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;
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
));
151 return CVector3D(bank
, attitude
, heading
);
154 CMatrix3D
CQuaternion::ToMatrix () const
161 void CQuaternion::ToMatrix(CMatrix3D
& result
) const
163 float wx
, wy
, wz
, xx
, xy
, xz
, yy
, yz
, zz
;
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
;
170 yy
= m_V
.Y
* m_V
.Y
* 2.f
;
171 yz
= m_V
.Y
* m_V
.Z
* 2.f
;
173 zz
= m_V
.Z
* m_V
.Z
* 2.f
;
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
;
179 result
._11
= 1.0f
- (yy
+ zz
);
180 result
._12
= xy
- wz
;
181 result
._13
= xz
+ wy
;
184 result
._21
= xy
+ wz
;
185 result
._22
= 1.0f
- (xx
+ zz
);
186 result
._23
= yz
- wx
;
189 result
._31
= xz
- wy
;
190 result
._32
= yz
+ wx
;
191 result
._33
= 1.0f
- (xx
+ yy
);
200 void CQuaternion::Slerp(const CQuaternion
& from
, const CQuaternion
& to
, float ratio
)
203 float omega
, cosom
, sinom
, scale0
, scale1
;
206 cosom
= from
.Dot(to
);
209 // adjust signs (if necessary)
226 // calculate coefficients
227 if ((1.0f
- cosom
) > EPSILON
)
229 // standard case (slerp)
230 omega
= acosf(cosom
);
232 scale0
= sinf((1.0f
- ratio
) * omega
) / sinom
;
233 scale1
= sinf(ratio
* omega
) / sinom
;
237 // "from" and "to" quaternions are very close
238 // ... so we can do a linear interpolation
239 scale0
= 1.0f
- ratio
;
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];
250 void CQuaternion::Nlerp(const CQuaternion
& from
, const CQuaternion
& to
, float ratio
)
252 float c
= from
.Dot(to
);
254 *this = from
- (to
+ from
) * ratio
;
256 *this = from
+ (to
- from
) * ratio
;
260 ///////////////////////////////////////////////////////////////////////////////////////////////
261 // FromAxisAngle: create a quaternion from axis/angle representation of a rotation
262 void CQuaternion::FromAxisAngle(const CVector3D
& axis
, float angle
)
264 float sinHalfTheta
=(float) sin(angle
/2);
265 float cosHalfTheta
=(float) cos(angle
/2);
267 m_V
.X
=axis
.X
*sinHalfTheta
;
268 m_V
.Y
=axis
.Y
*sinHalfTheta
;
269 m_V
.Z
=axis
.Z
*sinHalfTheta
;
273 ///////////////////////////////////////////////////////////////////////////////////////////////
274 // ToAxisAngle: convert the quaternion to axis/angle representation of a rotation
275 void CQuaternion::ToAxisAngle(CVector3D
& axis
, float& angle
)
277 CQuaternion q
= *this;
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
;
288 ///////////////////////////////////////////////////////////////////////////////////////////////
289 // Normalize: normalize this quaternion
290 void CQuaternion::Normalize()
292 float lensqrd
=SQR(m_V
.X
)+SQR(m_V
.Y
)+SQR(m_V
.Z
)+SQR(m_W
);
294 float invlen
=1.0f
/sqrtf(lensqrd
);
300 ///////////////////////////////////////////////////////////////////////////////////////////////
302 CVector3D
CQuaternion::Rotate(const CVector3D
& vec
) const
305 // (where v is the quat. with w=0, xyz=vec)
307 return (*this * CQuaternion(vec
.X
, vec
.Y
, vec
.Z
, 0.f
) * GetInverse()).m_V
;
310 CQuaternion
CQuaternion::GetInverse() const
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
);