EHS
Loading...
Searching...
No Matches
Quat.h
Go to the documentation of this file.
1#pragma once
2
3#include "EHS.h"
4#include "Math.h"
5#include "Mat4.h"
6#include "Vec3.h"
7
8namespace ehs
9{
10 template<typename T>
11 class Quat
12 {
13 public:
14 T w;
15 T x;
16 T y;
17 T z;
18
20 : w(0), x(0), y(0), z(0)
21 {
22 }
23
24 Quat(const T w, const T x, const T y, const T z)
25 : w(w), x(x), y(y), z(z)
26 {
27 }
28
29 Quat(const T yaw, const T pitch, const T roll)
30 : w(0), x(0), y(0), z(0)
31 {
32 T c1 = cos(yaw / 2);
33 T c2 = cos(pitch / 2);
34 T c3 = cos(roll / 2);
35 T s1 = sin(yaw / 2);
36 T s2 = sin(pitch / 2);
37 T s3 = sin(roll / 2);
38
39 w = c1 * c2 * c3 - s1 * s2 * s3;
40 x = s1 * s2 * c3 + c1 * c2 * s3;
41 y = s1 * c2 * c3 + c1 * s2 * s3;
42 z = c1 * s2 * c3 - s1 * c2 * s3;
43 }
44
45 explicit Quat(const Vec3<T>& euler)
46 : w(0), x(0), y(0), z(0)
47 {
48 T c1 = cos(euler.x / 2);
49 T c2 = cos(euler.y / 2);
50 T c3 = cos(euler.z / 2);
51 T s1 = sin(euler.x / 2);
52 T s2 = sin(euler.y / 2);
53 T s3 = sin(euler.z / 2);
54
55 w = c1 * c2 * c3 - s1 * s2 * s3;
56 x = s1 * s2 * c3 + c1 * c2 * s3;
57 y = s1 * c2 * c3 + c1 * s2 * s3;
58 z = c1 * s2 * c3 - s1 * c2 * s3;
59 }
60
61 Quat(const Vec3<T>& n, const T a)
62 : w(cosf(a / 2)), x(n.x * sinf(a / 2)), y(n.y * sinf(a / 2)), z(n.z * sinf(a / 2))
63 {
64 }
65
66 explicit Quat(const Mat4<T>& rotMatrix)
67 : w(0), x(0), y(0), z(0)
68 {
69 ToQuaternion(rotMatrix);
70 }
71
72 Quat(const Quat& quat)
73 : w(quat.w), x(quat.x), y(quat.y), z(quat.z)
74 {
75 }
76
77 explicit Quat(const T scalar)
78 : w(scalar), x(scalar), y(scalar), z(scalar)
79 {
80 }
81
82 Quat& operator=(const Quat& quat)
83 {
84 if (this == &quat)
85 return *this;
86
87 w = quat.w;
88 x = quat.x;
89 y = quat.y;
90 z = quat.z;
91
92 return *this;
93 }
94
95 Quat& operator=(const Mat4<T>& rotMatrix)
96 {
97 ToQuaternion(rotMatrix);
98
99 return *this;
100 }
101
102 Quat operator+(const Quat& other) const
103 {
104 return {w + other.w, x + other.x, y + other.y, z + other.z};
105 }
106
108 {
109 return {-w, -x, -y, -z};
110 }
111
112 Quat operator-(const Quat& other) const
113 {
114 return {w - other.w, x - other.x, y - other.y, z - other.z};
115 }
116
117 Quat operator*(const T scalar)
118 {
119 return {w * scalar, x * scalar, x * scalar, x * scalar};
120 }
121
122 Quat operator*(const Quat& other)
123 {
124 return Quat
125 (
126 w * other.w - x * other.x - y * other.y - z * other.z,
127 w * other.x + x * other.w + y * other.z - z * other.y,
128 w * other.y - x * other.z + y * other.w + z * other.x,
129 w * other.z + x * other.y - y * other.x + z * other.w
130 );
131 }
132
134 {
135 Quat tmp(0, vect[0], vect[1], vect[2]);
136 Vec3<T> tmpVect(x, y, z);
137
138 Vec3<T> vcV = tmpVect.CrossProduct(vect);
139 return vect + vcV * (2 * w) + tmpVect.CrossProduct(vcV) * 2;
140 }
141
142 Quat operator^(const T t)
143 {
144 Vec3<T> n;
145 T a;
146
147 ToAxisAngle(&n, &a);
148
149 float at = a * t;
150
151 return Quat<T>(n, at);
152 }
153
154 bool operator==(const Quat& quat) const
155 {
156 return w == quat.w && x == quat.x && y == quat.y && z == quat.z;
157 }
158
159 bool operator!=(const Quat& quat) const
160 {
161 return w != quat.w || x != quat.x || y != quat.y || z == quat.z;
162 }
163
164 T operator[](const UInt_64 index) const
165 {
166 switch (index)
167 {
168 case 0:
169 return w;
170 case 1:
171 return x;
172 case 2:
173 return y;
174 case 3:
175 return z;
176 default:
177 return w;
178 }
179 }
180
181 T& operator[](const UInt_64 index)
182 {
183 switch (index)
184 {
185 case 0:
186 return w;
187 case 1:
188 return x;
189 case 2:
190 return y;
191 case 3:
192 return z;
193 default:
194 return w;
195 }
196 }
197
198 void ToAxisAngle(Vec3<T>* vectAxis, T* flAngle)
199 {
200 Vec3<T> tmp(x, y, z);
201
202 if (tmp.GetDis2() < 0.0001f)
203 *vectAxis = Vec3<T>(1, 0, 0);
204 else
205 *vectAxis = tmp.GetNorm();
206
207 *flAngle = acosf(w) * 2;
208 *flAngle = Math::Degr<T>(*flAngle);
209 }
210
211 void ToQuaternion(const Mat4<T>& rotMatrix)
212 {
213 T trace = rotMatrix[0][0] + rotMatrix[1][1] + rotMatrix[2][2];
214
215 if (trace > 0)
216 {
217 T s = 0.5f / Math::Sqrt<T>(trace + 1.0f);
218 w = 0.25f / s;
219
220 x = (rotMatrix[2][1] - rotMatrix[1][2]) * s;
221 y = (rotMatrix[0][2] - rotMatrix[2][0]) * s;
222 z = (rotMatrix[1][0] - rotMatrix[0][1]) * s;
223 } else
224 {
225 if ((rotMatrix[0][0] > rotMatrix[1][1]) && (rotMatrix[0][0] > rotMatrix[2][2]))
226 {
227 T s = 2.0f * Math::Sqrt(1.0f + rotMatrix[0][0] - rotMatrix[1][1] - rotMatrix[2][2]);
228 w = (rotMatrix[2][1] - rotMatrix[1][2]) / s;
229 x = 0.25f * s;
230 y = (rotMatrix[0][1] + rotMatrix[1][0]) / s;
231 z = (rotMatrix[0][2] + rotMatrix[2][0]) / s;
232 } else if (rotMatrix[1][1] > rotMatrix[2][2])
233 {
234 T s = 2.0f * sqrtf(1.0f + rotMatrix[1][1] - rotMatrix[0][0] - rotMatrix[2][2]);
235 w = (rotMatrix[0][2] - rotMatrix[2][0]) / s;
236 x = (rotMatrix[0][1] + rotMatrix[1][0]) / s;
237 y = 0.25f * s;
238 z = (rotMatrix[1][2] + rotMatrix[2][1]) / s;
239 } else
240 {
241 T s = 2.0f * sqrtf(1.0f + rotMatrix[2][2] - rotMatrix[0][0] - rotMatrix[1][1]);
242 w = (rotMatrix[1][0] - rotMatrix[0][1]) / s;
243 x = (rotMatrix[0][2] + rotMatrix[2][0]) / s;
244 y = (rotMatrix[1][2] + rotMatrix[2][1]) / s;
245 z = 0.25f * s;
246 }
247 }
248 }
249
250 /*
251 Vec3<T> ToEulerAngle() const
252 {
253 Vec3<T> euler;
254
255 float ysqr = y * y;
256
257 float t0 = 2 * (w * x + y * z);
258 float t1 = 1 - 2 * (x * x + ysqr);
259 euler.z = std::atan2(t0, t1);
260
261 float t2 = 2 * (w * y - z * x);
262 t2 = t2 > 1 ? 1 : t2;
263 t2 = t2 < -1 ? -1 : t2;
264 euler.y = std::asin(t2);
265
266 float t3 = 2 * (w * z + x * y);
267 float t4 = 1 - 2 * (ysqr + z * z);
268 euler.x = std::atan2(t3, t4);
269
270 return euler;
271 }
272 */
273
275 {
276 Mat4<T> result;
277
278 T x2 = x + x;
279 T y2 = y + y;
280 T z2 = z + z;
281 T x2w = x2 * w;
282 T y2w = y2 * w;
283 T z2w = z2 * w;
284 T x2x = x2 * x;
285 T y2x = y2 * x;
286 T z2x = z2 * x;
287 T y2y = y2 * y;
288 T z2y = z2 * y;
289 T z2z = z2 * y;
290
291 result[0] = T(1) - (y2y + z2z);
292 result[1] = y2x - z2w;
293 result[2] = z2x + y2w;
294 result[3] = T(0);
295
296 result[4] = y2x + z2w;
297 result[5] = T(1) - (x2x + z2z);
298 result[6] = z2y - x2w;
299 result[7] = T(0);
300
301 result[8] = z2x - y2w;
302 result[9] = z2y + x2w;
303 result[10] = T(1) - (x2x + y2y);
304 result[11] = T(0);
305
306 result[12] = T(0);
307 result[13] = T(0);
308 result[14] = T(0);
309 result[15] = T(1);
310
311 return result;
312 }
313
315 {
316 return Math::Sqrt<T>(Math::Pow<T>(w, 2) + Math::Pow<T>(x, 2) + Math::Pow<T>(y, 2) + Math::Pow<T>(z, 2));
317 }
318
320 {
321 T mag = GetMagnitude();
322
323 return Quat<T>(w / mag, x / mag, y / mag, z / mag);
324 }
325
327 {
328 T mag = GetMagnitude();
329
330 w = w / mag;
331 x = x / mag;
332 y = y / mag;
333 z = z / mag;
334 }
335
336 T Dot(const Quat& other) const
337 {
338 return w * other.w + x * other.x + y * other.y + z * other.z;
339 }
340
342 {
343 return Quat<T>(w, -x, -y, -z);
344 }
345
347 {
348 x = -x;
349 y = -y;
350 z = -z;
351 }
352
354 {
355 return Quat<T>(w, -x, -y, -z);
356 }
357
358 void Inverse()
359 {
360 x = -x;
361 y = -y;
362 z = -z;
363 }
364
365 static Quat<T> Slerp(Quat<T> start, Quat<T> finish, const T t)
366 {
367 T cosHalfTheta = start.Dot(finish);
368 if (Math::Abs(cosHalfTheta) >= 1.0f)
369 return start;
370
371 float halfTheta = Math::ACos(cosHalfTheta);
372 float sinHalfTheta = Math::Sqrt(1.0f - cosHalfTheta * cosHalfTheta);
373 if (Math::Abs(sinHalfTheta) < 0.001f)
374 {
375 return {
376 start.w * 0.5f + finish.w * 0.5f,
377 start.x * 0.5f + finish.x * 0.5f,
378 start.y * 0.5f + finish.y * 0.5f,
379 start.z * 0.5f + finish.z * 0.5f
380 };
381 }
382
383 float ratioA = Math::Sin((1 - t) * halfTheta) / sinHalfTheta;
384 float ratioB = Math::Sin(t * halfTheta) / sinHalfTheta;
385
386 return {
387 start.w * ratioA + finish.w * ratioB,
388 start.x * ratioA + finish.x * ratioB,
389 start.y * ratioA + finish.y * ratioB,
390 start.z * ratioA + finish.z * ratioB
391 };
392 }
393 };
394
396}
Definition Mat4.h:13
static T Pow(const T base, const I exponent)
Definition Math.h:173
static T Degr(const T from)
Definition Math.h:99
static R Sin(const R angle, const R precision=0.001)
Definition Math.h:257
static R ACos(const R xPos, const T precision=10)
Definition Math.h:328
static T Abs(const T from)
Definition Math.h:70
static T Sqrt(const T from)
Definition Math.h:238
Definition Quat.h:12
Quat operator^(const T t)
Definition Quat.h:142
T & operator[](const UInt_64 index)
Definition Quat.h:181
Quat & operator=(const Mat4< T > &rotMatrix)
Definition Quat.h:95
float x
Definition Quat.h:15
Quat(const Quat &quat)
Definition Quat.h:72
Quat(const Mat4< T > &rotMatrix)
Definition Quat.h:66
Quat< T > GetInverse()
Definition Quat.h:353
void Inverse()
Definition Quat.h:358
void ToAxisAngle(Vec3< T > *vectAxis, T *flAngle)
Definition Quat.h:198
Quat(const T scalar)
Definition Quat.h:77
Mat4< T > ToMatrix() const
Definition Quat.h:274
bool operator!=(const Quat &quat) const
Definition Quat.h:159
Quat operator*(const T scalar)
Definition Quat.h:117
Quat(const Vec3< T > &n, const T a)
Definition Quat.h:61
Quat< T > GetConjugate()
Definition Quat.h:341
static Quat< T > Slerp(Quat< T > start, Quat< T > finish, const T t)
Definition Quat.h:365
Quat< T > GetNormalized()
Definition Quat.h:319
Quat(const T w, const T x, const T y, const T z)
Definition Quat.h:24
Quat operator-(const Quat &other) const
Definition Quat.h:112
T Dot(const Quat &other) const
Definition Quat.h:336
Quat operator-() const
Definition Quat.h:107
bool operator==(const Quat &quat) const
Definition Quat.h:154
Quat(const Vec3< T > &euler)
Definition Quat.h:45
void Conjugate()
Definition Quat.h:346
Vec3< T > operator*(const Vec3< T > &vect)
Definition Quat.h:133
void ToQuaternion(const Mat4< T > &rotMatrix)
Definition Quat.h:211
float y
Definition Quat.h:16
float z
Definition Quat.h:17
T operator[](const UInt_64 index) const
Definition Quat.h:164
Quat & operator=(const Quat &quat)
Definition Quat.h:82
Quat(const T yaw, const T pitch, const T roll)
Definition Quat.h:29
Quat operator*(const Quat &other)
Definition Quat.h:122
Quat operator+(const Quat &other) const
Definition Quat.h:102
void Normalize()
Definition Quat.h:326
float GetMagnitude()
Definition Quat.h:314
Quat()
Definition Quat.h:19
float w
Definition Quat.h:14
Definition Vec3.h:14
T x
Definition Vec3.h:16
Vec3< T > GetNorm() const
Definition Vec3.h:313
T z
Definition Vec3.h:18
T y
Definition Vec3.h:17
Definition Anchor.h:6
Quat< float > Quat_f
Definition Quat.h:395