Nebula
Loading...
Searching...
No Matches
quat.h
Go to the documentation of this file.
1#pragma once
2//------------------------------------------------------------------------------
11#include "core/types.h"
12#include "math/scalar.h"
13#include "math/vec4.h"
14#include "math/sse.h"
15
16//------------------------------------------------------------------------------
17namespace Math
18{
19struct quat;
20
21quat slerp(const quat& q1, const quat& q2, scalar t);
22
23quat rotationmatrix(const mat4& m);
24vec3 to_euler(const quat& q);
25quat from_euler(const vec3& v);
27vec3 rotate(quat const& q, vec3 const& v);
28
30{
31public:
33 quat();
35 quat(quat const&) = default;
37 quat(scalar x, scalar y, scalar z, scalar w);
39 quat(const vec4& rhs);
41 quat(const __m128& rhs);
42
44 void operator=(const __m128& rhs);
46 bool operator==(const quat& rhs) const;
48 bool operator!=(const quat& rhs) const;
49
51 void load(const scalar* ptr);
53 void loadu(const scalar* ptr);
55 void store(scalar* ptr) const;
57 void storeu(scalar* ptr) const;
59 void stream(scalar* ptr) const;
60
62 vec3 x_axis() const;
64 vec3 y_axis() const;
66 vec3 z_axis() const;
67
69 void set(scalar x, scalar y, scalar z, scalar w);
71 void set(vec4 const &f4);
72
73 friend struct mat4;
74 union
75 {
76 struct
77 {
78 float x, y, z, w;
79 };
80 __m128 vec;
81 };
82};
83
84//------------------------------------------------------------------------------
87__forceinline
89{
90 this->vec = _mm_setr_ps(0, 0, 0, 1);
91}
92
93//------------------------------------------------------------------------------
96__forceinline
98{
99 this->vec = _mm_setr_ps(x, y, z, w);
100}
101
102//------------------------------------------------------------------------------
105__forceinline
106quat::quat(const vec4& rhs) :
107 vec(rhs.vec)
108{
109 // empty
110}
111
112//------------------------------------------------------------------------------
115__forceinline
116quat::quat(const __m128& rhs)
117{
118 this->vec = rhs;
119}
120
121//------------------------------------------------------------------------------
124__forceinline void
125quat::operator=(const __m128& rhs)
126{
127 this->vec = rhs;
128}
129
130//------------------------------------------------------------------------------
133__forceinline bool
134quat::operator==(const quat& rhs) const
135{
136 return _mm_movemask_ps(_mm_cmpeq_ps(this->vec, rhs.vec)) == 0x0f;
137}
138
139//------------------------------------------------------------------------------
142__forceinline bool
143quat::operator!=(const quat& rhs) const
144{
145 return _mm_movemask_ps(_mm_cmpeq_ps(this->vec, rhs.vec)) != 0x0f;
146}
147
148//------------------------------------------------------------------------------
152__forceinline void
154{
155 this->vec = _mm_load_ps(ptr);
156}
157
158//------------------------------------------------------------------------------
162__forceinline void
164{
165 this->vec = _mm_loadu_ps(ptr);
166}
167
168//------------------------------------------------------------------------------
172__forceinline void
174{
175 _mm_store_ps(ptr, this->vec);
176}
177
178//------------------------------------------------------------------------------
182__forceinline void
184{
185 _mm_storeu_ps(ptr, this->vec);
186}
187
188//------------------------------------------------------------------------------
191__forceinline void
193{
194 this->store(ptr);
195}
196
197//------------------------------------------------------------------------------
200__forceinline vec3
202{
203 vec3 const v = vec3(1, 0, 0);
204 return rotate(*this, v);
205}
206
207//------------------------------------------------------------------------------
210__forceinline vec3
212{
213 vec3 const v = vec3(0, 1, 0);
214 return rotate(*this, v);
215}
216
217//------------------------------------------------------------------------------
220__forceinline vec3
222{
223 vec3 const v = vec3(0, 0, 1);
224 return rotate(*this, v);
225}
226
227//------------------------------------------------------------------------------
230__forceinline void
232{
233 this->vec = _mm_setr_ps(x, y, z, w);
234}
235
236//------------------------------------------------------------------------------
239__forceinline void
240quat::set(const vec4& f4)
241{
242 this->vec = f4.vec;
243}
244
245//------------------------------------------------------------------------------
248__forceinline bool
250{
251 const quat id(0, 0, 0, 1);
252 return id == q;
253}
254
255//------------------------------------------------------------------------------
258__forceinline scalar
259length(const quat& q)
260{
261 return _mm_cvtss_f32(_mm_sqrt_ss(_mm_dp_ps(q.vec, q.vec, 0xF1)));
262}
263
264//------------------------------------------------------------------------------
267__forceinline scalar
268lengthsq(const quat& q)
269{
270 return _mm_cvtss_f32(_mm_dp_ps(q.vec, q.vec, 0xF1));
271}
272
273//------------------------------------------------------------------------------
276__forceinline quat
278{
279 // nothing to do on the xbox, since denormal numbers are not supported by the vmx unit,
280 // it is being set to zero anyway
281 quat ret;
282#if __WIN32__
283 ret.x = Math::undenormalize(q.x);
284 ret.y = Math::undenormalize(q.y);
285 ret.z = Math::undenormalize(q.z);
286 ret.w = Math::undenormalize(q.w);
287#endif
288 return ret;
289}
290
291//------------------------------------------------------------------------------
294__forceinline quat
295barycentric(const quat& q0, const quat& q1, const quat& q2, scalar f, scalar g)
296{
297 scalar s = f + g;
298 if (s != 0.0f)
299 {
300 quat a = slerp(q0, q1, s);
301 quat b = slerp(q0, q2, s);
302 quat res = slerp(a, b, g / s);
303 return res;
304 }
305 else
306 {
307 return q0;
308 }
309}
310
311//------------------------------------------------------------------------------
314__forceinline quat
316{
317 const __m128 con = { -1.0f, -1.0f, -1.0f, 1.0f };
318 quat qq(_mm_mul_ps(q.vec, con));
319 return qq;
320}
321
322//------------------------------------------------------------------------------
325__forceinline scalar
326dot(const quat& q0, const quat& q1)
327{
328 return _mm_cvtss_f32(_mm_dp_ps(q0.vec, q1.vec, 0xF1));
329}
330
331//------------------------------------------------------------------------------
334__forceinline quat
335quatExp(const quat& q)
336{
337 vec4 f(q.vec);
338 scalar theta = length3(f);
339 scalar costheta = Math::cos(theta);
340 scalar sintheta = Math::sin(theta);
341
342 f *= sintheta / theta;
343 f.w = costheta;
344
345 return quat(f.vec);
346}
347
348//------------------------------------------------------------------------------
351__forceinline quat
353{
354 return quat(_mm_setr_ps(0.0f, 0.0f, 0.0f, 1.0f));
355}
356
357//------------------------------------------------------------------------------
360__forceinline quat
361inverse(const quat& q)
362{
363 scalar len = lengthsq(q);
364 if (len > 0.00001f)
365 {
366 quat con = conjugate(q);
367 __m128 temp = _mm_set1_ps(1.0f / len);
368 con.vec = _mm_mul_ps(con.vec, temp);
369 return con;
370 }
371 return quat(0.0f, 0.0f, 0.0f, 0.0f);
372}
373
374//------------------------------------------------------------------------------
377__forceinline quat
378ln(const quat& q)
379{
380 quat ret;
381
382 scalar a = Math::acos(q.w);
383 scalar isina = 1.0f / Math::sin(a);
384
385 scalar aisina = a * isina;
386 if (isina > 0)
387 {
388 vec3 mul(aisina, aisina, aisina);
389 ret = vec4(vec3(q.vec) * mul, 0.0f);
390 }
391 else
392 {
393 ret.set(0.0f, 0.0f, 0.0f, 0.0f);
394 }
395 return ret;
396}
397
398//------------------------------------------------------------------------------
401__forceinline quat
402operator*(const quat& q0, const quat& q1)
403{
404 static const __m128 controlWZYX = _mm_setr_ps(1, -1, 1, -1);
405 static const __m128 controlZWXY = _mm_setr_ps(1, 1, -1, -1);
406 static const __m128 controlYXWZ = _mm_setr_ps(-1, 1, 1, -1);
407 __m128 res = _mm_shuffle_ps(q1.vec, q1.vec, _MM_SHUFFLE(3, 3, 3, 3));
408 __m128 q1x = _mm_shuffle_ps(q1.vec, q1.vec, _MM_SHUFFLE(0, 0, 0, 0));
409 __m128 q1y = _mm_shuffle_ps(q1.vec, q1.vec, _MM_SHUFFLE(1, 1, 1, 1));
410 __m128 q1z = _mm_shuffle_ps(q1.vec, q1.vec, _MM_SHUFFLE(2, 2, 2, 2));
411
412 res = _mm_mul_ps(res, q0.vec);
413 __m128 q0shuffle = q0.vec;
414 q0shuffle = _mm_shuffle_ps(q0shuffle, q0shuffle, _MM_SHUFFLE(0, 1, 2, 3));
415
416 q1x = _mm_mul_ps(q1x, q0shuffle);
417 q0shuffle = _mm_shuffle_ps(q0shuffle, q0shuffle, _MM_SHUFFLE(2, 3, 0, 1));
418 res = fmadd(q1x, controlWZYX, res);
419
420 q1y = _mm_mul_ps(q1y, q0shuffle);
421 q0shuffle = _mm_shuffle_ps(q0shuffle, q0shuffle, _MM_SHUFFLE(0, 1, 2, 3));
422 q1y = _mm_mul_ps(q1y, controlZWXY);
423
424 q1z = _mm_mul_ps(q1z, q0shuffle);
425
426 q1y = fmadd(q1z, controlYXWZ, q1y);
427 return _mm_add_ps(res, q1y);
428}
429
430//------------------------------------------------------------------------------
433__forceinline quat
435{
436 return quat(_mm_div_ps(q.vec, _mm_sqrt_ps(_mm_dp_ps(q.vec, q.vec, 0xff))));
437}
438
439//------------------------------------------------------------------------------
443__forceinline quat
445{
446 n_assert2(Math::nearequal(length(axis), 1.0f, 0.001f), "axis needs to be normalized");
447
448 float sinangle = Math::sin(0.5f * angle);
449 float cosangle = Math::cos(0.5f * angle);
450
451 // set w component to 1
452 __m128 b = _mm_and_ps(axis.vec, _mask_xyz);
453 b = _mm_or_ps(b, _id_w);
454 return _mm_mul_ps(b, _mm_set_ps(cosangle, sinangle, sinangle, sinangle));
455}
456
457//------------------------------------------------------------------------------
462__forceinline quat
463slerp(const quat& q1, const quat& q2, scalar t)
464{
465 __m128 to;
466
467 float qdot = dot(q1, q2);
468 // flip when negative angle
469 if (qdot < 0)
470 {
471 qdot = -qdot;
472 to = _mm_mul_ps(q2.vec, _mm_set_ps1(-1.0f));
473 }
474 else
475 {
476 to = q2.vec;
477 }
478
479 // just lerp when angle between is narrow
480 if (qdot < 0.95f)
481 {
482 //dont break acos
483 float clamped = Math::clamp(qdot, -1.0f, 1.0f);
484 float angle = Math::acos(clamped);
485
486 float sin_angle = Math::sin(angle);
487 float sin_angle_t = Math::sin(angle * t);
488 float sin_omega_t = Math::sin(angle * (1.0f - t));
489
490 __m128 s0 = _mm_set_ps1(sin_omega_t);
491 __m128 s1 = _mm_set_ps1(sin_angle_t);
492 __m128 sin_div = _mm_set_ps1(1.0f / sin_angle);
493 return _mm_mul_ps(_mm_add_ps(_mm_mul_ps(q1.vec, s0), _mm_mul_ps(to, s1)), sin_div);
494 }
495 else
496 {
497
498 float scale0 = 1.0f - t;
499 float scale1 = t;
500 __m128 s0 = _mm_set_ps1(scale0);
501 __m128 s1 = _mm_set_ps1(scale1);
502
503 return _mm_add_ps(_mm_mul_ps(q1.vec, s0), _mm_mul_ps(to, s1));
504 }
505}
506
507//------------------------------------------------------------------------------
510__forceinline void
511squadsetup(const quat& q0, const quat& q1, const quat& q2, const quat& q3, quat& aOut, quat& bOut, quat& cOut)
512{
513 n_error("FIXME: not implemented");
514
515 // not sure what this is useful for or what it exactly does.
516 //XMquatSquadSetup(&aOut.vec, &bOut.vec, &cOut.vec, q0.vec, q1.vec, q2.vec, q3.vec);
517}
518
519//------------------------------------------------------------------------------
522__forceinline quat
523squad(const quat& q1, const quat& a, const quat& b, const quat& c, scalar t)
524{
525 return slerp(slerp(q1, c, t), slerp(a, b, t), 2.0f * t * (1.0f - t));
526}
527
528//------------------------------------------------------------------------------
531__forceinline void
532to_axisangle(const quat& q, vec4& outAxis, scalar& outAngle)
533{
534 outAxis = q.vec;
535 outAxis.w = 0;
536 outAngle = 2.0f * Math::acos(q.w);
537 outAxis.w = 0.0f;
538}
539
540//------------------------------------------------------------------------------
546__forceinline vec3
547rotate(quat const& q, vec3 const& v)
548{
549 vec3 const i = q.vec; // xyz values of q
550 vec3 const qxv = cross(i, v);
551 vec3 const rv = v * q.w;
552 vec3 const rot = cross(i, qxv + rv) * 2.0f;
553 return v + rot;
554}
555
556} // namespace Math
557//------------------------------------------------------------------------------
void __cdecl n_error(const char *msg,...)
This function is called when a serious situation is encountered which requires abortion of the applic...
Definition debug.cc:138
#define n_assert2(exp, msg)
Definition debug.h:51
Different curves.
Definition angularpfeedbackloop.h:17
__forceinline vec3 cross(const vec3 &v0, const vec3 &v1)
Definition vec3.h:417
vec3 to_euler(const quat &q)
Definition quat.cc:84
__forceinline quat barycentric(const quat &q0, const quat &q1, const quat &q2, scalar f, scalar g)
Definition quat.h:295
__forceinline float undenormalize(scalar s)
Returns 0 if scalar is denormal.
Definition scalar.h:644
quat rotationmatrix(const mat4 &m)
Definition quat.cc:17
__forceinline scalar length3(const vec4 &v)
Definition vec4.h:395
__forceinline quat conjugate(const quat &q)
Definition quat.h:315
__forceinline scalar angle(const vec3 &v0, const vec3 &v1)
Definition vec3.h:508
__forceinline quat identity()
Definition quat.h:352
quat slerp(const quat &q1, const quat &q2, scalar t)
quat slerp TODO: rewrite using sse/avx
Definition quat.h:463
__forceinline scalar length(const quat &q)
Definition quat.h:259
__forceinline scalar lengthsq(const quat &q)
Definition quat.h:268
__forceinline scalar dot(const plane &p, const vec4 &v1)
Definition plane.h:246
__forceinline plane normalize(const plane &p)
Definition plane.h:255
__forceinline float clamp(float val, float minVal, float maxVal)
Float clamping.
Definition scalar.h:487
__forceinline quat quatExp(const quat &q)
Definition quat.h:335
__forceinline __m128 mul(__m128 a, __m128 b)
Definition sse.h:64
static const __m128 _id_w
Definition vec3.h:32
__forceinline void to_axisangle(const quat &q, vec4 &outAxis, scalar &outAngle)
Definition quat.h:532
__forceinline bool isidentity(const mat4 &m)
Definition mat4.h:372
float scalar
Definition scalar.h:45
__forceinline void squadsetup(const quat &q0, const quat &q1, const quat &q2, const quat &q3, quat &aOut, quat &bOut, quat &cOut)
Definition quat.h:511
half operator*(half one, half two)
Definition half.h:123
quat quatyawpitchroll(scalar y, scalar x, scalar z)
Definition quat.cc:63
__forceinline quat squad(const quat &q1, const quat &a, const quat &b, const quat &c, scalar t)
Definition quat.h:523
__forceinline scalar cos(scalar x)
Definition scalar.h:191
__forceinline scalar acos(scalar x)
Definition scalar.h:218
__forceinline quat quatUndenormalize(const quat &q)
Definition quat.h:277
__forceinline bool nearequal(const point &v0, const point &v1, float epsilon)
Definition point.h:485
static const __m128 _mask_xyz
Definition vec3.h:37
__forceinline scalar sin(scalar x)
Definition scalar.h:182
__forceinline quat ln(const quat &q)
Definition quat.h:378
vec3 rotate(quat const &q, vec3 const &v)
Rotate a vector by a quaternion.
Definition quat.h:547
__forceinline quat rotationquataxis(const vec3 &axis, scalar angle)
quat from rotation axis + angle.
Definition quat.h:444
quat from_euler(const vec3 &v)
Definition quat.cc:100
__forceinline __m128 fmadd(__m128 a, __m128 b, __m128 c)
Fused multiply-add operation, (a * b) + c.
Definition sse.h:22
__forceinline mat4 inverse(const mat4 &m)
Definition mat4.h:429
Nebula's scalar datatype.
SSE support functions.
A 4x4 single point precision float matrix.
Definition mat4.h:49
A quaternion is usually used to represent an orientation in 3D space.
Definition quat.h:30
float y
Definition quat.h:78
quat(quat const &)=default
default copy constructor
vec3 y_axis() const
get the y axis of the cartesian coordinate system that this quaternion represents
Definition quat.h:211
quat()
default constructor
Definition quat.h:88
void stream(scalar *ptr) const
stream content to 16-byte-aligned memory circumventing the write-cache
Definition quat.h:192
void operator=(const __m128 &rhs)
assign __m128
Definition quat.h:125
bool operator==(const quat &rhs) const
equality operator
Definition quat.h:134
void load(const scalar *ptr)
load content from 16-byte-aligned memory
Definition quat.h:153
bool operator!=(const quat &rhs) const
inequality operator
Definition quat.h:143
void storeu(scalar *ptr) const
write content to unaligned memory through the write cache
Definition quat.h:183
void loadu(const scalar *ptr)
load content from unaligned memory
Definition quat.h:163
void store(scalar *ptr) const
write content to 16-byte-aligned memory through the write cache
Definition quat.h:173
__m128 vec
Definition quat.h:80
void set(scalar x, scalar y, scalar z, scalar w)
set content
Definition quat.h:231
float z
Definition quat.h:78
float w
Definition quat.h:78
float x
Definition quat.h:78
vec3 x_axis() const
get the x axis of the cartesian coordinate system that this quaternion represents
Definition quat.h:201
vec3 z_axis() const
get the z axis of the cartesian coordinate system that this quaternion represents
Definition quat.h:221
A 3D vector.
Definition vec3.h:40
__m128 vec
Definition vec3.h:96
A 4D vector.
Definition vec4.h:24
__m128 vec
Definition vec4.h:95
float w
Definition vec4.h:93
bool operator==(const TiXmlString &a, const TiXmlString &b)
Definition tinystr.h:272
bool operator!=(const TiXmlString &a, const TiXmlString &b)
Definition tinystr.h:282
#define NEBULA_ALIGN16
Definition types.h:154