1//************************************ bs::framework - Copyright 2018 Marko Pintera **************************************//
2//*********** Licensed under the MIT license. See LICENSE.md for full terms. This notice is not to be removed. ***********//
3#include "Math/BsQuaternion.h"
4
5#include "Math/BsMath.h"
6#include "Math/BsMatrix3.h"
7#include "Math/BsVector3.h"
8
9namespace bs
10{
11 const Quaternion Quaternion::ZERO{BS_ZERO()};
12 const Quaternion Quaternion::IDENTITY{BS_IDENTITY()};
13
14 void Quaternion::fromRotationMatrix(const Matrix3& mat)
15 {
16 // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
17 // article "Quaternion Calculus and Fast Animation".
18
19 float trace = mat[0][0]+mat[1][1]+mat[2][2];
20 float root;
21
22 if (trace > 0.0f)
23 {
24 // |w| > 1/2, may as well choose w > 1/2
25 root = Math::sqrt(trace + 1.0f); // 2w
26 w = 0.5f*root;
27 root = 0.5f/root; // 1/(4w)
28 x = (mat[2][1]-mat[1][2])*root;
29 y = (mat[0][2]-mat[2][0])*root;
30 z = (mat[1][0]-mat[0][1])*root;
31 }
32 else
33 {
34 // |w| <= 1/2
35 static UINT32 nextLookup[3] = { 1, 2, 0 };
36 UINT32 i = 0;
37
38 if (mat[1][1] > mat[0][0])
39 i = 1;
40
41 if (mat[2][2] > mat[i][i])
42 i = 2;
43
44 UINT32 j = nextLookup[i];
45 UINT32 k = nextLookup[j];
46
47 root = Math::sqrt(mat[i][i]-mat[j][j]-mat[k][k] + 1.0f);
48
49 float* cmpntLookup[3] = { &x, &y, &z };
50 *cmpntLookup[i] = 0.5f*root;
51 root = 0.5f/root;
52
53 w = (mat[k][j]-mat[j][k])*root;
54 *cmpntLookup[j] = (mat[j][i]+mat[i][j])*root;
55 *cmpntLookup[k] = (mat[k][i]+mat[i][k])*root;
56 }
57
58 normalize();
59 }
60
61 void Quaternion::fromAxisAngle(const Vector3& axis, const Radian& angle)
62 {
63 Radian halfAngle (0.5f*angle);
64 float sin = Math::sin(halfAngle);
65
66 w = Math::cos(halfAngle);
67 x = sin*axis.x;
68 y = sin*axis.y;
69 z = sin*axis.z;
70 }
71
72 void Quaternion::fromAxes(const Vector3& xaxis, const Vector3& yaxis, const Vector3& zaxis)
73 {
74 Matrix3 kRot;
75
76 kRot[0][0] = xaxis.x;
77 kRot[1][0] = xaxis.y;
78 kRot[2][0] = xaxis.z;
79
80 kRot[0][1] = yaxis.x;
81 kRot[1][1] = yaxis.y;
82 kRot[2][1] = yaxis.z;
83
84 kRot[0][2] = zaxis.x;
85 kRot[1][2] = zaxis.y;
86 kRot[2][2] = zaxis.z;
87
88 fromRotationMatrix(kRot);
89 }
90
91 void Quaternion::fromEulerAngles(const Radian& xAngle, const Radian& yAngle, const Radian& zAngle)
92 {
93 Radian halfXAngle = xAngle * 0.5f;
94 Radian halfYAngle = yAngle * 0.5f;
95 Radian halfZAngle = zAngle * 0.5f;
96
97 float cx = Math::cos(halfXAngle);
98 float sx = Math::sin(halfXAngle);
99
100 float cy = Math::cos(halfYAngle);
101 float sy = Math::sin(halfYAngle);
102
103 float cz = Math::cos(halfZAngle);
104 float sz = Math::sin(halfZAngle);
105
106 Quaternion quatX(cx, sx, 0.0f, 0.0f);
107 Quaternion quatY(cy, 0.0f, sy, 0.0f);
108 Quaternion quatZ(cz, 0.0f, 0.0f, sz);
109
110 *this = quatZ * (quatX * quatY);
111 }
112
113 void Quaternion::fromEulerAngles(const Radian& xAngle, const Radian& yAngle, const Radian& zAngle, EulerAngleOrder order)
114 {
115 static constexpr const EulerAngleOrderData EA_LOOKUP[6] = { { 0, 1, 2}, { 0, 2, 1}, { 1, 0, 2},
116 { 1, 2, 0}, { 2, 0, 1}, { 2, 1, 0} };
117 const EulerAngleOrderData& l = EA_LOOKUP[(int)order];
118
119 Radian halfXAngle = xAngle * 0.5f;
120 Radian halfYAngle = yAngle * 0.5f;
121 Radian halfZAngle = zAngle * 0.5f;
122
123 float cx = Math::cos(halfXAngle);
124 float sx = Math::sin(halfXAngle);
125
126 float cy = Math::cos(halfYAngle);
127 float sy = Math::sin(halfYAngle);
128
129 float cz = Math::cos(halfZAngle);
130 float sz = Math::sin(halfZAngle);
131
132 Quaternion quats[3];
133 quats[0] = Quaternion(cx, sx, 0.0f, 0.0f);
134 quats[1] = Quaternion(cy, 0.0f, sy, 0.0f);
135 quats[2] = Quaternion(cz, 0.0f, 0.0f, sz);
136
137 *this = quats[l.c] * (quats[l.b] * quats[l.a]);
138 }
139
140 void Quaternion::toRotationMatrix(Matrix3& mat) const
141 {
142 float tx = x+x;
143 float ty = y+y;
144 float tz = z+z;
145 float twx = tx*w;
146 float twy = ty*w;
147 float twz = tz*w;
148 float txx = tx*x;
149 float txy = ty*x;
150 float txz = tz*x;
151 float tyy = ty*y;
152 float tyz = tz*y;
153 float tzz = tz*z;
154
155 mat[0][0] = 1.0f-(tyy+tzz);
156 mat[0][1] = txy-twz;
157 mat[0][2] = txz+twy;
158 mat[1][0] = txy+twz;
159 mat[1][1] = 1.0f-(txx+tzz);
160 mat[1][2] = tyz-twx;
161 mat[2][0] = txz-twy;
162 mat[2][1] = tyz+twx;
163 mat[2][2] = 1.0f-(txx+tyy);
164 }
165
166 void Quaternion::toAxisAngle(Vector3& axis, Radian& angle) const
167 {
168 float sqrLength = x*x+y*y+z*z;
169 if ( sqrLength > 0.0 )
170 {
171 angle = 2.0*Math::acos(w);
172 float invLength = Math::invSqrt(sqrLength);
173 axis.x = x*invLength;
174 axis.y = y*invLength;
175 axis.z = z*invLength;
176 }
177 else
178 {
179 // Angle is 0 (mod 2*pi), so any axis will do
180 angle = Radian(0.0);
181 axis.x = 1.0;
182 axis.y = 0.0;
183 axis.z = 0.0;
184 }
185 }
186
187 void Quaternion::toAxes(Vector3& xaxis, Vector3& yaxis, Vector3& zaxis) const
188 {
189 Matrix3 matRot;
190 toRotationMatrix(matRot);
191
192 xaxis.x = matRot[0][0];
193 xaxis.y = matRot[1][0];
194 xaxis.z = matRot[2][0];
195
196 yaxis.x = matRot[0][1];
197 yaxis.y = matRot[1][1];
198 yaxis.z = matRot[2][1];
199
200 zaxis.x = matRot[0][2];
201 zaxis.y = matRot[1][2];
202 zaxis.z = matRot[2][2];
203 }
204
205 bool Quaternion::toEulerAngles(Radian& xAngle, Radian& yAngle, Radian& zAngle) const
206 {
207 Matrix3 matRot;
208 toRotationMatrix(matRot);
209 return matRot.toEulerAngles(xAngle, yAngle, zAngle);
210 }
211
212 Vector3 Quaternion::xAxis() const
213 {
214 float fTy = 2.0f*y;
215 float fTz = 2.0f*z;
216 float fTwy = fTy*w;
217 float fTwz = fTz*w;
218 float fTxy = fTy*x;
219 float fTxz = fTz*x;
220 float fTyy = fTy*y;
221 float fTzz = fTz*z;
222
223 return Vector3(1.0f-(fTyy+fTzz), fTxy+fTwz, fTxz-fTwy);
224 }
225
226 Vector3 Quaternion::yAxis() const
227 {
228 float fTx = 2.0f*x;
229 float fTy = 2.0f*y;
230 float fTz = 2.0f*z;
231 float fTwx = fTx*w;
232 float fTwz = fTz*w;
233 float fTxx = fTx*x;
234 float fTxy = fTy*x;
235 float fTyz = fTz*y;
236 float fTzz = fTz*z;
237
238 return Vector3(fTxy-fTwz, 1.0f-(fTxx+fTzz), fTyz+fTwx);
239 }
240
241 Vector3 Quaternion::zAxis() const
242 {
243 float fTx = 2.0f*x;
244 float fTy = 2.0f*y;
245 float fTz = 2.0f*z;
246 float fTwx = fTx*w;
247 float fTwy = fTy*w;
248 float fTxx = fTx*x;
249 float fTxz = fTz*x;
250 float fTyy = fTy*y;
251 float fTyz = fTz*y;
252
253 return Vector3(fTxz+fTwy, fTyz-fTwx, 1.0f-(fTxx+fTyy));
254 }
255
256 Quaternion Quaternion::inverse() const
257 {
258 float fNorm = w*w+x*x+y*y+z*z;
259 if (fNorm > 0.0f)
260 {
261 float fInvNorm = 1.0f/fNorm;
262 return Quaternion(w*fInvNorm,-x*fInvNorm,-y*fInvNorm,-z*fInvNorm);
263 }
264 else
265 {
266 // Return an invalid result to flag the error
267 return ZERO;
268 }
269 }
270
271 Vector3 Quaternion::rotate(const Vector3& v) const
272 {
273 // Note: Does compiler generate fast code here? Perhaps its better to pull all code locally without constructing
274 // an intermediate matrix.
275 Matrix3 rot;
276 toRotationMatrix(rot);
277 return rot.multiply(v);
278 }
279
280 void Quaternion::lookRotation(const Vector3& forwardDir)
281 {
282 if (forwardDir == Vector3::ZERO)
283 return;
284
285 Vector3 nrmForwardDir = Vector3::normalize(forwardDir);
286 Vector3 currentForwardDir = -zAxis();
287
288 if ((nrmForwardDir + currentForwardDir).squaredLength() < 0.00005f)
289 {
290 // Oops, a 180 degree turn (infinite possible rotation axes)
291 // Default to yaw i.e. use current UP
292 *this = Quaternion(-y, -z, w, x);
293 }
294 else
295 {
296 // Derive shortest arc to new direction
297 Quaternion rotQuat = getRotationFromTo(currentForwardDir, nrmForwardDir);
298 *this = rotQuat * *this;
299 }
300 }
301
302 void Quaternion::lookRotation(const Vector3& forwardDir, const Vector3& upDir)
303 {
304 Vector3 forward = Vector3::normalize(forwardDir);
305 Vector3 up = Vector3::normalize(upDir);
306
307 if (Math::approxEquals(Vector3::dot(forward, up), 1.0f))
308 {
309 lookRotation(forward);
310 return;
311 }
312
313 Vector3 x = Vector3::cross(forward, up);
314 Vector3 y = Vector3::cross(x, forward);
315
316 x.normalize();
317 y.normalize();
318
319 *this = Quaternion(x, y, -forward);
320 }
321
322 Quaternion Quaternion::slerp(float t, const Quaternion& p, const Quaternion& q, bool shortestPath)
323 {
324 float cos = p.dot(q);
325 Quaternion quat;
326
327 if (cos < 0.0f && shortestPath)
328 {
329 cos = -cos;
330 quat = -q;
331 }
332 else
333 {
334 quat = q;
335 }
336
337 if (Math::abs(cos) < 1 - EPSILON)
338 {
339 // Standard case (slerp)
340 float sin = Math::sqrt(1 - Math::sqr(cos));
341 Radian angle = Math::atan2(sin, cos);
342 float invSin = 1.0f / sin;
343 float coeff0 = Math::sin((1.0f - t) * angle) * invSin;
344 float coeff1 = Math::sin(t * angle) * invSin;
345 return coeff0 * p + coeff1 * quat;
346 }
347 else
348 {
349 // There are two situations:
350 // 1. "p" and "q" are very close (fCos ~= +1), so we can do a linear
351 // interpolation safely.
352 // 2. "p" and "q" are almost inverse of each other (fCos ~= -1), there
353 // are an infinite number of possibilities interpolation. but we haven't
354 // have method to fix this case, so just use linear interpolation here.
355 Quaternion ret = (1.0f - t) * p + t * quat;
356
357 // Taking the complement requires renormalization
358 ret.normalize();
359 return ret;
360 }
361 }
362
363 Quaternion Quaternion::getRotationFromTo(const Vector3& from, const Vector3& dest, const Vector3& fallbackAxis)
364 {
365 // Based on Stan Melax's article in Game Programming Gems
366 Quaternion q;
367
368 Vector3 v0 = from;
369 Vector3 v1 = dest;
370 v0.normalize();
371 v1.normalize();
372
373 float d = v0.dot(v1);
374
375 // If dot == 1, vectors are the same
376 if (d >= 1.0f)
377 return Quaternion::IDENTITY;
378
379 if (d < (1e-6f - 1.0f))
380 {
381 if (fallbackAxis != Vector3::ZERO)
382 {
383 // Rotate 180 degrees about the fallback axis
384 q.fromAxisAngle(fallbackAxis, Radian(Math::PI));
385 }
386 else
387 {
388 // Generate an axis
389 Vector3 axis = Vector3::UNIT_X.cross(from);
390 if (axis.isZeroLength()) // Pick another if colinear
391 axis = Vector3::UNIT_Y.cross(from);
392 axis.normalize();
393 q.fromAxisAngle(axis, Radian(Math::PI));
394 }
395 }
396 else
397 {
398 float s = Math::sqrt( (1+d)*2 );
399 float invs = 1 / s;
400
401 Vector3 c = v0.cross(v1);
402
403 q.x = c.x * invs;
404 q.y = c.y * invs;
405 q.z = c.z * invs;
406 q.w = s * 0.5f;
407 q.normalize();
408 }
409
410 return q;
411 }
412}
413