NW4F Sys
Loading...
Searching...
No Matches
math_Quaternion.h
Go to the documentation of this file.
1#ifndef NW_MATH_QUATERNION_H_
2#define NW_MATH_QUATERNION_H_
3
4#include <cstring>
5#include <nw/math/math_Config.h>
6
7#if defined(NW_PLATFORM_CAFE)
8 #include <cafe/mtx/mtxVec.h>
9#endif
10
11
12namespace nw { namespace math {
13
14struct QUAT;
15
16namespace internal { namespace standard {
17
18 QUAT* QUATAdd(QUAT* pOut, const QUAT* q1, const QUAT* q2);
19 QUAT* QUATSub(QUAT *pOut, const QUAT *q1, const QUAT *q2);
20 QUAT* QUATMult(QUAT* pOut, const QUAT* q1, const QUAT* q2);
21 f32 QUATDot(const QUAT* q1, const QUAT* q2);
22 QUAT* QUATScale(QUAT* pOut, const QUAT* q, f32 scale);
23 QUAT* QUATNormalize(QUAT* pOut, const QUAT* q);
24 QUAT* QUATInverse(QUAT* pOut, const QUAT* q);
25 QUAT* QUATExp(QUAT* pOut, const QUAT* q);
26 QUAT* QUATLogN(QUAT* pOut, const QUAT* q);
27 QUAT* QUATLerp(QUAT* pOut, const QUAT* q1, const QUAT* q2, f32 t);
28 QUAT* QUATSlerp(QUAT* pOut, const QUAT* q1, const QUAT* q2, f32 t);
29 QUAT* QUATMakeClosest(QUAT* pOut, const QUAT *q, const QUAT *qto);
30 VEC3* VEC3CalcRPY(VEC3* pOut, const QUAT *q);
31
32} } // namespace internal::standard
33
34#if defined(NW_MATH_ENABLE_INTRINSICS)
35
36namespace internal { namespace intrinsics {
37
38 QUAT* QUATAdd(QUAT* pOut, const QUAT* q1, const QUAT* q2);
39 QUAT* QUATSub(QUAT *pOut, const QUAT *q1, const QUAT *q2);
40 QUAT* QUATMult(QUAT* pOut, const QUAT* q1, const QUAT* q2);
41 f32 QUATDot(const QUAT* q1, const QUAT* q2);
42 QUAT* QUATScale(QUAT* pOut, const QUAT* q, f32 scale);
43 QUAT* QUATNormalize(QUAT* pOut, const QUAT* q);
44 QUAT* QUATInverse(QUAT* pOut, const QUAT* q);
45 QUAT* QUATExp(QUAT* pOut, const QUAT* q);
46 QUAT* QUATLogN(QUAT* pOut, const QUAT* q);
47 QUAT* QUATLerp(QUAT* pOut, const QUAT* q1, const QUAT* q2, f32 t);
48 QUAT* QUATSlerp(QUAT* pOut, const QUAT* q1, const QUAT* q2, f32 t);
49 QUAT* QUATMakeClosest(QUAT* pOut, const QUAT *q, const QUAT *qto);
50 VEC3* VEC3CalcRPY(VEC3* pOut, const QUAT *q);
51
52} } // namespace internal::intrinsics
53
54#endif // NW_MATH_ENABLE_INTRINSICS
55
56NW_MATH_INLINE QUAT* QUATAdd(QUAT* pOut, const QUAT* q1, const QUAT* q2);
57NW_MATH_INLINE QUAT* QUATSub(QUAT* pOut, const QUAT* q1, const QUAT* q2);
58NW_MATH_INLINE QUAT* QUATDivide(QUAT* pOut, const QUAT* q1, const QUAT* q2);
59NW_MATH_INLINE QUAT* QUATMult(QUAT* pOut, const QUAT* q1, const QUAT* q2);
60NW_MATH_INLINE f32 QUATDot(const QUAT* q1, const QUAT* q2);
61NW_MATH_INLINE QUAT* QUATInverse(QUAT* pOut, const QUAT* q);
62NW_MATH_INLINE QUAT* QUATScale(QUAT* pOut, const QUAT* q, f32 scale);
63NW_MATH_INLINE QUAT* QUATNormalize(QUAT* pOut, const QUAT* q);
64NW_MATH_INLINE QUAT* QUATExp(QUAT* pOut, const QUAT* q);
65NW_MATH_INLINE QUAT* QUATLogN(QUAT* pOut, const QUAT* q);
66NW_MATH_INLINE QUAT* QUATLerp(QUAT* pOut, const QUAT* q1, const QUAT* q2, f32 t);
67NW_MATH_INLINE QUAT* QUATSlerp(QUAT* pOut, const QUAT* q1, const QUAT* q2, f32 t);
68NW_MATH_INLINE QUAT* MTX34ToQUAT(QUAT* pOut, const MTX34* pMtx);
69NW_MATH_INLINE QUAT* QUATSquad(QUAT* pOut, const QUAT* p, const QUAT* a, const QUAT* b, const QUAT* q, f32 t);
70
71NW_MATH_INLINE QUAT* QUATMakeClosest( QUAT* pOut, const QUAT *q, const QUAT *qto );
72NW_MATH_INLINE QUAT* QUATRotAxisRad( QUAT* pOut, const VEC3 *axis, f32 rad );
73NW_MATH_INLINE QUAT* QUATMakeVectorRotation( QUAT *pOut, const VEC3* pFrom, const VEC3* pTo );
74
75struct QUAT_
76{
77 f32 x;
78 f32 y;
79 f32 z;
80 f32 w;
81};
82
83struct QUAT : public QUAT_
84{
85public:
86 typedef QUAT self_type;
87 typedef f32 value_type;
88public:
89 QUAT() {}
90 explicit QUAT(const f32* p) { x = p[0]; y = p[1]; z = p[2]; w = p[3]; }
91 explicit QUAT(const QUAT_& rhs) { x = rhs.x; y = rhs.y; z = rhs.z; w = rhs.w; }
92 QUAT(f32 fx, f32 fy, f32 fz, f32 fw) { x = fx; y = fy; z = fz; w = fw; }
93
94 operator f32*() { return &x; }
95 operator const f32*() const { return &x; }
96
97 f32* ToF32() { return &x; }
98 const f32* ToF32() const { return &x; }
99
100 template <typename ToPtr>
101 ToPtr Cast()
102 {
103 return reinterpret_cast<ToPtr>( this );
104 }
105
106 template <typename ToPtr>
107 ToPtr Cast() const
108 {
109 return reinterpret_cast<ToPtr>( this );
110 }
111
112 self_type& operator += (const self_type& rhs) { (void)QUATAdd(this, this, &rhs); return *this; }
113 self_type& operator -= (const self_type& rhs) { (void)QUATSub(this, this, &rhs); return *this; }
114 self_type& operator *= (f32 f) { (void)QUATScale(this, this, f); return *this; }
115 self_type& operator /= (f32 f) { return operator*=(1.f / f); }
116
117 self_type operator + () const { return *this; }
118 self_type operator - () const { return self_type(-x, -y, -z, -w); }
119
120 self_type operator + (const self_type& rhs) const { QUAT tmp; (void)QUATAdd(&tmp, this, &rhs); return tmp; }
121 self_type operator - (const self_type& rhs) const { QUAT tmp; (void)QUATSub(&tmp, this, &rhs); return tmp; }
122 self_type operator * (f32 f) const { QUAT tmp; (void)QUATScale(&tmp, this, f); return tmp; }
123 self_type operator / (f32 f) const { return operator*(1.f / f); }
124
125#pragma clang diagnostic push
126#pragma clang diagnostic ignored "-Wfloat-equal"
127
128 bool operator == (const self_type& rhs) const { return x == rhs.x && y == rhs.y && z == rhs.z && w == rhs.w; }
129 bool operator != (const self_type& rhs) const { return x != rhs.x || y != rhs.y || z != rhs.z || w != rhs.w; }
130
131#pragma clang diagnostic pop
132};
133
134typedef struct QUAT Quaternion;
135
136inline QUAT
137operator * (f32 f, const QUAT& rhs) { QUAT tmp; (void)QUATScale(&tmp, &rhs, f); return tmp; }
138
139} } // namespace nw::math
140
141
142#if defined(NW_MATH_AS_INLINE)
143
144#include <cmath>
145
146namespace nw { namespace math {
147
148#define NW_QUAT_EPSILON 0.00001F
149
150namespace internal { namespace standard {
151
152NW_MATH_INLINE QUAT*
153QUATAdd(QUAT* pOut, const QUAT* q1, const QUAT* q2)
154{
155 pOut->x = q1->x + q2->x;
156 pOut->y = q1->y + q2->y;
157 pOut->z = q1->z + q2->z;
158 pOut->w = q1->w + q2->w;
159
160 return pOut;
161}
162
163NW_MATH_INLINE QUAT*
164QUATSub(QUAT *pOut, const QUAT *q1, const QUAT *q2)
165{
166 pOut->x = q1->x - q2->x;
167 pOut->y = q1->y - q2->y;
168 pOut->z = q1->z - q2->z;
169 pOut->w = q1->w - q2->w;
170
171 return pOut;
172}
173
174NW_MATH_INLINE QUAT*
175QUATMult(QUAT* pOut, const QUAT* __restrict q1, const QUAT* __restrict q2)
176{
177 QUAT * __restrict pDst;
178 QUAT tmp;
179
180 if ( q1 == pOut || q2 == pOut )
181 {
182 pDst = &tmp;
183 }
184 else
185 {
186 pDst = pOut;
187 }
188
189 pDst->w = q1->w * q2->w - q1->x * q2->x - q1->y * q2->y - q1->z * q2->z;
190 pDst->x = q1->w * q2->x + q1->x * q2->w + q1->y * q2->z - q1->z * q2->y;
191 pDst->y = q1->w * q2->y + q1->y * q2->w + q1->z * q2->x - q1->x * q2->z;
192 pDst->z = q1->w * q2->z + q1->z * q2->w + q1->x * q2->y - q1->y * q2->x;
193
194 if ( pDst == &tmp )
195 {
196 *pOut = tmp;
197 }
198
199 return pOut;
200}
201
202NW_MATH_INLINE f32
203QUATDot(const QUAT* q1, const QUAT* q2)
204{
205 return (q1->x * q2->x + q1->y * q2->y + q1->z * q2->z + q1->w * q2->w);
206}
207
208NW_MATH_INLINE QUAT*
209QUATScale(QUAT* pOut, const QUAT* q, f32 scale)
210{
211 pOut->x = q->x * scale;
212 pOut->y = q->y * scale;
213 pOut->z = q->z * scale;
214 pOut->w = q->w * scale;
215
216 return pOut;
217}
218
219NW_MATH_INLINE QUAT*
220QUATNormalize(QUAT* pOut, const QUAT* __restrict q)
221{
222 f32 mag = (q->x * q->x) + (q->y * q->y) + (q->z * q->z) + (q->w * q->w);
223
224 if ( mag >= NW_QUAT_EPSILON )
225 {
226 mag = 1.0F / ::std::sqrt(mag);
227
228 pOut->x = q->x * mag;
229 pOut->y = q->y * mag;
230 pOut->z = q->z * mag;
231 pOut->w = q->w * mag;
232 }
233 else
234 {
235 pOut->x = pOut->y = pOut->z = pOut->w = 0.0F;
236 }
237
238 return pOut;
239}
240
241NW_MATH_INLINE QUAT*
242QUATInverse(QUAT* pOut, const QUAT* __restrict q)
243{
244 f32 mag, norminv;
245
246 mag = ( q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w );
247
248 if ( mag == 0.0f )
249 {
250 mag = 1.0f;
251 }
252
253 // [Inverse] = [Conjugate] / [Magnitude]
254 norminv = 1.0f / mag;
255 pOut->x = -q->x * norminv;
256 pOut->y = -q->y * norminv;
257 pOut->z = -q->z * norminv;
258 pOut->w = q->w * norminv;
259
260 return pOut;
261}
262
263NW_MATH_INLINE QUAT*
264QUATExp(QUAT* pOut, const QUAT* __restrict q)
265{
266 f32 theta, scale;
267
268 theta = ::std::sqrt( q->x * q->x + q->y * q->y + q->z * q->z );
269 scale = 1.0F;
270
271 if ( theta > NW_QUAT_EPSILON )
272 {
273 scale = (f32)::std::sin(theta) / theta;
274 }
275
276 pOut->x = scale * q->x;
277 pOut->y = scale * q->y;
278 pOut->z = scale * q->z;
279 pOut->w = (f32)::std::cos(theta);
280
281 return pOut;
282}
283
284NW_MATH_INLINE QUAT*
285QUATLogN(QUAT* pOut, const QUAT* __restrict q)
286{
287 f32 theta, scale;
288
289 scale = q->x * q->x + q->y * q->y + q->z * q->z;
290
291 scale = ::std::sqrt(scale);
292 theta = ::std::atan2( scale, q->w );
293
294 if ( scale > 0.0F )
295 {
296 scale = theta / scale;
297 }
298
299 pOut->x = scale * q->x;
300 pOut->y = scale * q->y;
301 pOut->z = scale * q->z;
302 pOut->w = 0.0F;
303
304 return pOut;
305}
306
307NW_MATH_INLINE QUAT*
308QUATLerp(QUAT* pOut, const QUAT* __restrict q1, const QUAT* __restrict q2, f32 t)
309{
310 pOut->x = t * ( q2->x - q1->x ) + q1->x;
311 pOut->y = t * ( q2->y - q1->y ) + q1->y;
312 pOut->z = t * ( q2->z - q1->z ) + q1->z;
313 pOut->w = t * ( q2->w - q1->w ) + q1->w;
314
315 return pOut;
316}
317
318NW_MATH_INLINE QUAT*
319QUATSlerp(QUAT* pOut, const QUAT* __restrict q1, const QUAT* __restrict q2, f32 t)
320{
321 f32 theta, sin_th, cos_th, tp, tq;
322
323 cos_th = q1->x * q2->x + q1->y * q2->y + q1->z * q2->z + q1->w * q2->w;
324 tq = 1.0F;
325
326 if ( cos_th < 0.0F )
327 {
328 cos_th = -cos_th;
329 tq = -tq;
330 }
331
332 if ( cos_th <= 1.0F - NW_QUAT_EPSILON )
333 {
334 theta = ::std::acos(cos_th);
335 sin_th = ::std::sin(theta);
336 tp = ::std::sin((1.0F - t) * theta) / sin_th;
337 tq *= ::std::sin( t * theta ) / sin_th;
338 }
339 else
340 {
341 // cos(theta) is close to 1.0F -> linear
342 tp = 1.0F - t;
343 tq = tq * t;
344 }
345
346 pOut->x = tp * q1->x + tq * q2->x;
347 pOut->y = tp * q1->y + tq * q2->y;
348 pOut->z = tp * q1->z + tq * q2->z;
349 pOut->w = tp * q1->w + tq * q2->w;
350
351 return pOut;
352}
353
354NW_MATH_INLINE QUAT*
355QUATMakeClosest(QUAT* pOut, const QUAT *q, const QUAT *qto)
356{
357 f32 dot;
358
359 dot = q->x * qto->x + q->y * qto->y + q->z * qto->z + q->w * qto->w;
360
361 if ( dot < 0.0f )
362 {
363 pOut->x = -q->x;
364 pOut->y = -q->y;
365 pOut->z = -q->z;
366 pOut->w = -q->w;
367 }
368 else
369 {
370 *pOut = *q;
371 }
372
373 return pOut;
374}
375
376// NW_MATH_INLINE VEC3*
377// VEC3CalcRPY(VEC3* pOut, const QUAT *q)
378// {
379// f32 q00 = q->w * q->w;
380// f32 q11 = q->x * q->x;
381// f32 q22 = q->y * q->y;
382// f32 q33 = q->z * q->z;
383//
384// f32 r11 = q00 + q11 - q22 - q33;
385// f32 r21 = 2.0f * (q->x * q->y + q->w * q->z);
386// f32 r31 = 2.0f * (q->x * q->z - q->w * q->y);
387// f32 r32 = 2.0f * (q->y * q->z + q->w * q->x);
388// f32 r33 = q00 - q11 - q22 + q33;
389//
390// f32 tmp = math::FAbs(r31);
391// if (1.0f - tmp < math::F_ULP)
392// {
393// f32 r12 = 2.0f * (q->x * q->y - q->w * q->z);
394// f32 r13 = 2.0f * (q->x * q->z + q->w * q->y);
395//
396// pOut->x = 0.f;
397// pOut->y = -math::F_PI / 2.0f * (r31 / tmp);
398// pOut->z = math::Atan2Rad(-r12, -r31 * r13);
399// }
400// else
401// {
402// pOut->x = math::Atan2Rad(r32, r33);
403// pOut->y = math::AsinRad(-r31);
404// pOut->z = math::Atan2Rad(r21, r11);
405// }
406//
407// return pOut;
408// }
409
410} } // namespace internal::standard
411
412#if defined(NW_MATH_ENABLE_INTRINSICS)
413
414namespace internal { namespace intrinsics {
415
416NW_MATH_INLINE QUAT*
417QUATAdd(QUAT* pOut, const QUAT* q1, const QUAT* q2)
418{
419 tof32x2(pOut->x) = __PS_ADD(tof32x2(q1->x), tof32x2(q2->x));
420 tof32x2(pOut->z) = __PS_ADD(tof32x2(q1->z), tof32x2(q2->z));
421
422 return pOut;
423}
424
425NW_MATH_INLINE QUAT*
426QUATSub(QUAT *pOut, const QUAT *q1, const QUAT *q2)
427{
428 tof32x2(pOut->x) = __PS_SUB(tof32x2(q1->x), tof32x2(q2->x));
429 tof32x2(pOut->z) = __PS_SUB(tof32x2(q1->z), tof32x2(q2->z));
430
431 return pOut;
432}
433
434NW_MATH_INLINE QUAT*
435QUATMult(QUAT* pOut, const QUAT* q1, const QUAT* q2)
436{
437 // o = a * b
438 //
439 // ox = + aw * bx + ax * bw + ay * bz - az * by;
440 // oy = + aw * by + ay * bw + az * bx - ax * bz;
441 // oz = + aw * bz + az * bw + ax * by - ay * bx;
442 // ow = + aw * bw - ax * bx - ay * by - az * bz;
443 //
444 // x y, z w のペアになるように項の順序を並び替える。
445 //
446 // ox = ( + aw * bx + ay * bz ) - az * by + ax * bw;
447 // oy = ( + az * bx - ax * bz ) + aw * by + ay * bw;
448 // oz = ( - ay * bx + aw * bz ) + az * bw - ( - ax * by );
449 // ow = ( - ax * bx - az * bz ) + aw * bw - ( + ay * by );
450 //
451 // 前2項は要素の順が逆なので演算後に入れ替える。
452
453 const f32x2 axay = tof32x2(q1->x); // { ax ay }
454 const f32x2 azaw = tof32x2(q1->z); // { az aw }
455 const f32x2 bxby = tof32x2(q2->x); // { bx by }
456 const f32x2 bzbw = tof32x2(q2->z); // { bz bw }
457
458 const f32x2 AxAy = __PS_NEG(axay);
459 const f32x2 Axay = __PS_MERGE01(AxAy, axay);
460 const f32x2 AzAw = __PS_NEG(azaw);
461 const f32x2 Azaw = __PS_MERGE01(AzAw, azaw);
462
463 f32x2 oxy, ozw;
464
465 oxy = __PS_MULS0(azaw, bxby);
466 oxy = __PS_MADDS0(Axay, bzbw, oxy);
467 oxy = __PS_MERGE10(oxy, oxy); // 要素順を入れ替え。
468 oxy = __PS_MADDS1(Azaw, bxby, oxy);
469 oxy = __PS_MADDS1(axay, bzbw, oxy);
470
471 ozw = __PS_MULS0(AxAy, bxby);
472 ozw = __PS_MADDS0(Azaw, bzbw, ozw);
473 ozw = __PS_MERGE10(ozw, ozw); // 要素順を入れ替え。
474 ozw = __PS_MADDS1(azaw, bzbw, ozw);
475 ozw = __PS_SUB(ozw, __PS_MULS1(Axay, bxby));
476
477 tof32x2(pOut->x) = oxy;
478 tof32x2(pOut->z) = ozw;
479
480 return pOut;
481}
482
483NW_MATH_INLINE f32
484QUATDot(const QUAT* q1, const QUAT* q2)
485{
486 // o = a . b
487
488 const f32x2 axay = tof32x2(q1->x);
489 const f32x2 azaw = tof32x2(q1->z);
490 const f32x2 bxby = tof32x2(q2->x);
491 const f32x2 bzbw = tof32x2(q2->z);
492
493 f32x2 f0;
494
495 f0 = __PS_MUL(axay, bxby);
496 f0 = __PS_MADD(azaw, bzbw, f0);
497 f0 = __PS_SUM0(f0, f0, f0);
498
499 return f0[0];
500}
501
502NW_MATH_INLINE QUAT*
503QUATScale(QUAT* pOut, const QUAT* q, f32 scale)
504{
505 tof32x2(pOut->x) = __PS_MULS0F(tof32x2(q->x), scale);
506 tof32x2(pOut->z) = __PS_MULS0F(tof32x2(q->z), scale);
507
508 return pOut;
509}
510
511NW_MATH_INLINE QUAT*
512QUATNormalize(QUAT* pOut, const QUAT* q)
513{
514 f32x2 xy = tof32x2(q->x);
515 f32x2 zw = tof32x2(q->z);
516
517 f32x2 f0 = __PS_MADD(xy, xy, __PS_MUL(zw, zw));
518 f0 = __PS_SUM0(f0, f0, f0);
519
520 f32 mag = f0[0];
521
522 if ( mag >= NW_QUAT_EPSILON )
523 {
524 mag = 1.0F / ::std::sqrt(mag);
525
526 tof32x2(pOut->x) = __PS_MULS0F(xy, mag);
527 tof32x2(pOut->z) = __PS_MULS0F(zw, mag);
528 }
529 else
530 {
531 f32x2 zero = __PS_SUB(f0, f0);
532 tof32x2(pOut->x) = zero;
533 tof32x2(pOut->z) = zero;
534 }
535
536 return pOut;
537}
538
539NW_MATH_INLINE QUAT*
540QUATInverse(QUAT* pOut, const QUAT* __restrict q)
541{
542 f32x2 xy = tof32x2(q->x);
543 f32x2 zw = tof32x2(q->z);
544
545 f32x2 f0 = __PS_MADD(xy, xy, __PS_MUL(zw, zw));
546 f0 = __PS_SUM0(f0, f0, f0);
547
548 f32 mag = f0[0];
549 f32 zero = xy[0] - xy[0];
550 if ( mag == zero )
551 {
552 mag = 1.0f;
553 }
554
555 // [Inverse] = [Conjugate] / [Magnitude]
556 f32x2 szw = __PS_FDUP(1.0f / mag);
557 f32x2 sxy = __PS_NEG(szw);
558 szw[0] = sxy[0];
559
560 tof32x2(pOut->x) = __PS_MUL(xy, sxy);
561 tof32x2(pOut->z) = __PS_MUL(zw, szw);
562
563 return pOut;
564}
565
566NW_MATH_INLINE QUAT*
567QUATLerp(QUAT* pOut, const QUAT* __restrict q1, const QUAT* __restrict q2, f32 t)
568{
569 f32x2 tt = __PS_FDUP(t);
570 f32x2 xy = tof32x2(q1->x);
571 f32x2 zw = tof32x2(q1->z);
572
573 tof32x2(pOut->x) = __PS_MADD(tof32x2(q2->x), tt, __PS_NMSUB(xy, tt, xy));
574 tof32x2(pOut->z) = __PS_MADD(tof32x2(q2->z), tt, __PS_NMSUB(zw, tt, zw));
575
576 return pOut;
577}
578
579NW_MATH_INLINE QUAT*
580QUATSlerp(QUAT* pOut, const QUAT* __restrict q1, const QUAT* __restrict q2, f32 t)
581{
582 f32 theta, sin_th, cos_th;
583 f32x2 tp, tq;
584
585 f32x2 xy1 = tof32x2(q1->x);
586 f32x2 zw1 = tof32x2(q1->z);
587 f32x2 xy2 = tof32x2(q2->x);
588 f32x2 zw2 = tof32x2(q2->z);
589
590 f32x2 f0 = __PS_MADD(xy1, xy2, __PS_MUL(zw1, zw2));
591 f0 = __PS_SUM0(f0, f0, f0);
592
593 cos_th = f0[0];
594 tq = __PS_FDUP(1.0F);
595
596 f32 zero = xy1[0] - xy1[0];
597 if ( cos_th < zero )
598 {
599 cos_th = -cos_th;
600 tq = __PS_NEG(tq);
601 }
602
603 if ( cos_th <= 1.0F - NW_QUAT_EPSILON )
604 {
605 theta = ::std::acos(cos_th);
606 sin_th = ::std::sin(theta);
607 tp = __PS_FDUP(::std::sin((1.0F - t) * theta) / sin_th);
608 tq = __PS_MULS0F(tq, ::std::sin( t * theta ) / sin_th);
609 }
610 else
611 {
612 // cos(theta) is close to 1.0F -> linear
613 tp = __PS_FDUP(1.0F - t);
614 tq = __PS_MULS0F(tq, t);
615 }
616
617 tof32x2(pOut->x) = __PS_MADD(tp, xy1, __PS_MUL(tq, xy2));
618 tof32x2(pOut->z) = __PS_MADD(tp, zw1, __PS_MUL(tq, zw2));
619
620 return pOut;
621}
622
623NW_MATH_INLINE QUAT*
624QUATMakeClosest(QUAT* pOut, const QUAT *q, const QUAT *qto)
625{
626 f32x2 xy1 = tof32x2(q->x);
627 f32x2 zw1 = tof32x2(q->z);
628 f32x2 xy2 = tof32x2(qto->x);
629 f32x2 zw2 = tof32x2(qto->z);
630
631 f32x2 f0 = __PS_MADD(xy1, xy2, __PS_MUL(zw1, zw2));
632 f0 = __PS_SUM0(f0, f0, f0);
633
634 f32 dot = f0[0];
635
636 f32 zero = xy1[0] - xy1[0];
637 if ( dot < zero )
638 {
639 xy1 = __PS_NEG(xy1);
640 zw1 = __PS_NEG(zw1);
641 }
642
643 tof32x2(pOut->x) = xy1;
644 tof32x2(pOut->z) = zw1;
645
646 return pOut;
647}
648
649} } // namespace internal::intrinsics
650
651#endif // NW_MATH_ENABLE_INTRINSICS
652
653NW_MATH_INLINE QUAT*
654QUATAdd(QUAT* pOut, const QUAT* q1, const QUAT* q2)
655{
656 return NW_MATH_IMPL_NS::QUATAdd(pOut, q1, q2);
657}
658
659NW_MATH_INLINE QUAT*
660QUATSub(QUAT *pOut, const QUAT *q1, const QUAT *q2)
661{
662 return NW_MATH_IMPL_NS::QUATSub(pOut, q1, q2);
663}
664
665NW_MATH_INLINE QUAT*
666QUATDivide(QUAT* pOut, const QUAT* q1, const QUAT* q2)
667{
668 QUAT qtmp;
669
670 QUATInverse(&qtmp, q2);
671 QUATMult(pOut, &qtmp, q1);
672
673 return pOut;
674}
675
676NW_MATH_INLINE QUAT*
677QUATMult(QUAT* pOut, const QUAT* __restrict q1, const QUAT* __restrict q2)
678{
679 return NW_MATH_IMPL_NS::QUATMult(pOut, q1, q2);
680}
681
682NW_MATH_INLINE f32
683QUATDot(const QUAT* q1, const QUAT* q2)
684{
685 return NW_MATH_IMPL_NS::QUATDot(q1, q2);
686}
687
688NW_MATH_INLINE QUAT*
689QUATScale(QUAT* pOut, const QUAT* q, f32 scale)
690{
691 return NW_MATH_IMPL_NS::QUATScale(pOut, q, scale);
692}
693
694NW_MATH_INLINE QUAT*
695QUATNormalize(QUAT* pOut, const QUAT* __restrict q)
696{
697 return NW_MATH_IMPL_NS::QUATNormalize(pOut, q);
698}
699
700NW_MATH_INLINE QUAT*
701QUATInverse(QUAT* pOut, const QUAT* __restrict q)
702{
703 return NW_MATH_IMPL_NS::QUATInverse(pOut, q);
704}
705
706NW_MATH_INLINE QUAT*
707QUATExp(QUAT* pOut, const QUAT* __restrict q)
708{
709 return internal::standard::QUATExp(pOut, q);
710}
711
712NW_MATH_INLINE QUAT*
713QUATLogN(QUAT* pOut, const QUAT* __restrict q)
714{
715 return internal::standard::QUATLogN(pOut, q);
716}
717
718NW_MATH_INLINE QUAT*
719QUATLerp(QUAT* pOut, const QUAT* __restrict q1, const QUAT* __restrict q2, f32 t)
720{
721 return NW_MATH_IMPL_NS::QUATLerp(pOut, q1, q2, t);
722}
723
724NW_MATH_INLINE QUAT*
725QUATSlerp(QUAT* pOut, const QUAT* __restrict q1, const QUAT* __restrict q2, f32 t)
726{
727 return NW_MATH_IMPL_NS::QUATSlerp(pOut, q1, q2, t);
728}
729
730NW_MATH_INLINE QUAT*
731QUATSquad(QUAT* pOut, const QUAT* p, const QUAT* a, const QUAT* b, const QUAT* q, f32 t)
732{
733 QUAT pq, ab;
734 f32 t2;
735
736 t2 = 2 * t * ( 1.0F - t );
737 QUATSlerp(&pq, p, q, t);
738 QUATSlerp(&ab, a, b, t);
739 QUATSlerp(pOut, &pq, &ab, t2);
740
741 return pOut;
742}
743
744NW_MATH_INLINE QUAT*
745QUATMakeClosest(QUAT* pOut, const QUAT *q, const QUAT *qto)
746{
747 return NW_MATH_IMPL_NS::QUATMakeClosest(pOut, q, qto);
748}
749
750NW_MATH_INLINE QUAT*
751QUATRotAxisRad( QUAT *pOut, const VEC3 *axis, f32 rad )
752{
753 f32 half, sh, ch;
754 VEC3 nAxis;
755
756 VEC3Normalize(&nAxis, axis);
757
758 half = rad * 0.50F;
759 sh = ::std::sin(half);
760 ch = ::std::cos(half);
761
762 pOut->x = sh * nAxis.x;
763 pOut->y = sh * nAxis.y;
764 pOut->z = sh * nAxis.z;
765 pOut->w = ch;
766
767 return pOut;
768}
769
770NW_MATH_INLINE QUAT*
771QUATMakeVectorRotation( QUAT *pOut, const VEC3* pFrom, const VEC3* pTo )
772{
773 VEC3 c;
774 c.SetCross( *pFrom, *pTo );
775 f32 dPlus1 = VEC3Dot( pFrom, pTo ) + 1.f;
776
777 if ( dPlus1 <= nw::math::F_ULP )
778 {
779 pOut->x = 1.f;
780 pOut->y = 0.f;
781 pOut->z = 0.f;
782 pOut->w = 0.f;
783
784 return pOut;
785 }
786
787 f32 s = FSqrt( dPlus1 * 2.f );
788 f32 oos = 1.f / s;
789
790 pOut->x = c.x * oos;
791 pOut->y = c.y * oos;
792 pOut->z = c.z * oos;
793 pOut->w = s * 0.5f;
794
795 return pOut;
796}
797
798NW_MATH_INLINE VEC3*
799VEC3CalcRPY(VEC3* pOut, const QUAT *q)
800{
801 return internal::standard::VEC3CalcRPY(pOut, q);
802}
803
804} } // namespace nw::math
805
806#endif
807
808namespace nw { namespace math {
809
810inline QUAT* QUATAdd(QUAT* pOut, const QUAT& q1, const QUAT& q2) { return QUATAdd( pOut, &q1, &q2 ); }
811inline QUAT* QUATSub(QUAT* pOut, const QUAT& q1, const QUAT& q2) { return QUATSub( pOut, &q1, &q2 ); }
812inline QUAT* QUATDivide(QUAT* pOut, const QUAT& q1, const QUAT& q2) { return QUATDivide( pOut, &q1, &q2 ); }
813inline QUAT* QUATMult(QUAT* pOut, const QUAT& q1, const QUAT& q2) { return QUATMult( pOut, &q1, &q2 ); }
814inline f32 QUATDot(const QUAT& q1, const QUAT& q2) { return QUATDot( &q1, &q2 ); }
815inline QUAT* QUATInverse(QUAT* pOut, const QUAT& q) { return QUATInverse( pOut, &q ); }
816inline QUAT* QUATScale(QUAT* pOut, const QUAT& q, f32 scale) { return QUATScale( pOut, &q, scale ); }
817inline QUAT* QUATNormalize(QUAT* pOut, const QUAT& q) { return QUATNormalize( pOut, &q ); }
818inline QUAT* QUATExp(QUAT* pOut, const QUAT& q) { return QUATExp( pOut, &q ); }
819inline QUAT* QUATLogN(QUAT* pOut, const QUAT& q) { return QUATLogN( pOut, &q ); }
820inline QUAT* QUATLerp(QUAT* pOut, const QUAT& q1, const QUAT& q2, f32 t) { return QUATLerp( pOut, &q1, &q2, t ); }
821inline QUAT* QUATSlerp(QUAT* pOut, const QUAT& q1, const QUAT& q2, f32 t) { return QUATSlerp( pOut, &q1, &q2, t ); }
822inline QUAT* MTX34ToQUAT(QUAT* pOut, const MTX34& mtx) { return MTX34ToQUAT( pOut, &mtx ); }
823inline QUAT* QUATSquad(QUAT* pOut, const QUAT& p, const QUAT& a, const QUAT& b, const QUAT& q, f32 t) { return QUATSquad( pOut, &p, &a, &b, &q, t ); }
824
825inline QUAT* QUATMakeClosest( QUAT* pOut, const QUAT& q, const QUAT& qto ) { return QUATMakeClosest( pOut, &q, &qto ); }
826inline QUAT* QUATRotAxisRad( QUAT* pOut, const VEC3& axis, f32 rad ) { return QUATRotAxisRad( pOut, &axis, rad ); }
827inline QUAT* QUATMakeVectorRotation( QUAT *pOut, const VEC3& from, const VEC3& to ) { return QUATMakeVectorRotation( pOut, &from, &to ); }
828
829} } // namespace nw::math
830
831#endif
NW_MATH_AS_INLINE
#define NW_MATH_AS_INLINE
Definition math_Config.h:6
NW_MATH_INLINE
#define NW_MATH_INLINE
Definition math_Config.h:7
NW_MATH_IMPL_NS
#define NW_MATH_IMPL_NS
Definition math_Config.h:14
nw::math::internal::standard
Definition math_Matrix34.h:11
nw::math::internal::standard::QUATSlerp
QUAT * QUATSlerp(QUAT *pOut, const QUAT *q1, const QUAT *q2, f32 t)
nw::math::internal::standard::QUATAdd
QUAT * QUATAdd(QUAT *pOut, const QUAT *q1, const QUAT *q2)
nw::math::internal::standard::QUATMakeClosest
QUAT * QUATMakeClosest(QUAT *pOut, const QUAT *q, const QUAT *qto)
nw::math::internal::standard::QUATDot
f32 QUATDot(const QUAT *q1, const QUAT *q2)
nw::math::internal::standard::QUATLerp
QUAT * QUATLerp(QUAT *pOut, const QUAT *q1, const QUAT *q2, f32 t)
nw::math::internal::standard::QUATLogN
QUAT * QUATLogN(QUAT *pOut, const QUAT *q)
nw::math::internal::standard::QUATSub
QUAT * QUATSub(QUAT *pOut, const QUAT *q1, const QUAT *q2)
nw::math::internal::standard::QUATExp
QUAT * QUATExp(QUAT *pOut, const QUAT *q)
nw::math::internal::standard::VEC3CalcRPY
VEC3 * VEC3CalcRPY(VEC3 *pOut, const QUAT *q)
nw::math::internal::standard::QUATScale
QUAT * QUATScale(QUAT *pOut, const QUAT *q, f32 scale)
nw::math::internal::standard::QUATInverse
QUAT * QUATInverse(QUAT *pOut, const QUAT *q)
nw::math::internal::standard::QUATNormalize
QUAT * QUATNormalize(QUAT *pOut, const QUAT *q)
nw::math::internal::standard::QUATMult
QUAT * QUATMult(QUAT *pOut, const QUAT *q1, const QUAT *q2)
nw::math::internal
Definition math_Triangular.cpp:3
nw::math
Definition math_Constant.cpp:5
nw::math::Quaternion
struct QUAT Quaternion
Definition math_Quaternion.h:134
nw::math::QUATInverse
NW_MATH_INLINE QUAT * QUATInverse(QUAT *pOut, const QUAT *q)
nw::math::QUATLogN
NW_MATH_INLINE QUAT * QUATLogN(QUAT *pOut, const QUAT *q)
nw::math::QUATInverse
QUAT * QUATInverse(QUAT *pOut, const QUAT &q)
Definition math_Quaternion.h:815
nw::math::QUATSlerp
NW_MATH_INLINE QUAT * QUATSlerp(QUAT *pOut, const QUAT *q1, const QUAT *q2, f32 t)
nw::math::QUATMakeClosest
NW_MATH_INLINE QUAT * QUATMakeClosest(QUAT *pOut, const QUAT *q, const QUAT *qto)
nw::math::QUATScale
QUAT * QUATScale(QUAT *pOut, const QUAT &q, f32 scale)
Definition math_Quaternion.h:816
nw::math::QUATLerp
NW_MATH_INLINE QUAT * QUATLerp(QUAT *pOut, const QUAT *q1, const QUAT *q2, f32 t)
nw::math::QUATMakeVectorRotation
QUAT * QUATMakeVectorRotation(QUAT *pOut, const VEC3 &from, const VEC3 &to)
Definition math_Quaternion.h:827
nw::math::QUATLogN
QUAT * QUATLogN(QUAT *pOut, const QUAT &q)
Definition math_Quaternion.h:819
nw::math::QUATExp
QUAT * QUATExp(QUAT *pOut, const QUAT &q)
Definition math_Quaternion.h:818
nw::math::QUATNormalize
QUAT * QUATNormalize(QUAT *pOut, const QUAT &q)
Definition math_Quaternion.h:817
nw::math::QUATMult
NW_MATH_INLINE QUAT * QUATMult(QUAT *pOut, const QUAT *q1, const QUAT *q2)
nw::math::MTX34ToQUAT
QUAT * MTX34ToQUAT(QUAT *pOut, const MTX34 &mtx)
Definition math_Quaternion.h:822
nw::math::QUATSlerp
QUAT * QUATSlerp(QUAT *pOut, const QUAT &q1, const QUAT &q2, f32 t)
Definition math_Quaternion.h:821
nw::math::QUATSquad
QUAT * QUATSquad(QUAT *pOut, const QUAT &p, const QUAT &a, const QUAT &b, const QUAT &q, f32 t)
Definition math_Quaternion.h:823
nw::math::QUATAdd
NW_MATH_INLINE QUAT * QUATAdd(QUAT *pOut, const QUAT *q1, const QUAT *q2)
nw::math::QUATDivide
QUAT * QUATDivide(QUAT *pOut, const QUAT &q1, const QUAT &q2)
Definition math_Quaternion.h:812
nw::math::QUATDivide
NW_MATH_INLINE QUAT * QUATDivide(QUAT *pOut, const QUAT *q1, const QUAT *q2)
nw::math::QUATLerp
QUAT * QUATLerp(QUAT *pOut, const QUAT &q1, const QUAT &q2, f32 t)
Definition math_Quaternion.h:820
nw::math::MTX34ToQUAT
NW_MATH_INLINE QUAT * MTX34ToQUAT(QUAT *pOut, const MTX34 *pMtx)
nw::math::QUATDot
f32 QUATDot(const QUAT &q1, const QUAT &q2)
Definition math_Quaternion.h:814
nw::math::QUATMakeClosest
QUAT * QUATMakeClosest(QUAT *pOut, const QUAT &q, const QUAT &qto)
Definition math_Quaternion.h:825
nw::math::QUATAdd
QUAT * QUATAdd(QUAT *pOut, const QUAT &q1, const QUAT &q2)
Definition math_Quaternion.h:810
nw::math::QUATDot
NW_MATH_INLINE f32 QUATDot(const QUAT *q1, const QUAT *q2)
nw::math::QUATScale
NW_MATH_INLINE QUAT * QUATScale(QUAT *pOut, const QUAT *q, f32 scale)
nw::math::QUATSub
NW_MATH_INLINE QUAT * QUATSub(QUAT *pOut, const QUAT *q1, const QUAT *q2)
nw::math::QUATSub
QUAT * QUATSub(QUAT *pOut, const QUAT &q1, const QUAT &q2)
Definition math_Quaternion.h:811
nw::math::QUATMakeVectorRotation
NW_MATH_INLINE QUAT * QUATMakeVectorRotation(QUAT *pOut, const VEC3 *pFrom, const VEC3 *pTo)
nw::math::QUATRotAxisRad
NW_MATH_INLINE QUAT * QUATRotAxisRad(QUAT *pOut, const VEC3 *axis, f32 rad)
nw::math::QUATExp
NW_MATH_INLINE QUAT * QUATExp(QUAT *pOut, const QUAT *q)
nw::math::QUATRotAxisRad
QUAT * QUATRotAxisRad(QUAT *pOut, const VEC3 &axis, f32 rad)
Definition math_Quaternion.h:826
nw::math::operator*
QUAT operator*(f32 f, const QUAT &rhs)
Definition math_Quaternion.h:137
nw::math::QUATNormalize
NW_MATH_INLINE QUAT * QUATNormalize(QUAT *pOut, const QUAT *q)
nw::math::QUATSquad
NW_MATH_INLINE QUAT * QUATSquad(QUAT *pOut, const QUAT *p, const QUAT *a, const QUAT *b, const QUAT *q, f32 t)
nw::math::QUATMult
QUAT * QUATMult(QUAT *pOut, const QUAT &q1, const QUAT &q2)
Definition math_Quaternion.h:813
nw
Definition math_Constant.cpp:5
nw::math::QUAT_
Definition math_Quaternion.h:76
nw::math::QUAT_::z
f32 z
Definition math_Quaternion.h:79
nw::math::QUAT_::y
f32 y
Definition math_Quaternion.h:78
nw::math::QUAT_::x
f32 x
Definition math_Quaternion.h:77
nw::math::QUAT_::w
f32 w
Definition math_Quaternion.h:80
nw::math::QUAT
Definition math_Quaternion.h:84
nw::math::QUAT::value_type
f32 value_type
Definition math_Quaternion.h:87
nw::math::QUAT::QUAT
QUAT(const QUAT_ &rhs)
Definition math_Quaternion.h:91
nw::math::QUAT::QUAT
QUAT(const f32 *p)
Definition math_Quaternion.h:90
nw::math::QUAT::self_type
QUAT self_type
Definition math_Quaternion.h:86
nw::math::QUAT::QUAT
QUAT()
Definition math_Quaternion.h:89
nw::math::QUAT::QUAT
QUAT(f32 fx, f32 fy, f32 fz, f32 fw)
Definition math_Quaternion.h:92
NW_PLATFORM_CAFE
#define NW_PLATFORM_CAFE
Definition types.h:8