-
Notifications
You must be signed in to change notification settings - Fork 807
/
PsUnixSse2InlineAoS.h
3239 lines (2767 loc) · 87.5 KB
/
PsUnixSse2InlineAoS.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// * Neither the name of NVIDIA CORPORATION nor the names of its
// contributors may be used to endorse or promote products derived
// from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ``AS IS'' AND ANY
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// Copyright (c) 2008-2019 NVIDIA Corporation. All rights reserved.
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
#ifndef PSFOUNDATION_PSUNIXSSE2INLINEAOS_H
#define PSFOUNDATION_PSUNIXSSE2INLINEAOS_H
#if !COMPILE_VECTOR_INTRINSICS
#error Vector intrinsics should not be included when using scalar implementation.
#endif
#ifdef __SSE4_2__
#include "smmintrin.h"
#endif
#include "../../PsVecMathSSE.h"
namespace physx
{
namespace shdfnd
{
namespace aos
{
#define PX_FPCLASS_SNAN 0x0001 /* signaling NaN */
#define PX_FPCLASS_QNAN 0x0002 /* quiet NaN */
#define PX_FPCLASS_NINF 0x0004 /* negative infinity */
#define PX_FPCLASS_PINF 0x0200 /* positive infinity */
PX_FORCE_INLINE __m128 m128_I2F(__m128i n)
{
return _mm_castsi128_ps(n);
}
PX_FORCE_INLINE __m128i m128_F2I(__m128 n)
{
return _mm_castps_si128(n);
}
//////////////////////////////////////////////////////////////////////
//Test that Vec3V and FloatV are legal
//////////////////////////////////////////////////////////////////////
#define FLOAT_COMPONENTS_EQUAL_THRESHOLD 0.01f
PX_FORCE_INLINE static bool isValidFloatV(const FloatV a)
{
const PxF32 x = V4ReadX(a);
const PxF32 y = V4ReadY(a);
const PxF32 z = V4ReadZ(a);
const PxF32 w = V4ReadW(a);
if (
(PxAbs(x - y) < FLOAT_COMPONENTS_EQUAL_THRESHOLD) &&
(PxAbs(x - z) < FLOAT_COMPONENTS_EQUAL_THRESHOLD) &&
(PxAbs(x - w) < FLOAT_COMPONENTS_EQUAL_THRESHOLD)
)
{
return true;
}
if (
(PxAbs((x - y) / x) < FLOAT_COMPONENTS_EQUAL_THRESHOLD) &&
(PxAbs((x - z) / x) < FLOAT_COMPONENTS_EQUAL_THRESHOLD) &&
(PxAbs((x - w) / x) < FLOAT_COMPONENTS_EQUAL_THRESHOLD)
)
{
return true;
}
return false;
}
PX_FORCE_INLINE bool isValidVec3V(const Vec3V a)
{
PX_ALIGN(16, PxF32 f[4]);
V4StoreA(a, f);
return (f[3] == 0.0f);
}
PX_FORCE_INLINE bool isFiniteLength(const Vec3V a)
{
return !FAllEq(V4LengthSq(a), FZero());
}
PX_FORCE_INLINE bool isAligned16(void* a)
{
return(0 == (size_t(a) & 0x0f));
}
//ASSERT_FINITELENGTH is deactivated because there is a lot of code that calls a simd normalisation function with zero length but then ignores the result.
#if PX_DEBUG
#define ASSERT_ISVALIDVEC3V(a) PX_ASSERT(isValidVec3V(a))
#define ASSERT_ISVALIDFLOATV(a) PX_ASSERT(isValidFloatV(a))
#define ASSERT_ISALIGNED16(a) PX_ASSERT(isAligned16(reinterpret_cast<void*>(a)))
#define ASSERT_ISFINITELENGTH(a) //PX_ASSERT(isFiniteLength(a))
#else
#define ASSERT_ISVALIDVEC3V(a)
#define ASSERT_ISVALIDFLOATV(a)
#define ASSERT_ISALIGNED16(a)
#define ASSERT_ISFINITELENGTH(a)
#endif
namespace internalUnitSSE2Simd
{
PX_FORCE_INLINE PxU32 BAllTrue4_R(const BoolV a)
{
const PxI32 moveMask = _mm_movemask_ps(a);
return PxU32(moveMask == 0xf);
}
PX_FORCE_INLINE PxU32 BAllTrue3_R(const BoolV a)
{
const PxI32 moveMask = _mm_movemask_ps(a);
return PxU32((moveMask & 0x7) == 0x7);
}
PX_FORCE_INLINE PxU32 BAnyTrue4_R(const BoolV a)
{
const PxI32 moveMask = _mm_movemask_ps(a);
return PxU32(moveMask != 0x0);
}
PX_FORCE_INLINE PxU32 BAnyTrue3_R(const BoolV a)
{
const PxI32 moveMask = _mm_movemask_ps(a);
return PxU32((moveMask & 0x7) != 0x0);
}
PX_FORCE_INLINE PxU32 FiniteTestEq(const Vec4V a, const Vec4V b)
{
// This is a bit of a bodge.
//_mm_comieq_ss returns 1 if either value is nan so we need to re-cast a and b with true encoded as a non-nan
// number.
// There must be a better way of doing this in sse.
const BoolV one = FOne();
const BoolV zero = FZero();
const BoolV a1 = V4Sel(a, one, zero);
const BoolV b1 = V4Sel(b, one, zero);
return (
_mm_comieq_ss(a1, b1) &&
_mm_comieq_ss(_mm_shuffle_ps(a1, a1, _MM_SHUFFLE(1, 1, 1, 1)), _mm_shuffle_ps(b1, b1, _MM_SHUFFLE(1, 1, 1, 1))) &&
_mm_comieq_ss(_mm_shuffle_ps(a1, a1, _MM_SHUFFLE(2, 2, 2, 2)), _mm_shuffle_ps(b1, b1, _MM_SHUFFLE(2, 2, 2, 2))) &&
_mm_comieq_ss(_mm_shuffle_ps(a1, a1, _MM_SHUFFLE(3, 3, 3, 3)), _mm_shuffle_ps(b1, b1, _MM_SHUFFLE(3, 3, 3, 3))));
}
#if !PX_EMSCRIPTEN
const PX_ALIGN(16, PxF32 gMaskXYZ[4]) = { physx::PxUnionCast<PxF32>(0xffffffff), physx::PxUnionCast<PxF32>(0xffffffff),
physx::PxUnionCast<PxF32>(0xffffffff), 0 };
#else
// emscripten doesn't like the PxUnionCast data structure
// the following is what windows and xbox does -- using these for emscripten
const PX_ALIGN(16, PxU32 gMaskXYZ[4]) = { 0xffffffff, 0xffffffff, 0xffffffff, 0 }; }
#endif
}
namespace _VecMathTests
{
// PT: this function returns an invalid Vec3V (W!=0.0f) just for unit-testing 'isValidVec3V'
PX_FORCE_INLINE Vec3V getInvalidVec3V()
{
const float f = 1.0f;
return _mm_load1_ps(&f);
}
PX_FORCE_INLINE bool allElementsEqualFloatV(const FloatV a, const FloatV b)
{
ASSERT_ISVALIDFLOATV(a);
ASSERT_ISVALIDFLOATV(b);
return _mm_comieq_ss(a, b) != 0;
}
PX_FORCE_INLINE bool allElementsEqualVec3V(const Vec3V a, const Vec3V b)
{
return V3AllEq(a, b) != 0;
}
PX_FORCE_INLINE bool allElementsEqualVec4V(const Vec4V a, const Vec4V b)
{
return V4AllEq(a, b) != 0;
}
PX_FORCE_INLINE bool allElementsEqualBoolV(const BoolV a, const BoolV b)
{
return internalUnitSSE2Simd::BAllTrue4_R(VecI32V_IsEq(m128_F2I(a), m128_F2I(b))) != 0;
}
PX_FORCE_INLINE bool allElementsEqualVecU32V(const VecU32V a, const VecU32V b)
{
return internalUnitSSE2Simd::BAllTrue4_R(V4IsEqU32(a, b)) != 0;
}
PX_FORCE_INLINE bool allElementsEqualVecI32V(const VecI32V a, const VecI32V b)
{
BoolV c = m128_I2F(_mm_cmpeq_epi32(a, b));
return internalUnitSSE2Simd::BAllTrue4_R(c) != 0;
}
#define VECMATH_AOS_EPSILON (1e-3f)
PX_FORCE_INLINE bool allElementsNearEqualFloatV(const FloatV a, const FloatV b)
{
ASSERT_ISVALIDFLOATV(a);
ASSERT_ISVALIDFLOATV(b);
const FloatV c = FSub(a, b);
const FloatV minError = FLoad(-VECMATH_AOS_EPSILON);
const FloatV maxError = FLoad(VECMATH_AOS_EPSILON);
return _mm_comigt_ss(c, minError) && _mm_comilt_ss(c, maxError);
}
PX_FORCE_INLINE bool allElementsNearEqualVec3V(const Vec3V a, const Vec3V b)
{
const Vec3V c = V3Sub(a, b);
const Vec3V minError = V3Load(-VECMATH_AOS_EPSILON);
const Vec3V maxError = V3Load(VECMATH_AOS_EPSILON);
return (_mm_comigt_ss(_mm_shuffle_ps(c, c, _MM_SHUFFLE(0, 0, 0, 0)), minError) &&
_mm_comilt_ss(_mm_shuffle_ps(c, c, _MM_SHUFFLE(0, 0, 0, 0)), maxError) &&
_mm_comigt_ss(_mm_shuffle_ps(c, c, _MM_SHUFFLE(1, 1, 1, 1)), minError) &&
_mm_comilt_ss(_mm_shuffle_ps(c, c, _MM_SHUFFLE(1, 1, 1, 1)), maxError) &&
_mm_comigt_ss(_mm_shuffle_ps(c, c, _MM_SHUFFLE(2, 2, 2, 2)), minError) &&
_mm_comilt_ss(_mm_shuffle_ps(c, c, _MM_SHUFFLE(2, 2, 2, 2)), maxError));
}
PX_FORCE_INLINE bool allElementsNearEqualVec4V(const Vec4V a, const Vec4V b)
{
const Vec4V c = V4Sub(a, b);
const Vec4V minError = V4Load(-VECMATH_AOS_EPSILON);
const Vec4V maxError = V4Load(VECMATH_AOS_EPSILON);
return (_mm_comigt_ss(_mm_shuffle_ps(c, c, _MM_SHUFFLE(0, 0, 0, 0)), minError) &&
_mm_comilt_ss(_mm_shuffle_ps(c, c, _MM_SHUFFLE(0, 0, 0, 0)), maxError) &&
_mm_comigt_ss(_mm_shuffle_ps(c, c, _MM_SHUFFLE(1, 1, 1, 1)), minError) &&
_mm_comilt_ss(_mm_shuffle_ps(c, c, _MM_SHUFFLE(1, 1, 1, 1)), maxError) &&
_mm_comigt_ss(_mm_shuffle_ps(c, c, _MM_SHUFFLE(2, 2, 2, 2)), minError) &&
_mm_comilt_ss(_mm_shuffle_ps(c, c, _MM_SHUFFLE(2, 2, 2, 2)), maxError) &&
_mm_comigt_ss(_mm_shuffle_ps(c, c, _MM_SHUFFLE(3, 3, 3, 3)), minError) &&
_mm_comilt_ss(_mm_shuffle_ps(c, c, _MM_SHUFFLE(3, 3, 3, 3)), maxError));
}
}
/////////////////////////////////////////////////////////////////////
////FUNCTIONS USED ONLY FOR ASSERTS IN VECTORISED IMPLEMENTATIONS
/////////////////////////////////////////////////////////////////////
PX_FORCE_INLINE bool isFiniteFloatV(const FloatV a)
{
PxF32 badNumber =
physx::PxUnionCast<PxF32, PxU32>(PX_FPCLASS_SNAN | PX_FPCLASS_QNAN | PX_FPCLASS_NINF | PX_FPCLASS_PINF);
const FloatV vBadNum = FLoad(badNumber);
const BoolV vMask = BAnd(vBadNum, a);
return internalUnitSSE2Simd::FiniteTestEq(vMask, BFFFF()) == 1;
}
PX_FORCE_INLINE bool isFiniteVec3V(const Vec3V a)
{
PxF32 badNumber =
physx::PxUnionCast<PxF32, PxU32>(PX_FPCLASS_SNAN | PX_FPCLASS_QNAN | PX_FPCLASS_NINF | PX_FPCLASS_PINF);
const Vec3V vBadNum = V3Load(badNumber);
const BoolV vMask = BAnd(BAnd(vBadNum, a), BTTTF());
return internalUnitSSE2Simd::FiniteTestEq(vMask, BFFFF()) == 1;
}
PX_FORCE_INLINE bool isFiniteVec4V(const Vec4V a)
{
/*Vec4V a;
PX_ALIGN(16, PxF32 f[4]);
F32Array_Aligned_From_Vec4V(a, f);
return PxIsFinite(f[0])
&& PxIsFinite(f[1])
&& PxIsFinite(f[2])
&& PxIsFinite(f[3]);*/
PxF32 badNumber =
physx::PxUnionCast<PxF32, PxU32>(PX_FPCLASS_SNAN | PX_FPCLASS_QNAN | PX_FPCLASS_NINF | PX_FPCLASS_PINF);
const Vec4V vBadNum = V4Load(badNumber);
const BoolV vMask = BAnd(vBadNum, a);
return internalUnitSSE2Simd::FiniteTestEq(vMask, BFFFF()) == 1;
}
PX_FORCE_INLINE bool hasZeroElementinFloatV(const FloatV a)
{
ASSERT_ISVALIDFLOATV(a);
return _mm_comieq_ss(_mm_shuffle_ps(a, a, _MM_SHUFFLE(0, 0, 0, 0)), FZero()) ? true : false;
}
PX_FORCE_INLINE bool hasZeroElementInVec3V(const Vec3V a)
{
return (_mm_comieq_ss(_mm_shuffle_ps(a, a, _MM_SHUFFLE(0, 0, 0, 0)), FZero()) ||
_mm_comieq_ss(_mm_shuffle_ps(a, a, _MM_SHUFFLE(1, 1, 1, 1)), FZero()) ||
_mm_comieq_ss(_mm_shuffle_ps(a, a, _MM_SHUFFLE(2, 2, 2, 2)), FZero()));
}
PX_FORCE_INLINE bool hasZeroElementInVec4V(const Vec4V a)
{
return (_mm_comieq_ss(_mm_shuffle_ps(a, a, _MM_SHUFFLE(0, 0, 0, 0)), FZero()) ||
_mm_comieq_ss(_mm_shuffle_ps(a, a, _MM_SHUFFLE(1, 1, 1, 1)), FZero()) ||
_mm_comieq_ss(_mm_shuffle_ps(a, a, _MM_SHUFFLE(2, 2, 2, 2)), FZero()) ||
_mm_comieq_ss(_mm_shuffle_ps(a, a, _MM_SHUFFLE(3, 3, 3, 3)), FZero()));
}
/////////////////////////////////////////////////////////////////////
////VECTORISED FUNCTION IMPLEMENTATIONS
/////////////////////////////////////////////////////////////////////
PX_FORCE_INLINE FloatV FLoad(const PxF32 f)
{
return _mm_load1_ps(&f);
}
PX_FORCE_INLINE Vec3V V3Load(const PxF32 f)
{
return _mm_set_ps(0.0f, f, f, f);
}
PX_FORCE_INLINE Vec4V V4Load(const PxF32 f)
{
return _mm_load1_ps(&f);
}
PX_FORCE_INLINE BoolV BLoad(const bool f)
{
const PxU32 i = -PxI32(f);
return _mm_load1_ps(reinterpret_cast<const float*>(&i));
}
PX_FORCE_INLINE Vec3V V3LoadA(const PxVec3& f)
{
ASSERT_ISALIGNED16(const_cast<PxVec3*>(&f));
#if !PX_EMSCRIPTEN
return _mm_and_ps(reinterpret_cast<const Vec3V&>(f), V4LoadA(internalUnitSSE2Simd::gMaskXYZ));
#else
return _mm_and_ps((Vec3V&)f, (VecI32V&)internalUnitSSE2Simd::gMaskXYZ);
#endif
}
PX_FORCE_INLINE Vec3V V3LoadU(const PxVec3& f)
{
return _mm_set_ps(0.0f, f.z, f.y, f.x);
}
PX_FORCE_INLINE Vec3V V3LoadUnsafeA(const PxVec3& f)
{
ASSERT_ISALIGNED16(const_cast<PxVec3*>(&f));
return _mm_set_ps(0.0f, f.z, f.y, f.x);
}
PX_FORCE_INLINE Vec3V V3LoadA(const PxF32* const f)
{
ASSERT_ISALIGNED16(const_cast<PxF32*>(f));
#if !PX_EMSCRIPTEN
return _mm_and_ps(V4LoadA(f), V4LoadA(internalUnitSSE2Simd::gMaskXYZ));
#else
return _mm_and_ps((Vec3V&)*f, (VecI32V&)internalUnitSSE2Simd::gMaskXYZ);
#endif
}
PX_FORCE_INLINE Vec3V V3LoadU(const PxF32* const i)
{
return _mm_set_ps(0.0f, i[2], i[1], i[0]);
}
PX_FORCE_INLINE Vec3V Vec3V_From_Vec4V(Vec4V v)
{
return V4ClearW(v);
}
PX_FORCE_INLINE Vec3V Vec3V_From_Vec4V_WUndefined(const Vec4V v)
{
return v;
}
PX_FORCE_INLINE Vec4V Vec4V_From_Vec3V(Vec3V f)
{
ASSERT_ISVALIDVEC3V(f);
return f; // ok if it is implemented as the same type.
}
PX_FORCE_INLINE Vec4V Vec4V_From_PxVec3_WUndefined(const PxVec3& f)
{
return _mm_set_ps(0.0f, f.z, f.y, f.x);
}
PX_FORCE_INLINE Vec4V Vec4V_From_FloatV(FloatV f)
{
return f;
}
PX_FORCE_INLINE Vec3V Vec3V_From_FloatV(FloatV f)
{
ASSERT_ISVALIDFLOATV(f);
return Vec3V_From_Vec4V(Vec4V_From_FloatV(f));
}
PX_FORCE_INLINE Vec3V Vec3V_From_FloatV_WUndefined(FloatV f)
{
ASSERT_ISVALIDVEC3V(f);
return Vec3V_From_Vec4V_WUndefined(Vec4V_From_FloatV(f));
}
PX_FORCE_INLINE Mat33V Mat33V_From_PxMat33(const PxMat33& m)
{
return Mat33V(V3LoadU(m.column0), V3LoadU(m.column1), V3LoadU(m.column2));
}
PX_FORCE_INLINE void PxMat33_From_Mat33V(const Mat33V& m, PxMat33& out)
{
V3StoreU(m.col0, out.column0);
V3StoreU(m.col1, out.column1);
V3StoreU(m.col2, out.column2);
}
PX_FORCE_INLINE Vec4V V4LoadA(const PxF32* const f)
{
ASSERT_ISALIGNED16(const_cast<PxF32*>(f));
return _mm_load_ps(f);
}
PX_FORCE_INLINE void V4StoreA(Vec4V a, PxF32* f)
{
ASSERT_ISALIGNED16(f);
_mm_store_ps(f, a);
}
PX_FORCE_INLINE void V4StoreU(const Vec4V a, PxF32* f)
{
_mm_storeu_ps(f, a);
}
PX_FORCE_INLINE void BStoreA(const BoolV a, PxU32* f)
{
ASSERT_ISALIGNED16(f);
_mm_store_ps(reinterpret_cast<PxF32*>(f), a);
}
PX_FORCE_INLINE void U4StoreA(const VecU32V uv, PxU32* u)
{
ASSERT_ISALIGNED16(u);
_mm_store_ps(reinterpret_cast<float*>(u), uv);
}
PX_FORCE_INLINE void I4StoreA(const VecI32V iv, PxI32* i)
{
ASSERT_ISALIGNED16(i);
_mm_store_ps(reinterpret_cast<float*>(i), m128_I2F(iv));
}
PX_FORCE_INLINE Vec4V V4LoadU(const PxF32* const f)
{
return _mm_loadu_ps(f);
}
PX_FORCE_INLINE BoolV BLoad(const bool* const f)
{
const PX_ALIGN(16, PxI32) b[4] = { -PxI32(f[0]), -PxI32(f[1]), -PxI32(f[2]), -PxI32(f[3]) };
return _mm_load_ps(reinterpret_cast<const float*>(&b));
}
PX_FORCE_INLINE void FStore(const FloatV a, PxF32* PX_RESTRICT f)
{
ASSERT_ISVALIDFLOATV(a);
_mm_store_ss(f, a);
}
PX_FORCE_INLINE void V3StoreA(const Vec3V a, PxVec3& f)
{
ASSERT_ISALIGNED16(&f);
PX_ALIGN(16, PxF32) f2[4];
_mm_store_ps(f2, a);
f = PxVec3(f2[0], f2[1], f2[2]);
}
PX_FORCE_INLINE void V3StoreU(const Vec3V a, PxVec3& f)
{
PX_ALIGN(16, PxF32) f2[4];
_mm_store_ps(f2, a);
f = PxVec3(f2[0], f2[1], f2[2]);
}
PX_FORCE_INLINE void Store_From_BoolV(const BoolV b, PxU32* b2)
{
_mm_store_ss(reinterpret_cast<PxF32*>(b2), b);
}
PX_FORCE_INLINE VecU32V U4Load(const PxU32 i)
{
return _mm_load1_ps(reinterpret_cast<const PxF32*>(&i));
}
PX_FORCE_INLINE VecU32V U4LoadU(const PxU32* i)
{
return _mm_loadu_ps(reinterpret_cast<const PxF32*>(i));
}
PX_FORCE_INLINE VecU32V U4LoadA(const PxU32* i)
{
ASSERT_ISALIGNED16(const_cast<PxU32*>(i));
return _mm_load_ps(reinterpret_cast<const PxF32*>(i));
}
//////////////////////////////////
// FLOATV
//////////////////////////////////
PX_FORCE_INLINE FloatV FZero()
{
return FLoad(0.0f);
}
PX_FORCE_INLINE FloatV FOne()
{
return FLoad(1.0f);
}
PX_FORCE_INLINE FloatV FHalf()
{
return FLoad(0.5f);
}
PX_FORCE_INLINE FloatV FEps()
{
return FLoad(PX_EPS_REAL);
}
PX_FORCE_INLINE FloatV FEps6()
{
return FLoad(1e-6f);
}
PX_FORCE_INLINE FloatV FMax()
{
return FLoad(PX_MAX_REAL);
}
PX_FORCE_INLINE FloatV FNegMax()
{
return FLoad(-PX_MAX_REAL);
}
PX_FORCE_INLINE FloatV IZero()
{
const PxU32 zero = 0;
return _mm_load1_ps(reinterpret_cast<const PxF32*>(&zero));
}
PX_FORCE_INLINE FloatV IOne()
{
const PxU32 one = 1;
return _mm_load1_ps(reinterpret_cast<const PxF32*>(&one));
}
PX_FORCE_INLINE FloatV ITwo()
{
const PxU32 two = 2;
return _mm_load1_ps(reinterpret_cast<const PxF32*>(&two));
}
PX_FORCE_INLINE FloatV IThree()
{
const PxU32 three = 3;
return _mm_load1_ps(reinterpret_cast<const PxF32*>(&three));
}
PX_FORCE_INLINE FloatV IFour()
{
PxU32 four = 4;
return _mm_load1_ps(reinterpret_cast<const PxF32*>(&four));
}
PX_FORCE_INLINE FloatV FNeg(const FloatV f)
{
ASSERT_ISVALIDFLOATV(f);
return _mm_sub_ps(_mm_setzero_ps(), f);
}
PX_FORCE_INLINE FloatV FAdd(const FloatV a, const FloatV b)
{
ASSERT_ISVALIDFLOATV(a);
ASSERT_ISVALIDFLOATV(b);
/*
if(!isValidFloatV(a))
{
assert(false);
}
if(!isValidFloatV(b))
{
assert(false);
}
*/
return _mm_add_ps(a, b);
}
PX_FORCE_INLINE FloatV FSub(const FloatV a, const FloatV b)
{
ASSERT_ISVALIDFLOATV(a);
ASSERT_ISVALIDFLOATV(b);
return _mm_sub_ps(a, b);
}
PX_FORCE_INLINE FloatV FMul(const FloatV a, const FloatV b)
{
ASSERT_ISVALIDFLOATV(a);
ASSERT_ISVALIDFLOATV(b);
return _mm_mul_ps(a, b);
}
PX_FORCE_INLINE FloatV FDiv(const FloatV a, const FloatV b)
{
ASSERT_ISVALIDFLOATV(a);
ASSERT_ISVALIDFLOATV(b);
return _mm_div_ps(a, b);
}
PX_FORCE_INLINE FloatV FDivFast(const FloatV a, const FloatV b)
{
ASSERT_ISVALIDFLOATV(a);
ASSERT_ISVALIDFLOATV(b);
return _mm_mul_ps(a, _mm_rcp_ps(b));
}
PX_FORCE_INLINE FloatV FRecip(const FloatV a)
{
ASSERT_ISVALIDFLOATV(a);
return _mm_div_ps(FOne(), a);
}
PX_FORCE_INLINE FloatV FRecipFast(const FloatV a)
{
ASSERT_ISVALIDFLOATV(a);
return _mm_rcp_ps(a);
}
PX_FORCE_INLINE FloatV FRsqrt(const FloatV a)
{
ASSERT_ISVALIDFLOATV(a);
return _mm_div_ps(FOne(), _mm_sqrt_ps(a));
}
PX_FORCE_INLINE FloatV FSqrt(const FloatV a)
{
ASSERT_ISVALIDFLOATV(a);
return _mm_sqrt_ps(a);
}
PX_FORCE_INLINE FloatV FRsqrtFast(const FloatV a)
{
ASSERT_ISVALIDFLOATV(a);
return _mm_rsqrt_ps(a);
}
PX_FORCE_INLINE FloatV FScaleAdd(const FloatV a, const FloatV b, const FloatV c)
{
ASSERT_ISVALIDFLOATV(a);
ASSERT_ISVALIDFLOATV(b);
ASSERT_ISVALIDFLOATV(c);
return FAdd(FMul(a, b), c);
}
PX_FORCE_INLINE FloatV FNegScaleSub(const FloatV a, const FloatV b, const FloatV c)
{
ASSERT_ISVALIDFLOATV(a);
ASSERT_ISVALIDFLOATV(b);
ASSERT_ISVALIDFLOATV(c);
return FSub(c, FMul(a, b));
}
PX_FORCE_INLINE FloatV FAbs(const FloatV a)
{
ASSERT_ISVALIDFLOATV(a);
PX_ALIGN(16, const PxU32) absMask[4] = { 0x7fFFffFF, 0x7fFFffFF, 0x7fFFffFF, 0x7fFFffFF };
return _mm_and_ps(a, _mm_load_ps(reinterpret_cast<const PxF32*>(absMask)));
}
PX_FORCE_INLINE FloatV FSel(const BoolV c, const FloatV a, const FloatV b)
{
PX_ASSERT(_VecMathTests::allElementsEqualBoolV(c,BTTTT()) ||
_VecMathTests::allElementsEqualBoolV(c,BFFFF()));
ASSERT_ISVALIDFLOATV(_mm_or_ps(_mm_andnot_ps(c, b), _mm_and_ps(c, a)));
return _mm_or_ps(_mm_andnot_ps(c, b), _mm_and_ps(c, a));
}
PX_FORCE_INLINE BoolV FIsGrtr(const FloatV a, const FloatV b)
{
ASSERT_ISVALIDFLOATV(a);
ASSERT_ISVALIDFLOATV(b);
return _mm_cmpgt_ps(a, b);
}
PX_FORCE_INLINE BoolV FIsGrtrOrEq(const FloatV a, const FloatV b)
{
ASSERT_ISVALIDFLOATV(a);
ASSERT_ISVALIDFLOATV(b);
return _mm_cmpge_ps(a, b);
}
PX_FORCE_INLINE BoolV FIsEq(const FloatV a, const FloatV b)
{
ASSERT_ISVALIDFLOATV(a);
ASSERT_ISVALIDFLOATV(b);
return _mm_cmpeq_ps(a, b);
}
PX_FORCE_INLINE FloatV FMax(const FloatV a, const FloatV b)
{
ASSERT_ISVALIDFLOATV(a);
ASSERT_ISVALIDFLOATV(b);
return _mm_max_ps(a, b);
}
PX_FORCE_INLINE FloatV FMin(const FloatV a, const FloatV b)
{
ASSERT_ISVALIDFLOATV(a);
ASSERT_ISVALIDFLOATV(b);
return _mm_min_ps(a, b);
}
PX_FORCE_INLINE FloatV FClamp(const FloatV a, const FloatV minV, const FloatV maxV)
{
ASSERT_ISVALIDFLOATV(minV);
ASSERT_ISVALIDFLOATV(maxV);
return _mm_max_ps(_mm_min_ps(a, maxV), minV);
}
PX_FORCE_INLINE PxU32 FAllGrtr(const FloatV a, const FloatV b)
{
ASSERT_ISVALIDFLOATV(a);
ASSERT_ISVALIDFLOATV(b);
return _mm_comigt_ss(a, b);
}
PX_FORCE_INLINE PxU32 FAllGrtrOrEq(const FloatV a, const FloatV b)
{
ASSERT_ISVALIDFLOATV(a);
ASSERT_ISVALIDFLOATV(b);
return _mm_comige_ss(a, b);
}
PX_FORCE_INLINE PxU32 FAllEq(const FloatV a, const FloatV b)
{
ASSERT_ISVALIDFLOATV(a);
ASSERT_ISVALIDFLOATV(b);
return _mm_comieq_ss(a, b);
}
PX_FORCE_INLINE FloatV FRound(const FloatV a)
{
ASSERT_ISVALIDFLOATV(a);
#ifdef __SSE4_2__
return _mm_round_ps(a, _MM_FROUND_TO_NEAREST_INT | _MM_FROUND_NO_EXC);
#else
// return _mm_round_ps(a, 0x0);
const FloatV half = FLoad(0.5f);
const __m128 signBit = _mm_cvtepi32_ps(_mm_srli_epi32(_mm_cvtps_epi32(a), 31));
const FloatV aRound = FSub(FAdd(a, half), signBit);
__m128i tmp = _mm_cvttps_epi32(aRound);
return _mm_cvtepi32_ps(tmp);
#endif
}
PX_FORCE_INLINE FloatV FSin(const FloatV a)
{
ASSERT_ISVALIDFLOATV(a);
// Modulo the range of the given angles such that -XM_2PI <= Angles < XM_2PI
const FloatV recipTwoPi = V4LoadA(g_PXReciprocalTwoPi.f);
const FloatV twoPi = V4LoadA(g_PXTwoPi.f);
const FloatV tmp = FMul(a, recipTwoPi);
const FloatV b = FRound(tmp);
const FloatV V1 = FNegScaleSub(twoPi, b, a);
// sin(V) ~= V - V^3 / 3! + V^5 / 5! - V^7 / 7! + V^9 / 9! - V^11 / 11! + V^13 / 13! -
// V^15 / 15! + V^17 / 17! - V^19 / 19! + V^21 / 21! - V^23 / 23! (for -PI <= V < PI)
const FloatV V2 = FMul(V1, V1);
const FloatV V3 = FMul(V2, V1);
const FloatV V5 = FMul(V3, V2);
const FloatV V7 = FMul(V5, V2);
const FloatV V9 = FMul(V7, V2);
const FloatV V11 = FMul(V9, V2);
const FloatV V13 = FMul(V11, V2);
const FloatV V15 = FMul(V13, V2);
const FloatV V17 = FMul(V15, V2);
const FloatV V19 = FMul(V17, V2);
const FloatV V21 = FMul(V19, V2);
const FloatV V23 = FMul(V21, V2);
const Vec4V sinCoefficients0 = V4LoadA(g_PXSinCoefficients0.f);
const Vec4V sinCoefficients1 = V4LoadA(g_PXSinCoefficients1.f);
const Vec4V sinCoefficients2 = V4LoadA(g_PXSinCoefficients2.f);
const FloatV S1 = V4GetY(sinCoefficients0);
const FloatV S2 = V4GetZ(sinCoefficients0);
const FloatV S3 = V4GetW(sinCoefficients0);
const FloatV S4 = V4GetX(sinCoefficients1);
const FloatV S5 = V4GetY(sinCoefficients1);
const FloatV S6 = V4GetZ(sinCoefficients1);
const FloatV S7 = V4GetW(sinCoefficients1);
const FloatV S8 = V4GetX(sinCoefficients2);
const FloatV S9 = V4GetY(sinCoefficients2);
const FloatV S10 = V4GetZ(sinCoefficients2);
const FloatV S11 = V4GetW(sinCoefficients2);
FloatV Result;
Result = FScaleAdd(S1, V3, V1);
Result = FScaleAdd(S2, V5, Result);
Result = FScaleAdd(S3, V7, Result);
Result = FScaleAdd(S4, V9, Result);
Result = FScaleAdd(S5, V11, Result);
Result = FScaleAdd(S6, V13, Result);
Result = FScaleAdd(S7, V15, Result);
Result = FScaleAdd(S8, V17, Result);
Result = FScaleAdd(S9, V19, Result);
Result = FScaleAdd(S10, V21, Result);
Result = FScaleAdd(S11, V23, Result);
return Result;
}
PX_FORCE_INLINE FloatV FCos(const FloatV a)
{
ASSERT_ISVALIDFLOATV(a);
// Modulo the range of the given angles such that -XM_2PI <= Angles < XM_2PI
const FloatV recipTwoPi = V4LoadA(g_PXReciprocalTwoPi.f);
const FloatV twoPi = V4LoadA(g_PXTwoPi.f);
const FloatV tmp = FMul(a, recipTwoPi);
const FloatV b = FRound(tmp);
const FloatV V1 = FNegScaleSub(twoPi, b, a);
// cos(V) ~= 1 - V^2 / 2! + V^4 / 4! - V^6 / 6! + V^8 / 8! - V^10 / 10! + V^12 / 12! -
// V^14 / 14! + V^16 / 16! - V^18 / 18! + V^20 / 20! - V^22 / 22! (for -PI <= V < PI)
const FloatV V2 = FMul(V1, V1);
const FloatV V4 = FMul(V2, V2);
const FloatV V6 = FMul(V4, V2);
const FloatV V8 = FMul(V4, V4);
const FloatV V10 = FMul(V6, V4);
const FloatV V12 = FMul(V6, V6);
const FloatV V14 = FMul(V8, V6);
const FloatV V16 = FMul(V8, V8);
const FloatV V18 = FMul(V10, V8);
const FloatV V20 = FMul(V10, V10);
const FloatV V22 = FMul(V12, V10);
const Vec4V cosCoefficients0 = V4LoadA(g_PXCosCoefficients0.f);
const Vec4V cosCoefficients1 = V4LoadA(g_PXCosCoefficients1.f);
const Vec4V cosCoefficients2 = V4LoadA(g_PXCosCoefficients2.f);
const FloatV C1 = V4GetY(cosCoefficients0);
const FloatV C2 = V4GetZ(cosCoefficients0);
const FloatV C3 = V4GetW(cosCoefficients0);
const FloatV C4 = V4GetX(cosCoefficients1);
const FloatV C5 = V4GetY(cosCoefficients1);
const FloatV C6 = V4GetZ(cosCoefficients1);
const FloatV C7 = V4GetW(cosCoefficients1);
const FloatV C8 = V4GetX(cosCoefficients2);
const FloatV C9 = V4GetY(cosCoefficients2);
const FloatV C10 = V4GetZ(cosCoefficients2);
const FloatV C11 = V4GetW(cosCoefficients2);
FloatV Result;
Result = FScaleAdd(C1, V2, V4One());
Result = FScaleAdd(C2, V4, Result);
Result = FScaleAdd(C3, V6, Result);
Result = FScaleAdd(C4, V8, Result);
Result = FScaleAdd(C5, V10, Result);
Result = FScaleAdd(C6, V12, Result);
Result = FScaleAdd(C7, V14, Result);
Result = FScaleAdd(C8, V16, Result);
Result = FScaleAdd(C9, V18, Result);
Result = FScaleAdd(C10, V20, Result);
Result = FScaleAdd(C11, V22, Result);
return Result;
}
PX_FORCE_INLINE PxU32 FOutOfBounds(const FloatV a, const FloatV min, const FloatV max)
{
ASSERT_ISVALIDFLOATV(a);
ASSERT_ISVALIDFLOATV(min);
ASSERT_ISVALIDFLOATV(max);
const BoolV c = BOr(FIsGrtr(a, max), FIsGrtr(min, a));
return !BAllEqFFFF(c);
}
PX_FORCE_INLINE PxU32 FInBounds(const FloatV a, const FloatV min, const FloatV max)
{
ASSERT_ISVALIDFLOATV(a);
ASSERT_ISVALIDFLOATV(min);
ASSERT_ISVALIDFLOATV(max)
const BoolV c = BAnd(FIsGrtrOrEq(a, min), FIsGrtrOrEq(max, a));
return BAllEqTTTT(c);
}
PX_FORCE_INLINE PxU32 FOutOfBounds(const FloatV a, const FloatV bounds)
{
ASSERT_ISVALIDFLOATV(a);
ASSERT_ISVALIDFLOATV(bounds);
return FOutOfBounds(a, FNeg(bounds), bounds);
}
PX_FORCE_INLINE PxU32 FInBounds(const FloatV a, const FloatV bounds)
{
ASSERT_ISVALIDFLOATV(a);
ASSERT_ISVALIDFLOATV(bounds);
return FInBounds(a, FNeg(bounds), bounds);
}
//////////////////////////////////
// VEC3V
//////////////////////////////////
PX_FORCE_INLINE Vec3V V3Splat(const FloatV f)
{
ASSERT_ISVALIDFLOATV(f);
const __m128 zero = FZero();
const __m128 fff0 = _mm_move_ss(f, zero);
return _mm_shuffle_ps(fff0, fff0, _MM_SHUFFLE(0, 1, 2, 3));
}
PX_FORCE_INLINE Vec3V V3Merge(const FloatVArg x, const FloatVArg y, const FloatVArg z)
{
ASSERT_ISVALIDFLOATV(x);
ASSERT_ISVALIDFLOATV(y);
ASSERT_ISVALIDFLOATV(z);
// static on zero causes compiler crash on x64 debug_opt
const __m128 zero = FZero();
const __m128 xy = _mm_move_ss(x, y);
const __m128 z0 = _mm_move_ss(zero, z);
return _mm_shuffle_ps(xy, z0, _MM_SHUFFLE(1, 0, 0, 1));
}
PX_FORCE_INLINE Vec3V V3UnitX()
{
const PX_ALIGN(16, PxF32) x[4] = { 1.0f, 0.0f, 0.0f, 0.0f };
const __m128 x128 = _mm_load_ps(x);
return x128;
}
PX_FORCE_INLINE Vec3V V3UnitY()
{
const PX_ALIGN(16, PxF32) y[4] = { 0.0f, 1.0f, 0.0f, 0.0f };
const __m128 y128 = _mm_load_ps(y);
return y128;
}
PX_FORCE_INLINE Vec3V V3UnitZ()
{
const PX_ALIGN(16, PxF32) z[4] = { 0.0f, 0.0f, 1.0f, 0.0f };
const __m128 z128 = _mm_load_ps(z);
return z128;
}
PX_FORCE_INLINE FloatV V3GetX(const Vec3V f)
{
ASSERT_ISVALIDVEC3V(f);
return _mm_shuffle_ps(f, f, _MM_SHUFFLE(0, 0, 0, 0));
}
PX_FORCE_INLINE FloatV V3GetY(const Vec3V f)
{
ASSERT_ISVALIDVEC3V(f)
return _mm_shuffle_ps(f, f, _MM_SHUFFLE(1, 1, 1, 1));
}
PX_FORCE_INLINE FloatV V3GetZ(const Vec3V f)
{
ASSERT_ISVALIDVEC3V(f);
return _mm_shuffle_ps(f, f, _MM_SHUFFLE(2, 2, 2, 2));
}
PX_FORCE_INLINE Vec3V V3SetX(const Vec3V v, const FloatV f)
{
ASSERT_ISVALIDVEC3V(v);
ASSERT_ISVALIDFLOATV(f);
return V4Sel(BFTTT(), v, f);
}