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:
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 float v[4];
81 __m128 vec;
82 };
83};
84
85//------------------------------------------------------------------------------
88__forceinline
90{
91 this->vec = _mm_setr_ps(0, 0, 0, 1);
92}
93
94//------------------------------------------------------------------------------
97__forceinline
99{
100 this->vec = _mm_setr_ps(x, y, z, w);
101}
102
103//------------------------------------------------------------------------------
106__forceinline
107quat::quat(const vec4& rhs) :
108 vec(rhs.vec)
109{
110 // empty
111}
112
113//------------------------------------------------------------------------------
116__forceinline
117quat::quat(const __m128& rhs)
118{
119 this->vec = rhs;
120}
121
122//------------------------------------------------------------------------------
125__forceinline void
126quat::operator=(const __m128& rhs)
127{
128 this->vec = rhs;
129}
130
131//------------------------------------------------------------------------------
134__forceinline bool
135quat::operator==(const quat& rhs) const
136{
137 return _mm_movemask_ps(_mm_cmpeq_ps(this->vec, rhs.vec)) == 0x0f;
138}
139
140//------------------------------------------------------------------------------
143__forceinline bool
144quat::operator!=(const quat& rhs) const
145{
146 return _mm_movemask_ps(_mm_cmpeq_ps(this->vec, rhs.vec)) != 0x0f;
147}
148
149//------------------------------------------------------------------------------
153__forceinline void
155{
156 this->vec = _mm_load_ps(ptr);
157}
158
159//------------------------------------------------------------------------------
163__forceinline void
165{
166 this->vec = _mm_loadu_ps(ptr);
167}
168
169//------------------------------------------------------------------------------
173__forceinline void
175{
176 _mm_store_ps(ptr, this->vec);
177}
178
179//------------------------------------------------------------------------------
183__forceinline void
185{
186 _mm_storeu_ps(ptr, this->vec);
187}
188
189//------------------------------------------------------------------------------
192__forceinline void
194{
195 this->store(ptr);
196}
197
198//------------------------------------------------------------------------------
201__forceinline vec3
203{
204 vec3 const v = vec3(1, 0, 0);
205 return rotate(*this, v);
206}
207
208//------------------------------------------------------------------------------
211__forceinline vec3
213{
214 vec3 const v = vec3(0, 1, 0);
215 return rotate(*this, v);
216}
217
218//------------------------------------------------------------------------------
221__forceinline vec3
223{
224 vec3 const v = vec3(0, 0, 1);
225 return rotate(*this, v);
226}
227
228//------------------------------------------------------------------------------
231__forceinline void
233{
234 this->vec = _mm_setr_ps(x, y, z, w);
235}
236
237//------------------------------------------------------------------------------
240__forceinline void
241quat::set(const vec4& f4)
242{
243 this->vec = f4.vec;
244}
245
246//------------------------------------------------------------------------------
249__forceinline bool
251{
252 const quat id(0, 0, 0, 1);
253 return id == q;
254}
255
256//------------------------------------------------------------------------------
259__forceinline scalar
260length(const quat& q)
261{
262 return _mm_cvtss_f32(_mm_sqrt_ss(_mm_dp_ps(q.vec, q.vec, 0xF1)));
263}
264
265//------------------------------------------------------------------------------
268__forceinline scalar
269lengthsq(const quat& q)
270{
271 return _mm_cvtss_f32(_mm_dp_ps(q.vec, q.vec, 0xF1));
272}
273
274//------------------------------------------------------------------------------
277__forceinline quat
279{
280 // nothing to do on the xbox, since denormal numbers are not supported by the vmx unit,
281 // it is being set to zero anyway
282 quat ret;
283#if __WIN32__
284 ret.x = Math::undenormalize(q.x);
285 ret.y = Math::undenormalize(q.y);
286 ret.z = Math::undenormalize(q.z);
287 ret.w = Math::undenormalize(q.w);
288#endif
289 return ret;
290}
291
292//------------------------------------------------------------------------------
295__forceinline quat
296barycentric(const quat& q0, const quat& q1, const quat& q2, scalar f, scalar g)
297{
298 scalar s = f + g;
299 if (s != 0.0f)
300 {
301 quat a = slerp(q0, q1, s);
302 quat b = slerp(q0, q2, s);
303 quat res = slerp(a, b, g / s);
304 return res;
305 }
306 else
307 {
308 return q0;
309 }
310}
311
312//------------------------------------------------------------------------------
315__forceinline quat
317{
318 const __m128 con = { -1.0f, -1.0f, -1.0f, 1.0f };
319 quat qq(_mm_mul_ps(q.vec, con));
320 return qq;
321}
322
323//------------------------------------------------------------------------------
326__forceinline scalar
327dot(const quat& q0, const quat& q1)
328{
329 return _mm_cvtss_f32(_mm_dp_ps(q0.vec, q1.vec, 0xF1));
330}
331
332//------------------------------------------------------------------------------
335__forceinline quat
336quatExp(const quat& q)
337{
338 vec4 f(q.vec);
339 scalar theta = length3(f);
340 scalar costheta = Math::cos(theta);
341 scalar sintheta = Math::sin(theta);
342
343 f *= sintheta / theta;
344 f.w = costheta;
345
346 return quat(f.vec);
347}
348
349//------------------------------------------------------------------------------
352__forceinline quat
354{
355 return quat(_mm_setr_ps(0.0f, 0.0f, 0.0f, 1.0f));
356}
357
358//------------------------------------------------------------------------------
361__forceinline quat
362inverse(const quat& q)
363{
364 scalar len = lengthsq(q);
365 if (len > 0.00001f)
366 {
367 quat con = conjugate(q);
368 __m128 temp = _mm_set1_ps(1.0f / len);
369 con.vec = _mm_mul_ps(con.vec, temp);
370 return con;
371 }
372 return quat(0.0f, 0.0f, 0.0f, 0.0f);
373}
374
375//------------------------------------------------------------------------------
378__forceinline quat
379ln(const quat& q)
380{
381 quat ret;
382
383 scalar a = Math::acos(q.w);
384 scalar isina = 1.0f / Math::sin(a);
385
386 scalar aisina = a * isina;
387 if (isina > 0)
388 {
389 vec3 mul(aisina, aisina, aisina);
390 ret = vec4(vec3(q.vec) * mul, 0.0f);
391 }
392 else
393 {
394 ret.set(0.0f, 0.0f, 0.0f, 0.0f);
395 }
396 return ret;
397}
398
399//------------------------------------------------------------------------------
402__forceinline quat
403operator*(const quat& q0, const quat& q1)
404{
405 static const __m128 controlWZYX = _mm_setr_ps(1, -1, 1, -1);
406 static const __m128 controlZWXY = _mm_setr_ps(1, 1, -1, -1);
407 static const __m128 controlYXWZ = _mm_setr_ps(-1, 1, 1, -1);
408 __m128 res = _mm_shuffle_ps(q1.vec, q1.vec, _MM_SHUFFLE(3, 3, 3, 3));
409 __m128 q1x = _mm_shuffle_ps(q1.vec, q1.vec, _MM_SHUFFLE(0, 0, 0, 0));
410 __m128 q1y = _mm_shuffle_ps(q1.vec, q1.vec, _MM_SHUFFLE(1, 1, 1, 1));
411 __m128 q1z = _mm_shuffle_ps(q1.vec, q1.vec, _MM_SHUFFLE(2, 2, 2, 2));
412
413 res = _mm_mul_ps(res, q0.vec);
414 __m128 q0shuffle = q0.vec;
415 q0shuffle = _mm_shuffle_ps(q0shuffle, q0shuffle, _MM_SHUFFLE(0, 1, 2, 3));
416
417 q1x = _mm_mul_ps(q1x, q0shuffle);
418 q0shuffle = _mm_shuffle_ps(q0shuffle, q0shuffle, _MM_SHUFFLE(2, 3, 0, 1));
419 res = fmadd(q1x, controlWZYX, res);
420
421 q1y = _mm_mul_ps(q1y, q0shuffle);
422 q0shuffle = _mm_shuffle_ps(q0shuffle, q0shuffle, _MM_SHUFFLE(0, 1, 2, 3));
423 q1y = _mm_mul_ps(q1y, controlZWXY);
424
425 q1z = _mm_mul_ps(q1z, q0shuffle);
426
427 q1y = fmadd(q1z, controlYXWZ, q1y);
428 return _mm_add_ps(res, q1y);
429}
430
431//------------------------------------------------------------------------------
434__forceinline quat
436{
437 return quat(_mm_div_ps(q.vec, _mm_sqrt_ps(_mm_dp_ps(q.vec, q.vec, 0xff))));
438}
439
440//------------------------------------------------------------------------------
444__forceinline quat
446{
447 n_assert2(Math::nearequal(length(axis), 1.0f, 0.001f), "axis needs to be normalized");
448
449 float sinangle = Math::sin(0.5f * angle);
450 float cosangle = Math::cos(0.5f * angle);
451
452 // set w component to 1
453 __m128 b = _mm_and_ps(axis.vec, _mask_xyz);
454 b = _mm_or_ps(b, _id_w);
455 return _mm_mul_ps(b, _mm_set_ps(cosangle, sinangle, sinangle, sinangle));
456}
457
458//------------------------------------------------------------------------------
463__forceinline quat
464slerp(const quat& q1, const quat& q2, scalar t)
465{
466 __m128 to;
467
468 float qdot = dot(q1, q2);
469 // flip when negative angle
470 if (qdot < 0)
471 {
472 qdot = -qdot;
473 to = _mm_mul_ps(q2.vec, _mm_set_ps1(-1.0f));
474 }
475 else
476 {
477 to = q2.vec;
478 }
479
480 // just lerp when angle between is narrow
481 if (qdot < 0.95f)
482 {
483 //dont break acos
484 float clamped = Math::clamp(qdot, -1.0f, 1.0f);
485 float angle = Math::acos(clamped);
486
487 float sin_angle = Math::sin(angle);
488 float sin_angle_t = Math::sin(angle * t);
489 float sin_omega_t = Math::sin(angle * (1.0f - t));
490
491 __m128 s0 = _mm_set_ps1(sin_omega_t);
492 __m128 s1 = _mm_set_ps1(sin_angle_t);
493 __m128 sin_div = _mm_set_ps1(1.0f / sin_angle);
494 return _mm_mul_ps(_mm_add_ps(_mm_mul_ps(q1.vec, s0), _mm_mul_ps(to, s1)), sin_div);
495 }
496 else
497 {
498
499 float scale0 = 1.0f - t;
500 float scale1 = t;
501 __m128 s0 = _mm_set_ps1(scale0);
502 __m128 s1 = _mm_set_ps1(scale1);
503
504 return _mm_add_ps(_mm_mul_ps(q1.vec, s0), _mm_mul_ps(to, s1));
505 }
506}
507
508//------------------------------------------------------------------------------
511__forceinline void
512squadsetup(const quat& q0, const quat& q1, const quat& q2, const quat& q3, quat& aOut, quat& bOut, quat& cOut)
513{
514 n_error("FIXME: not implemented");
515
516 // not sure what this is useful for or what it exactly does.
517 //XMquatSquadSetup(&aOut.vec, &bOut.vec, &cOut.vec, q0.vec, q1.vec, q2.vec, q3.vec);
518}
519
520//------------------------------------------------------------------------------
523__forceinline quat
524squad(const quat& q1, const quat& a, const quat& b, const quat& c, scalar t)
525{
526 return slerp(slerp(q1, c, t), slerp(a, b, t), 2.0f * t * (1.0f - t));
527}
528
529//------------------------------------------------------------------------------
532__forceinline void
533to_axisangle(const quat& q, vec4& outAxis, scalar& outAngle)
534{
535 outAxis = q.vec;
536 outAxis.w = 0;
537 outAngle = 2.0f * Math::acos(q.w);
538 outAxis.w = 0.0f;
539}
540
541//------------------------------------------------------------------------------
547__forceinline vec3
548rotate(quat const& q, vec3 const& v)
549{
550 vec3 const i = q.vec; // xyz values of q
551 vec3 const qxv = cross(i, v);
552 vec3 const rv = v * q.w;
553 vec3 const rot = cross(i, qxv + rv) * 2.0f;
554 return v + rot;
555}
556
557} // namespace Math
558//------------------------------------------------------------------------------
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:296
__forceinline float undenormalize(scalar s)
Returns 0 if scalar is denormal.
Definition scalar.h:653
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:316
__forceinline scalar angle(const vec3 &v0, const vec3 &v1)
Definition vec3.h:508
__forceinline quat identity()
Definition quat.h:353
quat slerp(const quat &q1, const quat &q2, scalar t)
quat slerp TODO: rewrite using sse/avx
Definition quat.h:464
__forceinline scalar length(const quat &q)
Definition quat.h:260
__forceinline scalar lengthsq(const quat &q)
Definition quat.h:269
__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:496
__forceinline quat quatExp(const quat &q)
Definition quat.h:336
__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:533
__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:512
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:524
__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:278
__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:379
vec3 rotate(quat const &q, vec3 const &v)
Rotate a vector by a quaternion.
Definition quat.h:548
__forceinline quat rotationquataxis(const vec3 &axis, scalar angle)
quat from rotation axis + angle.
Definition quat.h:445
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.
quat()
default constructor
Definition quat.h:89
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:212
quat()
default constructor
Definition quat.h:89
void stream(scalar *ptr) const
stream content to 16-byte-aligned memory circumventing the write-cache
Definition quat.h:193
void operator=(const __m128 &rhs)
assign __m128
Definition quat.h:126
bool operator==(const quat &rhs) const
equality operator
Definition quat.h:135
void load(const scalar *ptr)
load content from 16-byte-aligned memory
Definition quat.h:154
float v[4]
Definition quat.h:80
bool operator!=(const quat &rhs) const
inequality operator
Definition quat.h:144
void storeu(scalar *ptr) const
write content to unaligned memory through the write cache
Definition quat.h:184
void loadu(const scalar *ptr)
load content from unaligned memory
Definition quat.h:164
void store(scalar *ptr) const
write content to 16-byte-aligned memory through the write cache
Definition quat.h:174
friend struct mat4
Definition quat.h:73
__m128 vec
Definition quat.h:81
void set(scalar x, scalar y, scalar z, scalar w)
set content
Definition quat.h:232
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:202
vec3 z_axis() const
get the z axis of the cartesian coordinate system that this quaternion represents
Definition quat.h:222
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