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 : }
|
