NW4F Sys
Loading...
Searching...
No Matches
math_Types.h
Go to the documentation of this file.
1#ifndef NW_MATH_TYPES_H_
2#define NW_MATH_TYPES_H_
3
4#include <nw/types.h>
5//#include <nw/ut/ut_TypeTraits.h>
6#include <nw/math/math_Config.h>
7#include <nw/math/math_Triangular.h>
8#include <nw/math/math_Vector2.h>
9#include <nw/math/math_Vector3.h>
10#include <nw/math/math_Vector4.h>
11#include <nw/math/math_Quaternion.h>
12//#include <nw/math/math_Matrix22.h>
13//#include <nw/math/math_Matrix23.h>
14//#include <nw/math/math_Matrix33.h>
15#include <nw/math/math_Matrix34.h>
16#include <nw/math/math_Matrix44.h>
17//#include <nw/math/math_Matrix43.h>
18#include <nw/math/math_Transform.h>
19
20#include <cstring>
21
22namespace nw { namespace math {
23
24struct VEC2;
25struct VEC3;
26struct VEC4;
27struct QUAT;
28
29namespace internal { namespace standard {
30
31 VEC3* VEC3TransformArray(VEC3* pOut, const MTX34* pM, const VEC3* pV, s32 count);
32 VEC4* VEC3TransformArray(VEC4* pOut, const MTX44* pM, const VEC3* pV, u32 count);
33 VEC3* VEC3TransformNormal(VEC3* pOut, const MTX34* pM, const VEC3* pV);
34 VEC3* VEC3TransformNormalArray(VEC3* pOut, const MTX34* pM, const VEC3* pV, u32 count);
35 VEC3* VEC3TransformCoord(VEC3* pOut, const MTX44* pM, const VEC3* pV);
36 VEC3* VEC3TransformCoordArray(VEC3* pOut, const MTX44* pM, const VEC3* pV, u32 count);
37 VEC4* VEC4Transform(VEC4* pOut, const MTX44* pM, const VEC4* pV);
38 VEC4* VEC4TransformArray(VEC4* pOut, const MTX44* pM, const VEC4* pV, u32 count);
39 VEC4* VEC4Transform(VEC4* pOut, const MTX34* pM, const VEC4* pV);
40 VEC4* VEC4TransformArray(VEC4* pOut, const MTX34* pM, const VEC4* pV, u32 count);
41
42} } // namespace internal::standard
43
44#if defined(NW_MATH_ENABLE_INTRINSICS)
45
46namespace internal { namespace intrinsics {
47
48 VEC3* VEC3TransformArray(VEC3* pOut, const MTX34* pM, const VEC3* pV, s32 count);
49 VEC4* VEC3TransformArray(VEC4* pOut, const MTX44* pM, const VEC3* pV, u32 count);
50 VEC3* VEC3TransformNormal(VEC3* pOut, const MTX34* pM, const VEC3* pV);
51 VEC3* VEC3TransformNormalArray(VEC3* pOut, const MTX34* pM, const VEC3* pV, u32 count);
52 VEC3* VEC3TransformCoord(VEC3* pOut, const MTX44* pM, const VEC3* pV);
53 VEC3* VEC3TransformCoordArray(VEC3* pOut, const MTX44* pM, const VEC3* pV, u32 count);
54 VEC4* VEC4Transform(VEC4* pOut, const MTX44* pM, const VEC4* pV);
55 VEC4* VEC4TransformArray(VEC4* pOut, const MTX44* pM, const VEC4* pV, u32 count);
56 VEC4* VEC4Transform(VEC4* pOut, const MTX34* pM, const VEC4* pV);
57 VEC4* VEC4TransformArray(VEC4* pOut, const MTX34* pM, const VEC4* pV, u32 count);
58
59} } // namespace internal::intrinsics
60
61#endif // NW_MATH_ENABLE_INTRINSICS
62
63} } // namespace nw::math
64
65#if defined(NW_MATH_AS_INLINE)
66
67namespace nw { namespace math {
68
69namespace internal { namespace standard {
70
71NW_MATH_INLINE VEC3*
72VEC3TransformArray(VEC3* pOut, const MTX34* __restrict pM, const VEC3* __restrict pV, s32 count)
73{
74 VEC3 vTmp;
75 VEC3* __restrict pOutBase = pOut;
76 const f32 (*const m)[4] = pM->m;
77
78 for (s32 i = 0; i < count; ++i)
79 {
80 // Vec has a 4th implicit 'w' coordinate of 1
81 vTmp.x = m[0][0] * pV->x + m[0][1] * pV->y + m[0][2] * pV->z + m[0][3];
82 vTmp.y = m[1][0] * pV->x + m[1][1] * pV->y + m[1][2] * pV->z + m[1][3];
83 vTmp.z = m[2][0] * pV->x + m[2][1] * pV->y + m[2][2] * pV->z + m[2][3];
84
85 // copy back
86 pOut->x = vTmp.x;
87 pOut->y = vTmp.y;
88 pOut->z = vTmp.z;
89
90 pV++;
91 pOut++;
92 }
93
94 return pOutBase;
95}
96
97NW_MATH_INLINE VEC4*
98VEC3TransformArray(VEC4* pOut, const MTX44* __restrict pM, const VEC3* __restrict pV, u32 count)
99{
100 VEC4* pDst = pOut;
101 for (u32 i = 0; i < count; ++i)
102 {
103 (void)standard::VEC3Transform(pDst, pM, pV);
104 ++pDst;
105 ++pV;
106 }
107
108 return pOut;
109}
110
111NW_MATH_INLINE VEC3*
112VEC3TransformNormal(VEC3* pOut, const MTX34* pM, const VEC3* pV)
113{
114 VEC3 tmp;
115 tmp.x = pM->f._00 * pV->x + pM->f._01 * pV->y + pM->f._02 * pV->z;
116 tmp.y = pM->f._10 * pV->x + pM->f._11 * pV->y + pM->f._12 * pV->z;
117 tmp.z = pM->f._20 * pV->x + pM->f._21 * pV->y + pM->f._22 * pV->z;
118
119 pOut->x = tmp.x;
120 pOut->y = tmp.y;
121 pOut->z = tmp.z;
122
123 return pOut;
124}
125
126NW_MATH_INLINE VEC3*
127VEC3TransformNormalArray(VEC3* pOut, const MTX34* pM, const VEC3* pV, u32 count)
128{
129 u32 i;
130 VEC3 tmp;
131 VEC3* pDst = pOut;
132
133 for (i = 0; i < count; i++)
134 {
135 // Vec has a 4th implicit 'w' coordinate of 1
136 tmp.x = pM->f._00 * pV->x + pM->f._01 * pV->y + pM->f._02 * pV->z;
137 tmp.y = pM->f._10 * pV->x + pM->f._11 * pV->y + pM->f._12 * pV->z;
138 tmp.z = pM->f._20 * pV->x + pM->f._21 * pV->y + pM->f._22 * pV->z;
139
140 // copy back
141 pDst->x = tmp.x;
142 pDst->y = tmp.y;
143 pDst->z = tmp.z;
144
145 pV++;
146 pDst++;
147 }
148
149 return pOut;
150}
151
152NW_MATH_INLINE VEC3*
153VEC3TransformCoord(VEC3* pOut, const MTX44* pM, const VEC3* pV)
154{
155 VEC3 tmp;
156 f32 rw;
157
158 tmp.x = pM->f._00 * pV->x + pM->f._01 * pV->y + pM->f._02 * pV->z + pM->f._03;
159 tmp.y = pM->f._10 * pV->x + pM->f._11 * pV->y + pM->f._12 * pV->z + pM->f._13;
160 tmp.z = pM->f._20 * pV->x + pM->f._21 * pV->y + pM->f._22 * pV->z + pM->f._23;
161 rw = pM->f._30 * pV->x + pM->f._31 * pV->y + pM->f._32 * pV->z + pM->f._33;
162 rw = 1.f / rw;
163
164 pOut->x = rw * tmp.x;
165 pOut->y = rw * tmp.y;
166 pOut->z = rw * tmp.z;
167
168 return pOut;
169}
170
171NW_MATH_INLINE VEC3*
172VEC3TransformCoordArray(VEC3* pOut, const MTX44* pM, const VEC3* pV, u32 count)
173{
174 VEC3* pDst = pOut;
175 for (u32 i = 0; i < count; ++i)
176 {
177 (void)standard::VEC3TransformCoord(pDst, pM, pV);
178 ++pDst;
179 ++pV;
180 }
181
182 return pOut;
183}
184
185NW_MATH_INLINE VEC4*
186VEC4Transform(VEC4* pOut, const MTX44* __restrict pM, const VEC4* __restrict pV)
187{
188 VEC4 tmp;
189 tmp.x = pM->f._00 * pV->x + pM->f._01 * pV->y + pM->f._02 * pV->z + pM->f._03 * pV->w;
190 tmp.y = pM->f._10 * pV->x + pM->f._11 * pV->y + pM->f._12 * pV->z + pM->f._13 * pV->w;
191 tmp.z = pM->f._20 * pV->x + pM->f._21 * pV->y + pM->f._22 * pV->z + pM->f._23 * pV->w;
192 tmp.w = pM->f._30 * pV->x + pM->f._31 * pV->y + pM->f._32 * pV->z + pM->f._33 * pV->w;
193
194 pOut->x = tmp.x;
195 pOut->y = tmp.y;
196 pOut->z = tmp.z;
197 pOut->w = tmp.w;
198
199 return pOut;
200}
201
202NW_MATH_INLINE VEC4*
203VEC4TransformArray(VEC4* pOut, const MTX44* pM, const VEC4* pV, u32 count)
204{
205 VEC4* pDst = pOut;
206 for (u32 i = 0; i < count; ++i)
207 {
208 (void)standard::VEC4Transform(pDst, pM, pV);
209 ++pDst;
210 ++pV;
211 }
212
213 return pOut;
214}
215
216NW_MATH_INLINE VEC4*
217VEC4Transform(VEC4* pOut, const MTX34* __restrict pM, const VEC4* __restrict pV)
218{
219 VEC4 tmp;
220 tmp.x = pM->f._00 * pV->x + pM->f._01 * pV->y + pM->f._02 * pV->z + pM->f._03 * pV->w;
221 tmp.y = pM->f._10 * pV->x + pM->f._11 * pV->y + pM->f._12 * pV->z + pM->f._13 * pV->w;
222 tmp.z = pM->f._20 * pV->x + pM->f._21 * pV->y + pM->f._22 * pV->z + pM->f._23 * pV->w;
223
224 pOut->x = tmp.x;
225 pOut->y = tmp.y;
226 pOut->z = tmp.z;
227 pOut->w = pV->w;
228
229 return pOut;
230}
231
232NW_MATH_INLINE VEC4*
233VEC4TransformArray(VEC4* pOut, const MTX34* pM, const VEC4* pV, u32 count)
234{
235 VEC4* pDst = pOut;
236 for (u32 i = 0; i < count; ++i)
237 {
238 (void)standard::VEC4Transform(pDst, pM, pV);
239 ++pDst;
240 ++pV;
241 }
242
243 return pOut;
244}
245
246} } // namespace internal::standard
247
248#if defined(NW_MATH_ENABLE_INTRINSICS)
249
250namespace internal { namespace intrinsics {
251
252NW_MATH_INLINE VEC3*
253VEC3TransformArray(VEC3* pOut, const MTX34* __restrict pM, const VEC3* __restrict pV, s32 count)
254{
255 f32x2 m00m01 = __PSQ_LX(pM, offsetof(MTX44, f._00), 0, 0);
256 f32x2 m10m11 = __PSQ_LX(pM, offsetof(MTX44, f._10), 0, 0);
257 f32x2 m02m03 = __PSQ_LX(pM, offsetof(MTX44, f._02), 0, 0);
258 f32x2 m12m13 = __PSQ_LX(pM, offsetof(MTX44, f._12), 0, 0);
259
260 f32x2 m00m10 = __PS_MERGE00(m00m01, m10m11);
261 f32x2 m01m11 = __PS_MERGE11(m00m01, m10m11);
262 f32x2 m02m12 = __PS_MERGE00(m02m03, m12m13);
263 f32x2 m03m13 = __PS_MERGE11(m02m03, m12m13);
264
265 f32x2 m20m30 = __PSQ_LX(pM, offsetof(MTX34, f._20), 1, 0);
266 f32x2 m21m31 = __PSQ_LX(pM, offsetof(MTX34, f._21), 1, 0);
267 f32x2 m22m32 = __PSQ_LX(pM, offsetof(MTX34, f._22), 1, 0);
268 f32x2 m23m33 = __PSQ_LX(pM, offsetof(MTX34, f._23), 1, 0);
269
270 VEC3* pDst = pOut;
271 for (s32 i = 0; i < count; ++i)
272 {
273 f32x2 fp0, fp1;
274
275 // { xy zw } = { x y z 1 }
276 f32x2 xy = __PSQ_LX(pV, offsetof(VEC3, x), 0, 0);
277 f32x2 zw = __PSQ_LX(pV, offsetof(VEC3, z), 1, 0);
278
279 // fp0 = [ m00 m01 m02 m03 ] . T[ x y z 1 ]
280 // [ m10 m11 m12 m03 ] .
281 fp0 = __PS_MADDS0(m02m12, zw, m03m13);
282 fp0 = __PS_MADDS1(m01m11, xy, fp0);
283 fp0 = __PS_MADDS0(m00m10, xy, fp0);
284
285 // fp1 = [ m20 m21 m22, m23 ] . T[ x y z 1 ]
286 // [ m30 m31 m32, m33 ]
287 fp1 = __PS_MADDS0(m22m32, zw, m23m33);
288 fp1 = __PS_MADDS1(m21m31, xy, fp1);
289 fp1 = __PS_MADDS0(m20m30, xy, fp1);
290
291 tof32x2(pDst->x) = fp0;
292 pDst->z = fp1[0];
293
294 pV++;
295 pDst++;
296 }
297
298 return pOut;
299}
300
301NW_MATH_INLINE VEC4*
302VEC3TransformArray(VEC4* pOut, const MTX44* __restrict pM, const VEC3* __restrict pV, u32 count)
303{
304 f32x2 m00m01 = __PSQ_LX(pM, offsetof(MTX44, f._00), 0, 0);
305 f32x2 m10m11 = __PSQ_LX(pM, offsetof(MTX44, f._10), 0, 0);
306 f32x2 m02m03 = __PSQ_LX(pM, offsetof(MTX44, f._02), 0, 0);
307 f32x2 m12m13 = __PSQ_LX(pM, offsetof(MTX44, f._12), 0, 0);
308
309 f32x2 m00m10 = __PS_MERGE00(m00m01, m10m11);
310 f32x2 m01m11 = __PS_MERGE11(m00m01, m10m11);
311 f32x2 m02m12 = __PS_MERGE00(m02m03, m12m13);
312 f32x2 m03m13 = __PS_MERGE11(m02m03, m12m13);
313
314 f32x2 m20m21 = __PSQ_LX(pM, offsetof(MTX44, f._20), 0, 0);
315 f32x2 m30m31 = __PSQ_LX(pM, offsetof(MTX44, f._30), 0, 0);
316 f32x2 m22m23 = __PSQ_LX(pM, offsetof(MTX44, f._22), 0, 0);
317 f32x2 m32m33 = __PSQ_LX(pM, offsetof(MTX44, f._32), 0, 0);
318
319 f32x2 m20m30 = __PS_MERGE00(m20m21, m30m31);
320 f32x2 m21m31 = __PS_MERGE11(m20m21, m30m31);
321 f32x2 m22m32 = __PS_MERGE00(m22m23, m32m33);
322 f32x2 m23m33 = __PS_MERGE11(m22m23, m32m33);
323
324 VEC4* pDst = pOut;
325 for (u32 i = 0; i < count; ++i)
326 {
327 f32x2 fp0, fp1;
328
329 // { xy zw } = { x y z 1 }
330 f32x2 xy = __PSQ_LX(pV, offsetof(VEC3, x), 0, 0);
331 f32x2 zw = __PSQ_LX(pV, offsetof(VEC3, z), 1, 0);
332
333 // fp0 = [ m00 m01 m02, m03 ] . T[ x y z 1 ]
334 // [ m10 m11 m12, m13 ]
335 fp0 = __PS_MADDS0(m02m12, zw, m03m13);
336 fp0 = __PS_MADDS1(m01m11, xy, fp0);
337 fp0 = __PS_MADDS0(m00m10, xy, fp0);
338
339 // fp1 = [ m20 m21 m22, m23 ] . T[ x y z 1 ]
340 // [ m30 m31 m32, m33 ]
341 fp1 = __PS_MADDS0(m22m32, zw, m23m33);
342 fp1 = __PS_MADDS1(m21m31, xy, fp1);
343 fp1 = __PS_MADDS0(m20m30, xy, fp1);
344
345 tof32x2(pDst->x) = fp0;
346 tof32x2(pDst->z) = fp1;
347
348 ++pDst;
349 ++pV;
350 }
351
352 return pOut;
353}
354
355NW_MATH_INLINE VEC3*
356VEC3TransformNormal(VEC3* pOut, const MTX34* __restrict pM, const VEC3* __restrict pV)
357{
358 f32x2 m00m01 = __PSQ_LX(pM, offsetof(MTX34, m[0][0]), 0, 0);
359 f32x2 m02m03 = __PSQ_LX(pM, offsetof(MTX34, m[0][2]), 1, 0);
360 f32x2 m10m11 = __PSQ_LX(pM, offsetof(MTX34, m[1][0]), 0, 0);
361 f32x2 m12m13 = __PSQ_LX(pM, offsetof(MTX34, m[1][2]), 1, 0);
362 f32x2 m20m21 = __PSQ_LX(pM, offsetof(MTX34, m[2][0]), 0, 0);
363 f32x2 m22m23 = __PSQ_LX(pM, offsetof(MTX34, m[2][2]), 1, 0);
364
365 f32x2 fp0, fp1, fp2;
366
367 // { xy zw } = { x y z 1 }
368 f32x2 xy = __PSQ_LX(pV, offsetof(VEC3, x), 0, 0);
369 f32x2 zw = __PSQ_LX(pV, offsetof(VEC3, z), 1, 0);
370
371 // fp0 = [ m00 m01 m02 0 ] . T[ x y z 1 ]
372 fp0 = __PS_MUL(m00m01, xy);
373 fp0 = __PS_SUM0(fp0, fp0, fp0);
374 fp0 = __PS_MADD(m02m03, zw, fp0);
375
376 pOut->x = fp0[0];
377
378 // fp1 = [ m10 m11 m12 0 ] . T[ x y z 1 ]
379 fp1 = __PS_MUL(m10m11, xy);
380 fp1 = __PS_SUM0(fp1, fp1, fp1);
381 fp1 = __PS_MADD(m12m13, zw, fp1);
382
383 pOut->y = fp1[0];
384
385 // fp2 = [ m20 m21 m22 0 ] . T[ x y z 1 ]
386 fp2 = __PS_MUL(m20m21, xy);
387 fp2 = __PS_SUM0(fp2, fp2, fp2);
388 fp2 = __PS_MADD(m22m23, zw, fp2);
389
390 pOut->z = fp2[0];
391
392 return pOut;
393}
394
395NW_MATH_INLINE VEC3*
396VEC3TransformNormalArray(VEC3* pOut, const MTX34* __restrict pM, const VEC3* __restrict pV, u32 count)
397{
398 f32x2 m00m01 = __PSQ_LX(pM, offsetof(MTX34, f._00), 0, 0);
399 f32x2 m10m11 = __PSQ_LX(pM, offsetof(MTX34, f._10), 0, 0);
400 f32x2 m00m10 = __PS_MERGE00(m00m01, m10m11);
401 f32x2 m01m11 = __PS_MERGE11(m00m01, m10m11);
402 f32x2 m02m12 = { pM->f._02, pM->f._12 };
403 f32x2 m20m21 = __PSQ_LX(pM, offsetof(MTX34, f._20), 0, 0);
404 f32x2 m22 = __PSQ_LX(pM, offsetof(MTX34, f._22), 1, 0);
405
406 VEC3* pDst = pOut;
407 for (s32 i = 0; i < count; ++i)
408 {
409 f32x2 fp0, fp1;
410
411 // { xy zw } = { x y z 1 }
412 f32x2 zw = __PSQ_LX(pV, offsetof(VEC3, z), 1, 0);
413 f32x2 xy = __PSQ_LX(pV, offsetof(VEC3, x), 0, 0);
414
415 // fp0 = [ m00 m01 m02 m03 ] . T[ x y z 0 ]
416 // [ m10 m11 m12 m13 ] .
417 fp0 = __PS_MULS0(m02m12, zw);
418 fp0 = __PS_MADDS1(m01m11, xy, fp0);
419 fp0 = __PS_MADDS0(m00m10, xy, fp0);
420
421 // fp1 = [ m20 m21 m22 m23 ] . T[ x y z 0 ]
422 fp1 = __PS_MUL(m20m21, xy);
423 fp1 = __PS_SUM0(fp1, fp1, fp1);
424 fp1 = __PS_MADDS0(m22, zw, fp1);
425
426 tof32x2(pDst->x) = fp0;
427 pDst->z = fp1[0];
428
429 pV++;
430 pDst++;
431 }
432
433 return pOut;
434}
435
436NW_MATH_INLINE VEC3*
437VEC3TransformCoord(VEC3* pOut, const MTX44* __restrict pM, const VEC3* __restrict pV)
438{
439 // { xy zw } = { x y z 1 }
440 const f32x2 xy = __PSQ_LX(pV, offsetof(VEC3, x), 0, 0);
441 const f32x2 zw = __PSQ_LX(pV, offsetof(VEC3, z), 1, 0);
442
443 f32x2 m30m31 = __PSQ_LX(pM, offsetof(MTX44, f._30), 0, 0);
444 f32x2 m32m33 = __PSQ_LX(pM, offsetof(MTX44, f._32), 0, 0);
445
446 f32x2 fp0, fp1, fp2, fp3;
447
448 // fp3 = [ m30 m31 m32, m33 ] . T[ x y z 1 ]
449 fp3 = __PS_MUL(m30m31, xy);
450 fp3 = __PS_MADD(m32m33, zw, fp3);
451 fp3 = __PS_SUM0(fp3, fp3, fp3);
452
453 // rw[1] = 1 / w
454 const f32x2 rw = __PS_DIV(zw, __PS_MERGE00(fp3, fp3));
455
456 f32x2 m00m01 = __PSQ_LX(pM, offsetof(MTX44, f._00), 0, 0);
457 f32x2 m02m03 = __PSQ_LX(pM, offsetof(MTX44, f._02), 0, 0);
458 f32x2 m10m11 = __PSQ_LX(pM, offsetof(MTX44, f._10), 0, 0);
459 f32x2 m12m13 = __PSQ_LX(pM, offsetof(MTX44, f._12), 0, 0);
460 f32x2 m20m21 = __PSQ_LX(pM, offsetof(MTX44, f._20), 0, 0);
461 f32x2 m22m23 = __PSQ_LX(pM, offsetof(MTX44, f._22), 0, 0);
462
463 // fp0 = [ m00 m01 m02, m03 ] . T[ x y z 1 ]
464 // [ m10 m11 m12, m13 ]
465 fp0 = __PS_MUL(m00m01, xy);
466 fp1 = __PS_MUL(m10m11, xy);
467 fp0 = __PS_MADD(m02m03, zw, fp0);
468 fp1 = __PS_MADD(m12m13, zw, fp1);
469 fp0 = __PS_SUM0(fp0, fp0, fp0);
470 fp0 = __PS_SUM1(fp1, fp0, fp1);
471 fp0 = __PS_MULS1(fp0, rw);
472
473 // fp2 = [ m20 m21 m22, m23 ] . T[ x y z 1 ]
474 fp2 = __PS_MUL(m20m21, xy);
475 fp2 = __PS_MADD(m22m23, zw, fp2);
476 fp2 = __PS_SUM0(fp2, fp2, fp2);
477 fp2 = __PS_MULS1(fp2, rw);
478
479 tof32x2(pOut->x) = fp0;
480 pOut->z = fp2[0];
481
482 return pOut;
483}
484
485NW_MATH_INLINE VEC3*
486VEC3TransformCoordArray(VEC3* pOut, const MTX44* pM, const VEC3* pV, u32 count)
487{
488 const f32x2 m00m01 = __PSQ_LX(pM, offsetof(MTX44, f._00), 0, 0);
489 const f32x2 m02m03 = __PSQ_LX(pM, offsetof(MTX44, f._02), 0, 0);
490 const f32x2 m10m11 = __PSQ_LX(pM, offsetof(MTX44, f._10), 0, 0);
491 const f32x2 m12m13 = __PSQ_LX(pM, offsetof(MTX44, f._12), 0, 0);
492 const f32x2 m20m21 = __PSQ_LX(pM, offsetof(MTX44, f._20), 0, 0);
493 const f32x2 m22m23 = __PSQ_LX(pM, offsetof(MTX44, f._22), 0, 0);
494 const f32x2 m30m31 = __PSQ_LX(pM, offsetof(MTX44, f._30), 0, 0);
495 const f32x2 m32m33 = __PSQ_LX(pM, offsetof(MTX44, f._32), 0, 0);
496
497 const f32x2 m00m10 = __PS_MERGE00(m00m01, m10m11);
498 const f32x2 m01m11 = __PS_MERGE11(m00m01, m10m11);
499 const f32x2 m02m12 = __PS_MERGE00(m02m03, m12m13);
500 const f32x2 m03m13 = __PS_MERGE11(m02m03, m12m13);
501 const f32x2 m20m30 = __PS_MERGE00(m20m21, m30m31);
502 const f32x2 m21m31 = __PS_MERGE11(m20m21, m30m31);
503 const f32x2 m22m32 = __PS_MERGE00(m22m23, m32m33);
504 const f32x2 m23m33 = __PS_MERGE11(m22m23, m32m33);
505
506 VEC3* pDst = pOut;
507 for (u32 i = 0; i < count; ++i)
508 {
509 f32x2 fp0, fp1;
510
511 // { xy zw } = { x y z 1 }
512 const f32x2 xy = __PSQ_LX(pV, offsetof(VEC3, x), 0, 0);
513 const f32x2 zw = __PSQ_LX(pV, offsetof(VEC3, z), 1, 0);
514
515 // fp0 = [ m00 m01 m02, m03 ] . T[ x y z 1 ]
516 // [ m10 m11 m12, m13 ]
517 fp0 = __PS_MADDS0(m02m12, zw, m03m13);
518 fp0 = __PS_MADDS1(m01m11, xy, fp0);
519 fp0 = __PS_MADDS0(m00m10, xy, fp0);
520
521 // fp1 = [ m20 m21 m22, m23 ] . T[ x y z 1 ]
522 // [ m30 m31 m32, m33 ]
523 fp1 = __PS_MADDS0(m22m32, zw, m23m33);
524 fp1 = __PS_MADDS1(m21m31, xy, fp1);
525 fp1 = __PS_MADDS0(m20m30, xy, fp1);
526
527 // rw[1] = 1 / w
528 const f32x2 rw = __PS_DIV(zw, __PS_MERGE11(fp1, fp1));
529
530 fp0 = __PS_MULS1(fp0, rw);
531 fp1 = __PS_MULS1(fp1, rw);
532
533 tof32x2(pDst->x) = fp0;
534 pDst->z = fp1[0];
535
536 ++pDst;
537 ++pV;
538 }
539
540 return pOut;
541}
542
543NW_MATH_INLINE VEC4*
544VEC4Transform(VEC4* pOut, const MTX44* __restrict pM, const VEC4* __restrict pV)
545{
546 const f32x2 m00m01 = __PSQ_LX(pM, offsetof(MTX44, m[0][0]), 0, 0);
547 const f32x2 m02m03 = __PSQ_LX(pM, offsetof(MTX44, m[0][2]), 0, 0);
548 const f32x2 m10m11 = __PSQ_LX(pM, offsetof(MTX44, m[1][0]), 0, 0);
549 const f32x2 m12m13 = __PSQ_LX(pM, offsetof(MTX44, m[1][2]), 0, 0);
550 const f32x2 m20m21 = __PSQ_LX(pM, offsetof(MTX44, m[2][0]), 0, 0);
551 const f32x2 m22m23 = __PSQ_LX(pM, offsetof(MTX44, m[2][2]), 0, 0);
552 const f32x2 m30m31 = __PSQ_LX(pM, offsetof(MTX44, m[3][0]), 0, 0);
553 const f32x2 m32m33 = __PSQ_LX(pM, offsetof(MTX44, m[3][2]), 0, 0);
554
555 f32x2 fp0, fp1, fp2, fp3;
556
557 // { xy zw } = { x y z w }
558 const f32x2 xy = __PSQ_LX(pV, offsetof(VEC4, x), 0, 0);
559 const f32x2 zw = __PSQ_LX(pV, offsetof(VEC4, z), 0, 0);
560
561 // fp1 = [ m00 m01 m02, m03 ] . T[ x y z w ]
562 // [ m10 m11 m12, m13 ]
563 fp0 = __PS_MUL(m00m01, xy);
564 fp1 = __PS_MUL(m10m11, xy);
565 fp0 = __PS_MADD(m02m03, zw, fp0);
566 fp1 = __PS_MADD(m12m13, zw, fp1);
567 fp0 = __PS_SUM0(fp0, fp0, fp0);
568 fp1 = __PS_SUM1(fp1, fp0, fp1);
569
570 // fp3 = [ m20 m21 m22, m23 ] . T[ x y z w ]
571 // [ m30 m31 m32, m33 ]
572 fp2 = __PS_MUL(m20m21, xy);
573 fp3 = __PS_MUL(m30m31, xy);
574 fp2 = __PS_MADD(m22m23, zw, fp2);
575 fp3 = __PS_MADD(m32m33, zw, fp3);
576 fp2 = __PS_SUM0(fp2, fp2, fp2);
577 fp3 = __PS_SUM1(fp3, fp2, fp3);
578
579 tof32x2(pOut->x) = fp1;
580 tof32x2(pOut->z) = fp3;
581
582 return pOut;
583}
584
585NW_MATH_INLINE VEC4*
586VEC4TransformArray(VEC4* pOut, const MTX44* pM, const VEC4* pV, u32 count)
587{
588 f32x2 m00m01 = __PSQ_LX(pM, offsetof(MTX44, f._00), 0, 0);
589 f32x2 m02m03 = __PSQ_LX(pM, offsetof(MTX44, f._02), 0, 0);
590 f32x2 m10m11 = __PSQ_LX(pM, offsetof(MTX44, f._10), 0, 0);
591 f32x2 m12m13 = __PSQ_LX(pM, offsetof(MTX44, f._12), 0, 0);
592
593 f32x2 m00m10 = __PS_MERGE00(m00m01, m10m11);
594 f32x2 m01m11 = __PS_MERGE11(m00m01, m10m11);
595 f32x2 m02m12 = __PS_MERGE00(m02m03, m12m13);
596 f32x2 m03m13 = __PS_MERGE11(m02m03, m12m13);
597
598 f32x2 m20m21 = __PSQ_LX(pM, offsetof(MTX44, f._20), 0, 0);
599 f32x2 m22m23 = __PSQ_LX(pM, offsetof(MTX44, f._22), 0, 0);
600 f32x2 m30m31 = __PSQ_LX(pM, offsetof(MTX44, f._30), 0, 0);
601 f32x2 m32m33 = __PSQ_LX(pM, offsetof(MTX44, f._32), 0, 0);
602
603 f32x2 m20m30 = __PS_MERGE00(m20m21, m30m31);
604 f32x2 m21m31 = __PS_MERGE11(m20m21, m30m31);
605 f32x2 m22m32 = __PS_MERGE00(m22m23, m32m33);
606 f32x2 m23m33 = __PS_MERGE11(m22m23, m32m33);
607
608 VEC4* pDst = pOut;
609 for (u32 i = 0; i < count; ++i)
610 {
611 f32x2 fp0, fp1;
612
613 // { xy zw } = { x y z w }
614 f32x2 xy = __PSQ_LX(pV, offsetof(VEC4, x), 0, 0);
615 f32x2 zw = __PSQ_LX(pV, offsetof(VEC4, z), 0, 0);
616
617 // fp0 = [ m00 m01 m02, m03 ] . T[ x y z w ]
618 // [ m10 m11 m12, m13 ]
619 fp0 = __PS_MULS0(m00m10, xy);
620 fp0 = __PS_MADDS1(m01m11, xy, fp0);
621 fp0 = __PS_MADDS0(m02m12, zw, fp0);
622 fp0 = __PS_MADDS1(m03m13, zw, fp0);
623
624 // fp1 = [ m20 m21 m22, m23 ] . T[ x y z w ]
625 // [ m30 m31 m32, m33 ]
626 fp1 = __PS_MULS0(m20m30, xy);
627 fp1 = __PS_MADDS1(m21m31, xy, fp1);
628 fp1 = __PS_MADDS0(m22m32, zw, fp1);
629 fp1 = __PS_MADDS1(m23m33, zw, fp1);
630
631 tof32x2(pDst->x) = fp0;
632 tof32x2(pDst->z) = fp1;
633
634 ++pDst;
635 ++pV;
636 }
637
638 return pOut;
639}
640
641NW_MATH_INLINE VEC4*
642VEC4Transform(VEC4* pOut, const MTX34* __restrict pM, const VEC4* __restrict pV)
643{
644 const f32x2 m00m01 = __PSQ_LX(pM, offsetof(MTX44, m[0][0]), 0, 0);
645 const f32x2 m02m03 = __PSQ_LX(pM, offsetof(MTX44, m[0][2]), 0, 0);
646 const f32x2 m10m11 = __PSQ_LX(pM, offsetof(MTX44, m[1][0]), 0, 0);
647 const f32x2 m12m13 = __PSQ_LX(pM, offsetof(MTX44, m[1][2]), 0, 0);
648 const f32x2 m20m21 = __PSQ_LX(pM, offsetof(MTX44, m[2][0]), 0, 0);
649 const f32x2 m22m23 = __PSQ_LX(pM, offsetof(MTX44, m[2][2]), 0, 0);
650
651 f32x2 fp0, fp1, fp2;
652
653 // { xy zw } = { x y z w }
654 const f32x2 xy = __PSQ_LX(pV, offsetof(VEC4, x), 0, 0);
655 const f32x2 zw = __PSQ_LX(pV, offsetof(VEC4, z), 0, 0);
656
657 // fp1 = [ m00 m01 m02 m03 ] . T[ x y z w ]
658 // [ m10 m11 m12 m13 ]
659 fp0 = __PS_MUL(m00m01, xy);
660 fp0 = __PS_MADD(m02m03, zw, fp0);
661 fp0 = __PS_SUM0(fp0, fp0, fp0);
662
663 fp1 = __PS_MUL(m10m11, xy);
664 fp1 = __PS_MADD(m12m13, zw, fp1);
665 fp1 = __PS_SUM1(fp1, fp0, fp1);
666
667 // fp2 = [ m20 m21 m22 m23 ] . T[ x y z w ]
668 // [ 0 0 0 1 ]
669 fp2 = __PS_MUL(m20m21, xy);
670 fp2 = __PS_MADD(m22m23, zw, fp2);
671 fp2 = __PS_SUM0(fp2, zw, fp2);
672
673 tof32x2(pOut->x) = fp1;
674 tof32x2(pOut->z) = fp2;
675
676 return pOut;
677}
678
679NW_MATH_INLINE VEC4*
680VEC4TransformArray(VEC4* pOut, const MTX34* pM, const VEC4* pV, u32 count)
681{
682 f32x2 m00m01 = __PSQ_LX(pM, offsetof(MTX44, f._00), 0, 0);
683 f32x2 m02m03 = __PSQ_LX(pM, offsetof(MTX44, f._02), 0, 0);
684 f32x2 m10m11 = __PSQ_LX(pM, offsetof(MTX44, f._10), 0, 0);
685 f32x2 m12m13 = __PSQ_LX(pM, offsetof(MTX44, f._12), 0, 0);
686
687 f32x2 m20m21 = __PSQ_LX(pM, offsetof(MTX44, f._20), 0, 0);
688 f32x2 m22m23 = __PSQ_LX(pM, offsetof(MTX44, f._22), 0, 0);
689
690 f32x2 m00m10 = __PS_MERGE00(m00m01, m10m11);
691 f32x2 m01m11 = __PS_MERGE11(m00m01, m10m11);
692 f32x2 m02m12 = __PS_MERGE00(m02m03, m12m13);
693 f32x2 m03m13 = __PS_MERGE11(m02m03, m12m13);
694
695 VEC4* pDst = pOut;
696 for (u32 i = 0; i < count; ++i)
697 {
698 f32x2 fp0, fp1;
699
700 // { xy zw } = { x y z w }
701 f32x2 xy = __PSQ_LX(pV, offsetof(VEC4, x), 0, 0);
702 f32x2 zw = __PSQ_LX(pV, offsetof(VEC4, z), 0, 0);
703
704 // fp0 = [ m00 m01 m02, m03 ] . T[ x y z w ]
705 // [ m10 m11 m12, m13 ]
706 // fp1 = [ m20 m21 m22, m23 ] . T[ x y z w ]
707 // [ 0 0 0 1 ]
708 fp0 = __PS_MULS0(m00m10, xy);
709 fp0 = __PS_MADDS1(m01m11, xy, fp0);
710 fp0 = __PS_MADDS0(m02m12, zw, fp0);
711 fp0 = __PS_MADDS1(m03m13, zw, fp0);
712
713 fp1 = __PS_MUL(m20m21, xy);
714 fp1 = __PS_MADD(m22m23, zw, fp1);
715 fp1 = __PS_SUM0(fp1, zw, fp1);
716
717 tof32x2(pDst->x) = fp0;
718 tof32x2(pDst->z) = fp1;
719
720 ++pDst;
721 ++pV;
722 }
723
724 return pOut;
725}
726
727} } // namespace internal::intrinsics
728
729#endif // NW_MATH_ENABLE_INTRINSICS
730
731NW_MATH_INLINE QUAT*
732MTX34ToQUAT(QUAT* pOut, const MTX34* pMtx)
733{
734 f32 tr, s;
735 s32 i, j, k;
736 s32 nxt[3] = {1, 2, 0};
737 f32 q[3];
738
739 const f32 (*const m)[4] = pMtx->m;
740
741 tr = m[0][0] + m[1][1] + m[2][2];
742 if ( tr > 0.0f )
743 {
744 s = (f32)::std::sqrt(tr + 1.0f);
745 pOut->w = s * 0.5f;
746 s = 0.5f / s;
747 pOut->x = (m[2][1] - m[1][2]) * s;
748 pOut->y = (m[0][2] - m[2][0]) * s;
749 pOut->z = (m[1][0] - m[0][1]) * s;
750 }
751 else
752 {
753 i = 0;
754 if (m[1][1] > m[0][0]) i = 1;
755 if (m[2][2] > m[i][i]) i = 2;
756 j = nxt[i];
757 k = nxt[j];
758 s = (f32)::std::sqrt( (m[i][i] - (m[j][j] + m[k][k])) + 1.0f );
759 q[i] = s * 0.5f;
760
761 if (s!=0.0f)
762 s = 0.5f / s;
763
764 pOut->w = (m[k][j] - m[j][k]) * s;
765 q[j] = (m[i][j] + m[j][i]) * s;
766 q[k] = (m[i][k] + m[k][i]) * s;
767
768 pOut->x = q[0];
769 pOut->y = q[1];
770 pOut->z = q[2];
771 }
772
773 return pOut;
774}
775
776NW_MATH_INLINE VEC3*
777VEC3TransformArray(VEC3* pOut, const MTX34* __restrict pM, const VEC3* __restrict pV, s32 count)
778{
779 return NW_MATH_IMPL_NS::VEC3TransformArray(pOut, pM, pV, count);
780}
781
782NW_MATH_INLINE VEC4*
783VEC3TransformArray(VEC4* pOut, const MTX44* __restrict pM, const VEC3* __restrict pV, u32 count)
784{
785 return NW_MATH_IMPL_NS::VEC3TransformArray(pOut, pM, pV, count);
786}
787
788NW_MATH_INLINE VEC3*
789VEC3TransformNormal(VEC3* pOut, const MTX34* pM, const VEC3* pV)
790{
791 return NW_MATH_IMPL_NS::VEC3TransformNormal(pOut, pM, pV);
792}
793
794NW_MATH_INLINE VEC3*
795VEC3TransformNormalArray(VEC3* pOut, const MTX34* pM, const VEC3* pV, u32 count)
796{
797 return NW_MATH_IMPL_NS::VEC3TransformNormalArray(pOut, pM, pV, count);
798}
799
800NW_MATH_INLINE VEC3*
801VEC3TransformCoord(VEC3* pOut, const MTX44* pM, const VEC3* pV)
802{
803 return NW_MATH_IMPL_NS::VEC3TransformCoord(pOut, pM, pV);
804}
805
806NW_MATH_INLINE VEC3*
807VEC3TransformCoordArray(VEC3* pOut, const MTX44* pM, const VEC3* pV, u32 count)
808{
809 return NW_MATH_IMPL_NS::VEC3TransformCoordArray(pOut, pM, pV, count);
810}
811
812NW_MATH_INLINE VEC4*
813VEC4Transform(VEC4* pOut, const MTX44* __restrict pM, const VEC4* __restrict pV)
814{
815 return NW_MATH_IMPL_NS::VEC4Transform(pOut, pM, pV);
816}
817
818NW_MATH_INLINE VEC4*
819VEC4TransformArray(VEC4* pOut, const MTX44* pM, const VEC4* pV, u32 count)
820{
821 return NW_MATH_IMPL_NS::VEC4TransformArray(pOut, pM, pV, count);
822}
823
824NW_MATH_INLINE VEC4*
825VEC4Transform(VEC4* pOut, const MTX34* __restrict pM, const VEC4* __restrict pV)
826{
827 return NW_MATH_IMPL_NS::VEC4Transform(pOut, pM, pV);
828}
829
830NW_MATH_INLINE VEC4*
831VEC4TransformArray(VEC4* pOut, const MTX34* pM, const VEC4* pV, u32 count)
832{
833 return NW_MATH_IMPL_NS::VEC4TransformArray(pOut, pM, pV, count);
834}
835
836} } // namespace nw::math
837
838#endif
839
840#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::VEC4Transform
VEC4 * VEC4Transform(VEC4 *pOut, const MTX34 *pM, const VEC4 *pV)
nw::math::internal::standard::VEC3TransformArray
VEC4 * VEC3TransformArray(VEC4 *pOut, const MTX44 *pM, const VEC3 *pV, u32 count)
nw::math::internal::standard::VEC3TransformNormal
VEC3 * VEC3TransformNormal(VEC3 *pOut, const MTX34 *pM, const VEC3 *pV)
nw::math::internal::standard::VEC3TransformNormalArray
VEC3 * VEC3TransformNormalArray(VEC3 *pOut, const MTX34 *pM, const VEC3 *pV, u32 count)
nw::math::internal::standard::VEC3TransformArray
VEC3 * VEC3TransformArray(VEC3 *pOut, const MTX34 *pM, const VEC3 *pV, s32 count)
nw::math::internal::standard::VEC4TransformArray
VEC4 * VEC4TransformArray(VEC4 *pOut, const MTX44 *pM, const VEC4 *pV, u32 count)
nw::math::internal::standard::VEC3TransformCoordArray
VEC3 * VEC3TransformCoordArray(VEC3 *pOut, const MTX44 *pM, const VEC3 *pV, u32 count)
nw::math::internal::standard::VEC4TransformArray
VEC4 * VEC4TransformArray(VEC4 *pOut, const MTX34 *pM, const VEC4 *pV, u32 count)
nw::math::internal::standard::VEC4Transform
VEC4 * VEC4Transform(VEC4 *pOut, const MTX44 *pM, const VEC4 *pV)
nw::math::internal::standard::VEC3TransformCoord
VEC3 * VEC3TransformCoord(VEC3 *pOut, const MTX44 *pM, const VEC3 *pV)
nw::math::internal
Definition math_Triangular.cpp:3
nw::math
Definition math_Constant.cpp:5
nw::math::VEC3TransformArray
NW_MATH_INLINE VEC4 * VEC3TransformArray(VEC4 *pOut, const MTX44 *pM, const VEC3 *pV, u32 count)
nw::math::VEC4Transform
NW_MATH_INLINE VEC4 * VEC4Transform(VEC4 *pOut, const MTX44 *pM, const VEC4 *pV)
nw::math::VEC4TransformArray
NW_MATH_INLINE VEC4 * VEC4TransformArray(VEC4 *pOut, const MTX44 *pM, const VEC4 *pV, u32 n)
nw::math::VEC4Transform
NW_MATH_INLINE VEC4 * VEC4Transform(VEC4 *pOut, const MTX34 *pM, const VEC4 *pV)
nw::math::VEC3TransformCoordArray
NW_MATH_INLINE VEC3 * VEC3TransformCoordArray(VEC3 *pOut, const MTX44 *pM, const VEC3 *pV, u32 count)
nw::math::VEC3TransformArray
NW_MATH_INLINE VEC3 * VEC3TransformArray(VEC3 *pOut, const MTX34 *pM, const VEC3 *pV, s32 count)
nw::math::VEC3TransformNormal
NW_MATH_INLINE VEC3 * VEC3TransformNormal(VEC3 *pOut, const MTX34 *pM, const VEC3 *pV)
nw::math::VEC3TransformCoord
NW_MATH_INLINE VEC3 * VEC3TransformCoord(VEC3 *pOut, const MTX44 *pM, const VEC3 *pV)
nw::math::VEC3TransformNormalArray
NW_MATH_INLINE VEC3 * VEC3TransformNormalArray(VEC3 *pOutArray, const MTX34 *pM, const VEC3 *pArray, u32 n)
nw::math::VEC4TransformArray
NW_MATH_INLINE VEC4 * VEC4TransformArray(VEC4 *pOut, const MTX34 *pM, const VEC4 *pV, u32 n)
nw
Definition math_Constant.cpp:5
nw::math::MTX34
Definition math_Matrix34.h:136
nw::math::MTX44
Definition math_Matrix44.h:189
nw::math::QUAT
Definition math_Quaternion.h:84
nw::math::VEC2
Definition math_Vector2.h:49
nw::math::VEC3
Definition math_Vector3.h:62
nw::math::VEC4
Definition math_Vector4.h:69