CSEngine
Loading...
Searching...
No Matches
Quaternion.h
1#pragma once
2
3#include "Matrix.h"
4#include "MoreMath.h"
5
6namespace CSE {
7 template <typename T>
8 struct QuaternionT {
9 T x;
10 T y;
11 T z;
12 T w;
13
14 QuaternionT();
15
16 QuaternionT(T x, T y, T z, T w);
17
18 void Set(T x, T y, T z, T w);
19
20 QuaternionT<T> Clone() const;
21
22 QuaternionT<T> Lerp(float t, const QuaternionT<T>& q) const;
23
24 QuaternionT<T> Slerp(float t, const QuaternionT<T>& q) const;
25
26 QuaternionT<T> Rotated(const QuaternionT<T>& b) const;
27
28 QuaternionT<T> Multiplied(const QuaternionT<T>& q2) const;
29
30 QuaternionT<T> Scaled(T scale) const;
31
32 T Dot(const QuaternionT<T>& q) const;
33
34 Matrix3 <T> ToMatrix3() const;
35
36 Matrix4 <T> ToMatrix4() const;
37
38 Vector4<T> ToVector() const;
39
40 Vector3<T> ToEulerAngle() const;
41
42 QuaternionT<T> operator-(const QuaternionT<T>& q) const;
43
44 QuaternionT<T> operator+(const QuaternionT<T>& q) const;
45
46 bool operator==(const QuaternionT<T>& q) const;
47
48 bool operator!=(const QuaternionT<T>& q) const;
49
50 void Normalize();
51
52 void Rotate(const QuaternionT<T>& q);
53
54 void Conjugate();
55
56 static QuaternionT<T> CreateFromVectors(const Vector3<T>& v0, const Vector3<T>& v1);
57
58 static QuaternionT<T> AngleAxis(const Vector3<T>& axis, float radians);
59
60 static QuaternionT<T> ToQuaternion(const Matrix4 <T>& matrix);
61 };
62
63 template <typename T>
64 inline QuaternionT<T>::QuaternionT() : x(0), y(0), z(0), w(1) {
65 }
66
67 template <typename T>
68 inline QuaternionT<T>::QuaternionT(T x, T y, T z, T w) : x(x), y(y), z(z), w(w) {
69 }
70
71 template <typename T>
72 void QuaternionT<T>::Set(T x, T y, T z, T w) {
73 this->x = x;
74 this->y = y;
75 this->z = z;
76 this->w = w;
77 }
78
79 template <typename T>
80 QuaternionT<T> QuaternionT<T>::Clone() const {
81 return QuaternionT<T>(x, y, z, w);
82 }
83
84 template <typename T>
85 inline QuaternionT<T> QuaternionT<T>::Lerp(float t, const QuaternionT<T>& q) const {
86
87 QuaternionT<T> result = QuaternionT<T>(0, 0, 0, 1);
88 float dot = w * q.w + x * q.x + y * q.y + z * q.z;
89 float deltaTime = 1.f - t;
90 if (dot < 0) {
91 result.w = deltaTime * w + t * -q.w;
92 result.x = deltaTime * x + t * -q.x;
93 result.y = deltaTime * y + t * -q.y;
94 result.z = deltaTime * z + t * -q.z;
95 } else {
96 result.w = deltaTime * w + t * q.w;
97 result.x = deltaTime * x + t * q.x;
98 result.y = deltaTime * y + t * q.y;
99 result.z = deltaTime * z + t * q.z;
100 }
101 result.Normalize();
102
103 return result;
104 }
105
106 template <typename T>
107 inline QuaternionT<T> QuaternionT<T>::Slerp(float t, const QuaternionT<T>& q) const {
108
109 // Get cosine of angle between quats.
110 const float rawCosom = x * q.x + y * q.y + z * q.z + w * q.w;
111 // Unaligned quats - compensate, results in taking shorter route.
112 const auto cosom = ValueSelect<float>(rawCosom, rawCosom, -rawCosom);
113
114 float scale0, scale1;
115
116 if (cosom < 0.9999f) {
117 const float omega = acosf(cosom);
118 const float invSin = 1.f / sinf(omega);
119 scale0 = sinf((1.f - t) * omega) * invSin;
120 scale1 = sinf(t * omega) * invSin;
121 }
122 else {
123 // Use linear interpolation.
124 scale0 = 1.0f - t;
125 scale1 = t;
126 }
127
128 // In keeping with our flipped cosom:
129 scale1 = ValueSelect<float>(rawCosom, scale1, -scale1);
130
131 QuaternionT<T> result;
132
133 result.x = scale0 * x + scale1 * q.x;
134 result.y = scale0 * y + scale1 * q.y;
135 result.z = scale0 * z + scale1 * q.z;
136 result.w = scale0 * w + scale1 * q.w;
137
138 return result;
139 }
140
141 template <typename T>
142 inline QuaternionT<T> QuaternionT<T>::Rotated(const QuaternionT<T>& b) const {
143 QuaternionT<T> q;
144 q.w = w * b.w - x * b.x - y * b.y - z * b.z;
145 q.x = w * b.x + x * b.w + y * b.z - z * b.y;
146 q.y = w * b.y + y * b.w + z * b.x - x * b.z;
147 q.z = w * b.z + z * b.w + x * b.y - y * b.x;
148 q.Normalize();
149 return q;
150 }
151
152 template <typename T>
153 inline QuaternionT<T> QuaternionT<T>::Multiplied(const QuaternionT<T>& q2) const {
154 QuaternionT<T> res;
155 T A, B, C, D, E, F, G, H;
156
157 A = (w + x) * (q2.w + q2.x);
158
159 B = (z - y) * (q2.y - q2.z);
160
161 C = (w - x) * (q2.y + q2.z);
162
163 D = (y + z) * (q2.w - q2.x);
164
165 E = (x + z) * (q2.x + q2.y);
166
167 F = (x - z) * (q2.x - q2.y);
168
169 G = (w + y) * (q2.w - q2.z);
170
171 H = (w - y) * (q2.w + q2.z);
172
173
174 res.w = B + (-E - F + G + H) / 2;
175
176 res.x = A - (E + F + G + H) / 2;
177
178 res.y = C + (E - F + G - H) / 2;
179
180 res.z = D + (E - F - G + H) / 2;
181
182
183 return res;
184 }
185
186 template <typename T>
187 inline QuaternionT<T> QuaternionT<T>::Scaled(T s) const {
188 return QuaternionT<T>(x * s, y * s, z * s, w * s);
189 }
190
191 template <typename T>
192 inline T QuaternionT<T>::Dot(const QuaternionT<T>& q) const {
193 return x * q.x + y * q.y + z * q.z + w * q.w;
194 }
195
196 template <typename T>
197 inline Matrix3 <T> QuaternionT<T>::ToMatrix3() const {
198 QuaternionT<T> q = *this;
199 q.Normalize();
200
201 float result[9] = { 1.0f - 2.0f * q.y * q.y - 2.0f * q.z * q.z, 2.0f * q.x * q.y - 2.0f * q.z * q.w,
202 2.0f * q.x * q.z + 2.0f * q.y * q.w,
203 2.0f * q.x * q.y + 2.0f * q.z * q.w, 1.0f - 2.0f * q.x * q.x - 2.0f * q.z * q.z,
204 2.0f * q.y * q.z - 2.0f * q.x * q.w,
205 2.0f * q.x * q.z - 2.0f * q.y * q.w, 2.0f * q.y * q.z + 2.0f * q.x * q.w,
206 1.0f - 2.0f * q.x * q.x - 2.0f * q.y * q.y };
207 return Matrix3<T>(result);
208// const T s = 2;
209// T xs, ys, zs;
210// T wx, wy, wz;
211// T xx, xy, xz;
212// T yy, yz, zz;
213// xs = x * s; ys = y * s; zs = z * s;
214// wx = w * xs; wy = w * ys; wz = w * zs;
215// xx = x * xs; xy = x * ys; xz = x * zs;
216// yy = y * ys; yz = y * zs; zz = z * zs;
217// Matrix3<T> m;
218// m.x.x = 1 - (yy + zz); m.y.x = xy - wz; m.z.x = xz + wy;
219// m.x.y = xy + wz; m.y.y = 1 - (xx + zz); m.z.y = yz - wx;
220// m.x.z = xz - wy; m.y.z = yz + wx; m.z.z = 1 - (xx + yy);
221// return m;
222 }
223
224 template <typename T>
225 inline Matrix4 <T> QuaternionT<T>::ToMatrix4() const {
226 Matrix4<T> matrix = Matrix4<T>::Identity();
227 T xy = x * y;
228 T xz = x * z;
229 T xw = x * w;
230 T yz = y * z;
231 T yw = y * w;
232 T zw = z * w;
233 T xSquared = x * x;
234 T ySquared = y * y;
235 T zSquared = z * z;
236 matrix.x.x = 1 - 2 * (ySquared + zSquared);
237 matrix.x.y = 2 * (xy - zw);
238 matrix.x.z = 2 * (xz + yw);
239 matrix.x.w = 0;
240 matrix.y.x = 2 * (xy + zw);
241 matrix.y.y = 1 - 2 * (xSquared + zSquared);
242 matrix.y.z = 2 * (yz - xw);
243 matrix.y.w = 0;
244 matrix.z.x = 2 * (xz - yw);
245 matrix.z.y = 2 * (yz + xw);
246 matrix.z.z = 1 - 2 * (xSquared + ySquared);
247 matrix.z.w = 0;
248 matrix.w.x = 0;
249 matrix.w.y = 0;
250 matrix.w.z = 0;
251 matrix.w.w = 1;
252 return matrix;
253 }
254
255 template <typename T>
256 inline Vector4<T> QuaternionT<T>::ToVector() const {
257 return Vector4<T>(x, y, z, w);
258 }
259
260 template <typename T>
261 inline Vector3<T> QuaternionT<T>::ToEulerAngle() const {
262
263 // T roll, pitch, yaw;
264
265 // // roll (x-axis rotation)
266 // T sinr_cosp = +2.0 * (w * x + y * z);
267 // T cosr_cosp = +1.0 - 2.0 * (x * x + y * y);
268 // roll = atan2(sinr_cosp, cosr_cosp);
269
270 // // pitch (y-axis rotation)
271 // T sinp = +2.0 * (w * y - z * x);
272 // if (fabs(sinp) >= 1)
273 // pitch = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
274 // else
275 // pitch = asin(sinp);
276
277 // // yaw (z-axis rotation)
278 // T siny_cosp = +2.0 * (w * z + x * y);
279 // T cosy_cosp = +1.0 - 2.0 * (y * y + z * z);
280 // yaw = atan2(siny_cosp, cosy_cosp);
281
282 // return Vector3<T>(roll, pitch, yaw);
283
284 T heading;
285 T attitude;
286 T bank;
287
288 T sqw = w * w;
289 T sqx = x * x;
290 T sqy = y * y;
291 T sqz = z * z;
292 T unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
293 T test = x * y + z * w;
294 if (test > 0.499 * unit) { // singularity at North Pole
295 heading = 2 * atan2(x, w);
296 attitude = M_PI / 2;
297 bank = 0;
298 return Vector3<T>(bank, heading, attitude);
299 }
300 if (test < -0.499 * unit) { // singularity at South Pole
301 heading = -2 * atan2(x, w);
302 attitude = -M_PI / 2;
303 bank = 0;
304 return Vector3<T>(bank, heading, attitude);
305
306 }
307 heading = atan2(2 * y * w - 2 * x * z, sqx - sqy - sqz + sqw);
308 attitude = asin(2 * test / unit);
309 bank = atan2(2 * x * w - 2 * y * z, -sqx + sqy - sqz + sqw);
310
311 return Vector3<T>(bank, heading, attitude);
312 }
313
314 template <typename T>
315 QuaternionT<T> QuaternionT<T>::operator-(const QuaternionT<T>& q) const {
316 return QuaternionT<T>(x - q.x, y - q.y, z - q.z, w - q.w);
317 }
318
319 template <typename T>
320 QuaternionT<T> QuaternionT<T>::operator+(const QuaternionT<T>& q) const {
321 return QuaternionT<T>(x + q.x, y + q.y, z + q.z, w + q.w);
322 }
323
324 template <typename T>
325 bool QuaternionT<T>::operator==(const QuaternionT<T>& q) const {
326 return x == q.x && y == q.y && z == q.z && w == q.w;
327 }
328
329 template <typename T>
330 bool QuaternionT<T>::operator!=(const QuaternionT<T>& q) const {
331 return *this != q;
332 }
333
334// Compute the quaternion that rotates from a to b, avoiding numerical instability.
335// Taken from "The Shortest Arc Quaternion" by Stan Melax in "Game Programming Gems".
336 template <typename T>
337 inline QuaternionT<T> QuaternionT<T>::CreateFromVectors(const Vector3<T>& v0, const Vector3<T>& v1) {
338 if (v0 == -v1)
339 return QuaternionT<T>::AngleAxis(vec3(1, 0, 0), Pi);
340
341 Vector3<T> c = v0.Cross(v1);
342 T d = v0.Dot(v1);
343 T s = std::sqrt((1 + d) * 2);
344
345 QuaternionT<T> q;
346 q.x = c.x / s;
347 q.y = c.y / s;
348 q.z = c.z / s;
349 q.w = s / 2.0f;
350 return q;
351 }
352
353 template <typename T>
354 inline QuaternionT<T> QuaternionT<T>::AngleAxis(const Vector3<T>& axis, float radians) {
355 QuaternionT<T> q;
356 q.w = std::cos(radians / 2);
357 q.x = q.y = q.z = std::sin(radians / 2);
358 q.x *= axis.x;
359 q.y *= axis.y;
360 q.z *= axis.z;
361 return q;
362
363 // T cy = cos(axis.z * radians * 0.5);
364 // T sy = sin(axis.z * radians * 0.5);
365 // T cp = cos(axis.y * radians * 0.5);
366 // T sp = sin(axis.y * radians * 0.5);
367 // T cr = cos(axis.x * radians * 0.5);
368 // T sr = sin(axis.x * radians * 0.5);
369
370 // QuaternionT<T> q;
371 // q.w = cy * cp * cr + sy * sp * sr;
372 // q.x = cy * cp * sr - sy * sp * cr;
373 // q.y = sy * cp * sr + cy * sp * cr;
374 // q.z = sy * cp * cr - cy * sp * sr;
375 // return q;
376 }
377
378 template <typename T>
379 inline void QuaternionT<T>::Normalize() {
380 *this = Scaled(1 / std::sqrt(Dot(*this)));
381 }
382
383 template <typename T>
384 inline void QuaternionT<T>::Rotate(const QuaternionT<T>& q2) {
385 QuaternionT<T> q;
386 QuaternionT<T>& q1 = *this;
387
388 q.w = w * q2.w - x * q2.x - y * q2.y - z * q2.z;
389 q.x = w * q2.x + x * q2.w + y * q2.z - z * q2.y;
390 q.y = w * q2.y + y * q2.w + z * q2.x - x * q2.z;
391 q.z = w * q2.z + z * q2.w + x * q2.y - y * q2.x;
392
393 q.Normalize();
394 *this = q;
395 }
396
397 template <typename T>
398 inline void QuaternionT<T>::Conjugate() {
399 QuaternionT<T> q;
400 QuaternionT<T>& q1 = *this;
401
402 q.w = w;
403 q.x = -x;
404 q.y = -y;
405 q.z = -z;
406
407 *this = q;
408 }
409
410 template <typename T>
411 QuaternionT<T> QuaternionT<T>::ToQuaternion(const Matrix4 <T>& matrix) {
412 QuaternionT<T> q;
413 q.w = sqrt(fmax(0, 1 + matrix.x.x + matrix.y.y + matrix.z.z)) / 2;
414 q.x = sqrt(fmax(0, 1 + matrix.x.x - matrix.y.y - matrix.z.z)) / 2;
415 q.y = sqrt(fmax(0, 1 - matrix.x.x + matrix.y.y - matrix.z.z)) / 2;
416 q.z = sqrt(fmax(0, 1 - matrix.x.x - matrix.y.y + matrix.z.z)) / 2;
417 q.x *= sign<T>(q.x * (matrix.z.y - matrix.y.z));
418 q.y *= sign<T>(q.y * (matrix.x.z - matrix.z.x));
419 q.z *= sign<T>(q.z * (matrix.y.x - matrix.x.y));
420 return q;
421 }
422
423 typedef QuaternionT<float> Quaternion;
424}