| 1 | // |
|---|
| 2 | // Redistribution and use in source and binary forms, with or without |
|---|
| 3 | // modification, are permitted provided that the following conditions |
|---|
| 4 | // are met: |
|---|
| 5 | // * Redistributions of source code must retain the above copyright |
|---|
| 6 | // notice, this list of conditions and the following disclaimer. |
|---|
| 7 | // * Redistributions in binary form must reproduce the above copyright |
|---|
| 8 | // notice, this list of conditions and the following disclaimer in the |
|---|
| 9 | // documentation and/or other materials provided with the distribution. |
|---|
| 10 | // * Neither the name of NVIDIA CORPORATION nor the names of its |
|---|
| 11 | // contributors may be used to endorse or promote products derived |
|---|
| 12 | // from this software without specific prior written permission. |
|---|
| 13 | // |
|---|
| 14 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY |
|---|
| 15 | // EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
|---|
| 16 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR |
|---|
| 17 | // PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR |
|---|
| 18 | // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, |
|---|
| 19 | // EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, |
|---|
| 20 | // PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR |
|---|
| 21 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY |
|---|
| 22 | // OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT |
|---|
| 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE |
|---|
| 24 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
|---|
| 25 | // |
|---|
| 26 | // Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved. |
|---|
| 27 | // Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved. |
|---|
| 28 | // Copyright (c) 2001-2004 NovodeX AG. All rights reserved. |
|---|
| 29 | |
|---|
| 30 | #ifndef PSFOUNDATION_PSVECMATHSSE_H |
|---|
| 31 | #define PSFOUNDATION_PSVECMATHSSE_H |
|---|
| 32 | |
|---|
| 33 | namespace physx |
|---|
| 34 | { |
|---|
| 35 | namespace shdfnd |
|---|
| 36 | { |
|---|
| 37 | namespace aos |
|---|
| 38 | { |
|---|
| 39 | |
|---|
| 40 | namespace |
|---|
| 41 | { |
|---|
| 42 | const PX_ALIGN(16, PxF32) minus1w[4] = { 0.0f, 0.0f, 0.0f, -1.0f }; |
|---|
| 43 | } |
|---|
| 44 | |
|---|
| 45 | PX_FORCE_INLINE void QuatGetMat33V(const QuatVArg q, Vec3V& column0, Vec3V& column1, Vec3V& column2) |
|---|
| 46 | { |
|---|
| 47 | const __m128 q2 = V4Add(a: q, b: q); |
|---|
| 48 | const __m128 qw2 = V4MulAdd(a: q2, b: V4GetW(f: q), c: _mm_load_ps(p: minus1w)); // (2wx, 2wy, 2wz, 2ww-1) |
|---|
| 49 | const __m128 nw2 = Vec3V_From_Vec4V(v: V4Neg(c: qw2)); // (-2wx, -2wy, -2wz, 0) |
|---|
| 50 | const __m128 v = Vec3V_From_Vec4V(v: q); |
|---|
| 51 | |
|---|
| 52 | const __m128 a0 = _mm_shuffle_ps(qw2, nw2, _MM_SHUFFLE(3, 1, 2, 3)); // (2ww-1, 2wz, -2wy, 0) |
|---|
| 53 | column0 = V4MulAdd(a: v, b: V4GetX(f: q2), c: a0); |
|---|
| 54 | |
|---|
| 55 | const __m128 a1 = _mm_shuffle_ps(qw2, nw2, _MM_SHUFFLE(3, 2, 0, 3)); // (2ww-1, 2wx, -2wz, 0) |
|---|
| 56 | column1 = V4MulAdd(a: v, b: V4GetY(f: q2), _mm_shuffle_ps(a1, a1, _MM_SHUFFLE(3, 1, 0, 2))); |
|---|
| 57 | |
|---|
| 58 | const __m128 a2 = _mm_shuffle_ps(qw2, nw2, _MM_SHUFFLE(3, 0, 1, 3)); // (2ww-1, 2wy, -2wx, 0) |
|---|
| 59 | column2 = V4MulAdd(a: v, b: V4GetZ(f: q2), _mm_shuffle_ps(a2, a2, _MM_SHUFFLE(3, 0, 2, 1))); |
|---|
| 60 | } |
|---|
| 61 | |
|---|
| 62 | } // namespace aos |
|---|
| 63 | } // namespace shdfnd |
|---|
| 64 | } // namespace physx |
|---|
| 65 | |
|---|
| 66 | |
|---|
| 67 | #endif // PSFOUNDATION_PSVECMATHSSE_H |
|---|
| 68 | |
|---|
| 69 | |
|---|
