Jolt Physics
A multi core friendly Game Physics Engine
Loading...
Searching...
No Matches
Mat44.inl
Go to the documentation of this file.
1// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
2// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
3// SPDX-License-Identifier: MIT
4
5#pragma once
6
7#include <Jolt/Math/Vec3.h>
8#include <Jolt/Math/Vec4.h>
9#include <Jolt/Math/Quat.h>
10
11JPH_NAMESPACE_BEGIN
12
13#define JPH_EL(r, c) mCol[c].mF32[r]
14
15Mat44::Mat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, Vec4Arg inC4) :
16 mCol { inC1, inC2, inC3, inC4 }
17{
18}
19
20Mat44::Mat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, Vec3Arg inC4) :
21 mCol { inC1, inC2, inC3, Vec4(inC4, 1.0f) }
22{
23}
24
25Mat44::Mat44(Type inC1, Type inC2, Type inC3, Type inC4) :
26 mCol { inC1, inC2, inC3, inC4 }
27{
28}
29
34
35Mat44 Mat44::sIdentity()
36{
37 return Mat44(Vec4(1, 0, 0, 0), Vec4(0, 1, 0, 0), Vec4(0, 0, 1, 0), Vec4(0, 0, 0, 1));
38}
39
44
45Mat44 Mat44::sLoadFloat4x4(const Float4 *inV)
46{
47 Mat44 result;
48 for (int c = 0; c < 4; ++c)
49 result.mCol[c] = Vec4::sLoadFloat4(inV + c);
50 return result;
51}
52
53Mat44 Mat44::sLoadFloat4x4Aligned(const Float4 *inV)
54{
55 Mat44 result;
56 for (int c = 0; c < 4; ++c)
57 result.mCol[c] = Vec4::sLoadFloat4Aligned(inV + c);
58 return result;
59}
60
61Mat44 Mat44::sRotationX(float inX)
62{
63 Vec4 sv, cv;
64 Vec4::sReplicate(inX).SinCos(sv, cv);
65 float s = sv.GetX(), c = cv.GetX();
66 return Mat44(Vec4(1, 0, 0, 0), Vec4(0, c, s, 0), Vec4(0, -s, c, 0), Vec4(0, 0, 0, 1));
67}
68
69Mat44 Mat44::sRotationY(float inY)
70{
71 Vec4 sv, cv;
72 Vec4::sReplicate(inY).SinCos(sv, cv);
73 float s = sv.GetX(), c = cv.GetX();
74 return Mat44(Vec4(c, 0, -s, 0), Vec4(0, 1, 0, 0), Vec4(s, 0, c, 0), Vec4(0, 0, 0, 1));
75}
76
77Mat44 Mat44::sRotationZ(float inZ)
78{
79 Vec4 sv, cv;
80 Vec4::sReplicate(inZ).SinCos(sv, cv);
81 float s = sv.GetX(), c = cv.GetX();
82 return Mat44(Vec4(c, s, 0, 0), Vec4(-s, c, 0, 0), Vec4(0, 0, 1, 0), Vec4(0, 0, 0, 1));
83}
84
85Mat44 Mat44::sRotation(QuatArg inQuat)
86{
87 JPH_ASSERT(inQuat.IsNormalized());
88
89 // See: https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation section 'Quaternion-derived rotation matrix'
90#ifdef JPH_USE_SSE4_1
91 __m128 xyzw = inQuat.mValue.mValue;
92 __m128 two_xyzw = _mm_add_ps(xyzw, xyzw);
93 __m128 yzxw = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(3, 0, 2, 1));
94 __m128 two_yzxw = _mm_add_ps(yzxw, yzxw);
95 __m128 zxyw = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(3, 1, 0, 2));
96 __m128 two_zxyw = _mm_add_ps(zxyw, zxyw);
97 __m128 wwww = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(3, 3, 3, 3));
98 __m128 diagonal = _mm_sub_ps(_mm_sub_ps(_mm_set1_ps(1.0f), _mm_mul_ps(two_yzxw, yzxw)), _mm_mul_ps(two_zxyw, zxyw)); // (1 - 2 y^2 - 2 z^2, 1 - 2 x^2 - 2 z^2, 1 - 2 x^2 - 2 y^2, 1 - 4 w^2)
99 __m128 plus = _mm_add_ps(_mm_mul_ps(two_xyzw, zxyw), _mm_mul_ps(two_yzxw, wwww)); // 2 * (xz + yw, xy + zw, yz + xw, ww)
100 __m128 minus = _mm_sub_ps(_mm_mul_ps(two_yzxw, xyzw), _mm_mul_ps(two_zxyw, wwww)); // 2 * (xy - zw, yz - xw, xz - yw, 0)
101
102 // Workaround for compiler changing _mm_sub_ps(_mm_mul_ps(...), ...) into a fused multiply sub instruction, resulting in w not being 0
103 // There doesn't appear to be a reliable way to turn this off in Clang
104 minus = _mm_insert_ps(minus, minus, 0b1000);
105
106 __m128 col0 = _mm_blend_ps(_mm_blend_ps(plus, diagonal, 0b0001), minus, 0b1100); // (1 - 2 y^2 - 2 z^2, 2 xy + 2 zw, 2 xz - 2 yw, 0)
107 __m128 col1 = _mm_blend_ps(_mm_blend_ps(diagonal, minus, 0b1001), plus, 0b0100); // (2 xy - 2 zw, 1 - 2 x^2 - 2 z^2, 2 yz + 2 xw, 0)
108 __m128 col2 = _mm_blend_ps(_mm_blend_ps(minus, plus, 0b0001), diagonal, 0b0100); // (2 xz + 2 yw, 2 yz - 2 xw, 1 - 2 x^2 - 2 y^2, 0)
109 __m128 col3 = _mm_set_ps(1, 0, 0, 0);
110
111 return Mat44(col0, col1, col2, col3);
112#else
113 float x = inQuat.GetX();
114 float y = inQuat.GetY();
115 float z = inQuat.GetZ();
116 float w = inQuat.GetW();
117
118 float tx = x + x; // Note: Using x + x instead of 2.0f * x to force this function to return the same value as the SSE4.1 version across platforms.
119 float ty = y + y;
120 float tz = z + z;
121
122 float xx = tx * x;
123 float yy = ty * y;
124 float zz = tz * z;
125 float xy = tx * y;
126 float xz = tx * z;
127 float xw = tx * w;
128 float yz = ty * z;
129 float yw = ty * w;
130 float zw = tz * w;
131
132 return Mat44(Vec4((1.0f - yy) - zz, xy + zw, xz - yw, 0.0f), // Note: Added extra brackets to force this function to return the same value as the SSE4.1 version across platforms.
133 Vec4(xy - zw, (1.0f - zz) - xx, yz + xw, 0.0f),
134 Vec4(xz + yw, yz - xw, (1.0f - xx) - yy, 0.0f),
135 Vec4(0.0f, 0.0f, 0.0f, 1.0f));
136#endif
137}
138
139Mat44 Mat44::sRotation(Vec3Arg inAxis, float inAngle)
140{
141 return sRotation(Quat::sRotation(inAxis, inAngle));
142}
143
144Mat44 Mat44::sTranslation(Vec3Arg inV)
145{
146 return Mat44(Vec4(1, 0, 0, 0), Vec4(0, 1, 0, 0), Vec4(0, 0, 1, 0), Vec4(inV, 1));
147}
148
149Mat44 Mat44::sRotationTranslation(QuatArg inR, Vec3Arg inT)
150{
151 Mat44 m = sRotation(inR);
152 m.SetTranslation(inT);
153 return m;
154}
155
156Mat44 Mat44::sInverseRotationTranslation(QuatArg inR, Vec3Arg inT)
157{
158 Mat44 m = sRotation(inR.Conjugated());
159 m.SetTranslation(-m.Multiply3x3(inT));
160 return m;
161}
162
163Mat44 Mat44::sScale(float inScale)
164{
165 return Mat44(Vec4(inScale, 0, 0, 0), Vec4(0, inScale, 0, 0), Vec4(0, 0, inScale, 0), Vec4(0, 0, 0, 1));
166}
167
168Mat44 Mat44::sScale(Vec3Arg inV)
169{
170 return Mat44(Vec4(inV.GetX(), 0, 0, 0), Vec4(0, inV.GetY(), 0, 0), Vec4(0, 0, inV.GetZ(), 0), Vec4(0, 0, 0, 1));
171}
172
173Mat44 Mat44::sOuterProduct(Vec3Arg inV1, Vec3Arg inV2)
174{
175 Vec4 v1(inV1, 0);
176 return Mat44(v1 * inV2.SplatX(), v1 * inV2.SplatY(), v1 * inV2.SplatZ(), Vec4(0, 0, 0, 1));
177}
178
179Mat44 Mat44::sCrossProduct(Vec3Arg inV)
180{
181#ifdef JPH_USE_SSE4_1
182 // Zero out the W component
183 __m128 zero = _mm_setzero_ps();
184 __m128 v = _mm_blend_ps(inV.mValue, zero, 0b1000);
185
186 // Negate
187 __m128 min_v = _mm_sub_ps(zero, v);
188
189 return Mat44(
190 _mm_shuffle_ps(v, min_v, _MM_SHUFFLE(3, 1, 2, 3)), // [0, z, -y, 0]
191 _mm_shuffle_ps(min_v, v, _MM_SHUFFLE(3, 0, 3, 2)), // [-z, 0, x, 0]
192 _mm_blend_ps(_mm_shuffle_ps(v, v, _MM_SHUFFLE(3, 3, 3, 1)), _mm_shuffle_ps(min_v, min_v, _MM_SHUFFLE(3, 3, 0, 3)), 0b0010), // [y, -x, 0, 0]
193 Vec4(0, 0, 0, 1));
194#else
195 float x = inV.GetX();
196 float y = inV.GetY();
197 float z = inV.GetZ();
198
199 return Mat44(
200 Vec4(0, z, -y, 0),
201 Vec4(-z, 0, x, 0),
202 Vec4(y, -x, 0, 0),
203 Vec4(0, 0, 0, 1));
204#endif
205}
206
207Mat44 Mat44::sLookAt(Vec3Arg inPos, Vec3Arg inTarget, Vec3Arg inUp)
208{
209 Vec3 direction = (inTarget - inPos).NormalizedOr(-Vec3::sAxisZ());
210 Vec3 right = direction.Cross(inUp).NormalizedOr(Vec3::sAxisX());
211 Vec3 up = right.Cross(direction);
212
213 return Mat44(Vec4(right, 0), Vec4(up, 0), Vec4(-direction, 0), Vec4(inPos, 1)).InversedRotationTranslation();
214}
215
216Mat44 Mat44::sPerspective(float inFovY, float inAspect, float inNear, float inFar)
217{
218 float height = 1.0f / Tan(0.5f * inFovY);
219 float width = height / inAspect;
220 float range = inFar / (inNear - inFar);
221
222 return Mat44(Vec4(width, 0.0f, 0.0f, 0.0f), Vec4(0.0f, height, 0.0f, 0.0f), Vec4(0.0f, 0.0f, range, -1.0f), Vec4(0.0f, 0.0f, range * inNear, 0.0f));
223}
224
225bool Mat44::operator == (Mat44Arg inM2) const
226{
227 return UVec4::sAnd(
228 UVec4::sAnd(Vec4::sEquals(mCol[0], inM2.mCol[0]), Vec4::sEquals(mCol[1], inM2.mCol[1])),
229 UVec4::sAnd(Vec4::sEquals(mCol[2], inM2.mCol[2]), Vec4::sEquals(mCol[3], inM2.mCol[3]))
230 ).TestAllTrue();
231}
232
233bool Mat44::IsClose(Mat44Arg inM2, float inMaxDistSq) const
234{
235 for (int i = 0; i < 4; ++i)
236 if (!mCol[i].IsClose(inM2.mCol[i], inMaxDistSq))
237 return false;
238 return true;
239}
240
241Mat44 Mat44::operator * (Mat44Arg inM) const
242{
243 Mat44 result;
244#if defined(JPH_USE_SSE)
245 for (int i = 0; i < 4; ++i)
246 {
247 __m128 c = inM.mCol[i].mValue;
248 __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(0, 0, 0, 0)));
249 t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(1, 1, 1, 1))));
250 t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(2, 2, 2, 2))));
251 t = _mm_add_ps(t, _mm_mul_ps(mCol[3].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(3, 3, 3, 3))));
252 result.mCol[i].mValue = t;
253 }
254#elif defined(JPH_USE_NEON)
255 for (int i = 0; i < 4; ++i)
256 {
257 Type c = inM.mCol[i].mValue;
258 Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(c, 0));
259 t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(c, 1));
260 t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(c, 2));
261 t = vmlaq_f32(t, mCol[3].mValue, vdupq_laneq_f32(c, 3));
262 result.mCol[i].mValue = t;
263 }
264#elif defined(JPH_USE_RVV)
265 for (int i = 0; i < 4; ++i)
266 {
267 const float *c = inM.mCol[i].mF32;
268 const vfloat32m1_t rep_0 = __riscv_vfmv_v_f_f32m1(c[0], 4);
269 const vfloat32m1_t rep_1 = __riscv_vfmv_v_f_f32m1(c[1], 4);
270 const vfloat32m1_t rep_2 = __riscv_vfmv_v_f_f32m1(c[2], 4);
271 const vfloat32m1_t rep_3 = __riscv_vfmv_v_f_f32m1(c[3], 4);
272
273 const vfloat32m1_t col0 = __riscv_vle32_v_f32m1(mCol[0].mF32, 4);
274 const vfloat32m1_t col1 = __riscv_vle32_v_f32m1(mCol[1].mF32, 4);
275 const vfloat32m1_t col2 = __riscv_vle32_v_f32m1(mCol[2].mF32, 4);
276 const vfloat32m1_t col3 = __riscv_vle32_v_f32m1(mCol[3].mF32, 4);
277
278 const vfloat32m1_t mul1 = __riscv_vfmul_vv_f32m1(col1, rep_1, 4);
279 const vfloat32m1_t mul2 = __riscv_vfmul_vv_f32m1(col2, rep_2, 4);
280 const vfloat32m1_t mul3 = __riscv_vfmul_vv_f32m1(col3, rep_3, 4);
281
282 vfloat32m1_t t = __riscv_vfmul_vv_f32m1(col0, rep_0, 4);
283 t = __riscv_vfadd_vv_f32m1(t, mul1, 4);
284 t = __riscv_vfadd_vv_f32m1(t, mul2, 4);
285 t = __riscv_vfadd_vv_f32m1(t, mul3, 4);
286 __riscv_vse32_v_f32m1(result.mCol[i].mF32, t, 4);
287 }
288#else
289 for (int i = 0; i < 4; ++i)
290 result.mCol[i] = mCol[0] * inM.mCol[i].mF32[0] + mCol[1] * inM.mCol[i].mF32[1] + mCol[2] * inM.mCol[i].mF32[2] + mCol[3] * inM.mCol[i].mF32[3];
291#endif
292 return result;
293}
294
295Vec3 Mat44::operator * (Vec3Arg inV) const
296{
297#if defined(JPH_USE_SSE)
298 __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(0, 0, 0, 0)));
299 t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(1, 1, 1, 1))));
300 t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(2, 2, 2, 2))));
301 t = _mm_add_ps(t, mCol[3].mValue);
302 return Vec3::sFixW(t);
303#elif defined(JPH_USE_NEON)
304 Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(inV.mValue, 0));
305 t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(inV.mValue, 1));
306 t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(inV.mValue, 2));
307 t = vaddq_f32(t, mCol[3].mValue); // Don't combine this with the first mul into a fused multiply add, causes precision issues
308 return Vec3::sFixW(t);
309#elif defined(JPH_USE_RVV)
310 const vfloat32m1_t v0 = __riscv_vfmv_v_f_f32m1(inV.mF32[0], 4);
311 const vfloat32m1_t v1 = __riscv_vfmv_v_f_f32m1(inV.mF32[1], 4);
312 const vfloat32m1_t v2 = __riscv_vfmv_v_f_f32m1(inV.mF32[2], 4);
313
314 const vfloat32m1_t col0 = __riscv_vle32_v_f32m1(mCol[0].mF32, 4);
315 const vfloat32m1_t col1 = __riscv_vle32_v_f32m1(mCol[1].mF32, 4);
316 const vfloat32m1_t col2 = __riscv_vle32_v_f32m1(mCol[2].mF32, 4);
317 const vfloat32m1_t col3 = __riscv_vle32_v_f32m1(mCol[3].mF32, 4);
318
319 const vfloat32m1_t mul1 = __riscv_vfmul_vv_f32m1(col1, v1, 4);
320 const vfloat32m1_t mul2 = __riscv_vfmul_vv_f32m1(col2, v2, 4);
321
322 vfloat32m1_t t = __riscv_vfmul_vv_f32m1(col0, v0, 4);
323 t = __riscv_vfadd_vv_f32m1(t, mul1, 4);
324 t = __riscv_vfadd_vv_f32m1(t, mul2, 4);
325 t = __riscv_vfadd_vv_f32m1(t, col3, 4);
326
327 Type v;
328 __riscv_vse32_v_f32m1(v.mData, t, 4);
329 return Vec3::sFixW(v);
330#else
331 return Vec3(
332 mCol[0].mF32[0] * inV.mF32[0] + mCol[1].mF32[0] * inV.mF32[1] + mCol[2].mF32[0] * inV.mF32[2] + mCol[3].mF32[0],
333 mCol[0].mF32[1] * inV.mF32[0] + mCol[1].mF32[1] * inV.mF32[1] + mCol[2].mF32[1] * inV.mF32[2] + mCol[3].mF32[1],
334 mCol[0].mF32[2] * inV.mF32[0] + mCol[1].mF32[2] * inV.mF32[1] + mCol[2].mF32[2] * inV.mF32[2] + mCol[3].mF32[2]);
335#endif
336}
337
338Vec4 Mat44::operator * (Vec4Arg inV) const
339{
340#if defined(JPH_USE_SSE)
341 __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(0, 0, 0, 0)));
342 t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(1, 1, 1, 1))));
343 t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(2, 2, 2, 2))));
344 t = _mm_add_ps(t, _mm_mul_ps(mCol[3].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(3, 3, 3, 3))));
345 return t;
346#elif defined(JPH_USE_NEON)
347 Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(inV.mValue, 0));
348 t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(inV.mValue, 1));
349 t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(inV.mValue, 2));
350 t = vmlaq_f32(t, mCol[3].mValue, vdupq_laneq_f32(inV.mValue, 3));
351 return t;
352#elif defined(JPH_USE_RVV)
353 const vfloat32m1_t v0 = __riscv_vfmv_v_f_f32m1(inV.mF32[0], 4);
354 const vfloat32m1_t v1 = __riscv_vfmv_v_f_f32m1(inV.mF32[1], 4);
355 const vfloat32m1_t v2 = __riscv_vfmv_v_f_f32m1(inV.mF32[2], 4);
356 const vfloat32m1_t v3 = __riscv_vfmv_v_f_f32m1(inV.mF32[3], 4);
357
358 const vfloat32m1_t col0 = __riscv_vle32_v_f32m1(mCol[0].mF32, 4);
359 const vfloat32m1_t col1 = __riscv_vle32_v_f32m1(mCol[1].mF32, 4);
360 const vfloat32m1_t col2 = __riscv_vle32_v_f32m1(mCol[2].mF32, 4);
361 const vfloat32m1_t col3 = __riscv_vle32_v_f32m1(mCol[3].mF32, 4);
362
363 const vfloat32m1_t mul1 = __riscv_vfmul_vv_f32m1(col1, v1, 4);
364 const vfloat32m1_t mul2 = __riscv_vfmul_vv_f32m1(col2, v2, 4);
365 const vfloat32m1_t mul3 = __riscv_vfmul_vv_f32m1(col3, v3, 4);
366
367 vfloat32m1_t t = __riscv_vfmul_vv_f32m1(col0, v0, 4);
368 t = __riscv_vfadd_vv_f32m1(t, mul1, 4);
369 t = __riscv_vfadd_vv_f32m1(t, mul2, 4);
370 t = __riscv_vfadd_vv_f32m1(t, mul3, 4);
371
372 Vec4 v;
373 __riscv_vse32_v_f32m1(v.mF32, t, 4);
374 return v;
375#else
376 return Vec4(
377 mCol[0].mF32[0] * inV.mF32[0] + mCol[1].mF32[0] * inV.mF32[1] + mCol[2].mF32[0] * inV.mF32[2] + mCol[3].mF32[0] * inV.mF32[3],
378 mCol[0].mF32[1] * inV.mF32[0] + mCol[1].mF32[1] * inV.mF32[1] + mCol[2].mF32[1] * inV.mF32[2] + mCol[3].mF32[1] * inV.mF32[3],
379 mCol[0].mF32[2] * inV.mF32[0] + mCol[1].mF32[2] * inV.mF32[1] + mCol[2].mF32[2] * inV.mF32[2] + mCol[3].mF32[2] * inV.mF32[3],
380 mCol[0].mF32[3] * inV.mF32[0] + mCol[1].mF32[3] * inV.mF32[1] + mCol[2].mF32[3] * inV.mF32[2] + mCol[3].mF32[3] * inV.mF32[3]);
381#endif
382}
383
384Vec3 Mat44::Multiply3x3(Vec3Arg inV) const
385{
386#if defined(JPH_USE_SSE)
387 __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(0, 0, 0, 0)));
388 t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(1, 1, 1, 1))));
389 t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(2, 2, 2, 2))));
390 return Vec3::sFixW(t);
391#elif defined(JPH_USE_NEON)
392 Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(inV.mValue, 0));
393 t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(inV.mValue, 1));
394 t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(inV.mValue, 2));
395 return Vec3::sFixW(t);
396#elif defined(JPH_USE_RVV)
397 const vfloat32m1_t v0 = __riscv_vfmv_v_f_f32m1(inV.mF32[0], 4);
398 const vfloat32m1_t v1 = __riscv_vfmv_v_f_f32m1(inV.mF32[1], 4);
399 const vfloat32m1_t v2 = __riscv_vfmv_v_f_f32m1(inV.mF32[2], 4);
400
401 const vfloat32m1_t col0 = __riscv_vle32_v_f32m1(mCol[0].mF32, 4);
402 const vfloat32m1_t col1 = __riscv_vle32_v_f32m1(mCol[1].mF32, 4);
403 const vfloat32m1_t col2 = __riscv_vle32_v_f32m1(mCol[2].mF32, 4);
404
405 const vfloat32m1_t mul1 = __riscv_vfmul_vv_f32m1(v1, col1, 4);
406 const vfloat32m1_t mul2 = __riscv_vfmul_vv_f32m1(v2, col2, 4);
407
408 vfloat32m1_t t = __riscv_vfmul_vv_f32m1(v0, col0, 4);
409 t = __riscv_vfadd_vv_f32m1(t, mul1, 4);
410 t = __riscv_vfadd_vv_f32m1(t, mul2, 4);
411
412 Type v;
413 __riscv_vse32_v_f32m1(v.mData, t, 4);
414 return Vec3::sFixW(v);
415#else
416 return Vec3(
417 mCol[0].mF32[0] * inV.mF32[0] + mCol[1].mF32[0] * inV.mF32[1] + mCol[2].mF32[0] * inV.mF32[2],
418 mCol[0].mF32[1] * inV.mF32[0] + mCol[1].mF32[1] * inV.mF32[1] + mCol[2].mF32[1] * inV.mF32[2],
419 mCol[0].mF32[2] * inV.mF32[0] + mCol[1].mF32[2] * inV.mF32[1] + mCol[2].mF32[2] * inV.mF32[2]);
420#endif
421}
422
423Vec3 Mat44::Multiply3x3Transposed(Vec3Arg inV) const
424{
425#if defined(JPH_USE_SSE4_1)
426 __m128 x = _mm_dp_ps(mCol[0].mValue, inV.mValue, 0x7f);
427 __m128 y = _mm_dp_ps(mCol[1].mValue, inV.mValue, 0x7f);
428 __m128 xy = _mm_blend_ps(x, y, 0b0010);
429 __m128 z = _mm_dp_ps(mCol[2].mValue, inV.mValue, 0x7f);
430 __m128 xyzz = _mm_blend_ps(xy, z, 0b1100);
431 return xyzz;
432#else
433 return Transposed3x3().Multiply3x3(inV);
434#endif
435}
436
437Mat44 Mat44::Multiply3x3(Mat44Arg inM) const
438{
439 JPH_ASSERT(mCol[0][3] == 0.0f);
440 JPH_ASSERT(mCol[1][3] == 0.0f);
441 JPH_ASSERT(mCol[2][3] == 0.0f);
442
443 Mat44 result;
444#if defined(JPH_USE_SSE)
445 for (int i = 0; i < 3; ++i)
446 {
447 __m128 c = inM.mCol[i].mValue;
448 __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(0, 0, 0, 0)));
449 t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(1, 1, 1, 1))));
450 t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(2, 2, 2, 2))));
451 result.mCol[i].mValue = t;
452 }
453#elif defined(JPH_USE_NEON)
454 for (int i = 0; i < 3; ++i)
455 {
456 Type c = inM.mCol[i].mValue;
457 Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(c, 0));
458 t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(c, 1));
459 t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(c, 2));
460 result.mCol[i].mValue = t;
461 }
462#elif defined(JPH_USE_RVV)
463 for (int i = 0; i < 3; ++i)
464 {
465 const float* col_i = inM.mCol[i].mF32;
466 const vfloat32m1_t v0 = __riscv_vfmv_v_f_f32m1(col_i[0], 4);
467 const vfloat32m1_t v1 = __riscv_vfmv_v_f_f32m1(col_i[1], 4);
468 const vfloat32m1_t v2 = __riscv_vfmv_v_f_f32m1(col_i[2], 4);
469
470 const vfloat32m1_t col0 = __riscv_vle32_v_f32m1(mCol[0].mF32, 4);
471 const vfloat32m1_t col1 = __riscv_vle32_v_f32m1(mCol[1].mF32, 4);
472 const vfloat32m1_t col2 = __riscv_vle32_v_f32m1(mCol[2].mF32, 4);
473
474 const vfloat32m1_t mul1 = __riscv_vfmul_vv_f32m1(v1, col1, 4);
475 const vfloat32m1_t mul2 = __riscv_vfmul_vv_f32m1(v2, col2, 4);
476
477 vfloat32m1_t t = __riscv_vfmul_vv_f32m1(v0, col0, 4);
478 t = __riscv_vfadd_vv_f32m1(t, mul1, 4);
479 t = __riscv_vfadd_vv_f32m1(t, mul2, 4);
480 __riscv_vse32_v_f32m1(result.mCol[i].mF32, t, 4);
481 }
482#else
483 for (int i = 0; i < 3; ++i)
484 result.mCol[i] = mCol[0] * inM.mCol[i].mF32[0] + mCol[1] * inM.mCol[i].mF32[1] + mCol[2] * inM.mCol[i].mF32[2];
485#endif
486 result.mCol[3] = Vec4(0, 0, 0, 1);
487 return result;
488}
489
490Mat44 Mat44::Multiply3x3LeftTransposed(Mat44Arg inM) const
491{
492 // Transpose left hand side
493 Mat44 trans = Transposed3x3();
494
495 // Do 3x3 matrix multiply
496 Mat44 result;
497 result.mCol[0] = trans.mCol[0] * inM.mCol[0].SplatX() + trans.mCol[1] * inM.mCol[0].SplatY() + trans.mCol[2] * inM.mCol[0].SplatZ();
498 result.mCol[1] = trans.mCol[0] * inM.mCol[1].SplatX() + trans.mCol[1] * inM.mCol[1].SplatY() + trans.mCol[2] * inM.mCol[1].SplatZ();
499 result.mCol[2] = trans.mCol[0] * inM.mCol[2].SplatX() + trans.mCol[1] * inM.mCol[2].SplatY() + trans.mCol[2] * inM.mCol[2].SplatZ();
500 result.mCol[3] = Vec4(0, 0, 0, 1);
501 return result;
502}
503
504Mat44 Mat44::Multiply3x3RightTransposed(Mat44Arg inM) const
505{
506 JPH_ASSERT(mCol[0][3] == 0.0f);
507 JPH_ASSERT(mCol[1][3] == 0.0f);
508 JPH_ASSERT(mCol[2][3] == 0.0f);
509
510 Mat44 result;
511 result.mCol[0] = mCol[0] * inM.mCol[0].SplatX() + mCol[1] * inM.mCol[1].SplatX() + mCol[2] * inM.mCol[2].SplatX();
512 result.mCol[1] = mCol[0] * inM.mCol[0].SplatY() + mCol[1] * inM.mCol[1].SplatY() + mCol[2] * inM.mCol[2].SplatY();
513 result.mCol[2] = mCol[0] * inM.mCol[0].SplatZ() + mCol[1] * inM.mCol[1].SplatZ() + mCol[2] * inM.mCol[2].SplatZ();
514 result.mCol[3] = Vec4(0, 0, 0, 1);
515 return result;
516}
517
518Mat44 Mat44::operator * (float inV) const
519{
520 Vec4 multiplier = Vec4::sReplicate(inV);
521
522 Mat44 result;
523 for (int c = 0; c < 4; ++c)
524 result.mCol[c] = mCol[c] * multiplier;
525 return result;
526}
527
528Mat44 &Mat44::operator *= (float inV)
529{
530 for (int c = 0; c < 4; ++c)
531 mCol[c] *= inV;
532
533 return *this;
534}
535
536Mat44 Mat44::operator + (Mat44Arg inM) const
537{
538 Mat44 result;
539 for (int i = 0; i < 4; ++i)
540 result.mCol[i] = mCol[i] + inM.mCol[i];
541 return result;
542}
543
544Mat44 Mat44::operator - () const
545{
546 Mat44 result;
547 for (int i = 0; i < 4; ++i)
548 result.mCol[i] = -mCol[i];
549 return result;
550}
551
552Mat44 Mat44::operator - (Mat44Arg inM) const
553{
554 Mat44 result;
555 for (int i = 0; i < 4; ++i)
556 result.mCol[i] = mCol[i] - inM.mCol[i];
557 return result;
558}
559
560Mat44 &Mat44::operator += (Mat44Arg inM)
561{
562 for (int c = 0; c < 4; ++c)
563 mCol[c] += inM.mCol[c];
564
565 return *this;
566}
567
568void Mat44::StoreFloat4x4(Float4 *outV) const
569{
570 for (int c = 0; c < 4; ++c)
571 mCol[c].StoreFloat4(outV + c);
572}
573
574Mat44 Mat44::Transposed() const
575{
576#if defined(JPH_USE_SSE)
577 __m128 tmp1 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(1, 0, 1, 0));
578 __m128 tmp3 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(3, 2, 3, 2));
579 __m128 tmp2 = _mm_shuffle_ps(mCol[2].mValue, mCol[3].mValue, _MM_SHUFFLE(1, 0, 1, 0));
580 __m128 tmp4 = _mm_shuffle_ps(mCol[2].mValue, mCol[3].mValue, _MM_SHUFFLE(3, 2, 3, 2));
581
582 Mat44 result;
583 result.mCol[0].mValue = _mm_shuffle_ps(tmp1, tmp2, _MM_SHUFFLE(2, 0, 2, 0));
584 result.mCol[1].mValue = _mm_shuffle_ps(tmp1, tmp2, _MM_SHUFFLE(3, 1, 3, 1));
585 result.mCol[2].mValue = _mm_shuffle_ps(tmp3, tmp4, _MM_SHUFFLE(2, 0, 2, 0));
586 result.mCol[3].mValue = _mm_shuffle_ps(tmp3, tmp4, _MM_SHUFFLE(3, 1, 3, 1));
587 return result;
588#elif defined(JPH_USE_NEON)
589 float32x4x2_t tmp1 = vzipq_f32(mCol[0].mValue, mCol[2].mValue);
590 float32x4x2_t tmp2 = vzipq_f32(mCol[1].mValue, mCol[3].mValue);
591 float32x4x2_t tmp3 = vzipq_f32(tmp1.val[0], tmp2.val[0]);
592 float32x4x2_t tmp4 = vzipq_f32(tmp1.val[1], tmp2.val[1]);
593
594 Mat44 result;
595 result.mCol[0].mValue = tmp3.val[0];
596 result.mCol[1].mValue = tmp3.val[1];
597 result.mCol[2].mValue = tmp4.val[0];
598 result.mCol[3].mValue = tmp4.val[1];
599 return result;
600#elif defined(JPH_USE_RVV)
601 const vfloat32m1_t row0 = __riscv_vlse32_v_f32m1(&mCol[0].mF32[0], sizeof(Vec4), 4);
602 const vfloat32m1_t row1 = __riscv_vlse32_v_f32m1(&mCol[0].mF32[1], sizeof(Vec4), 4);
603 const vfloat32m1_t row2 = __riscv_vlse32_v_f32m1(&mCol[0].mF32[2], sizeof(Vec4), 4);
604 const vfloat32m1_t row3 = __riscv_vlse32_v_f32m1(&mCol[0].mF32[3], sizeof(Vec4), 4);
605
606 Mat44 result;
607 __riscv_vse32_v_f32m1(result.mCol[0].mF32, row0, 4);
608 __riscv_vse32_v_f32m1(result.mCol[1].mF32, row1, 4);
609 __riscv_vse32_v_f32m1(result.mCol[2].mF32, row2, 4);
610 __riscv_vse32_v_f32m1(result.mCol[3].mF32, row3, 4);
611 return result;
612#else
613 Mat44 result;
614 for (int c = 0; c < 4; ++c)
615 for (int r = 0; r < 4; ++r)
616 result.mCol[r].mF32[c] = mCol[c].mF32[r];
617 return result;
618#endif
619}
620
621Mat44 Mat44::Transposed3x3() const
622{
623#if defined(JPH_USE_SSE)
624 __m128 zero = _mm_setzero_ps();
625 __m128 tmp1 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(1, 0, 1, 0));
626 __m128 tmp3 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(3, 2, 3, 2));
627 __m128 tmp2 = _mm_shuffle_ps(mCol[2].mValue, zero, _MM_SHUFFLE(1, 0, 1, 0));
628 __m128 tmp4 = _mm_shuffle_ps(mCol[2].mValue, zero, _MM_SHUFFLE(3, 2, 3, 2));
629
630 Mat44 result;
631 result.mCol[0].mValue = _mm_shuffle_ps(tmp1, tmp2, _MM_SHUFFLE(2, 0, 2, 0));
632 result.mCol[1].mValue = _mm_shuffle_ps(tmp1, tmp2, _MM_SHUFFLE(3, 1, 3, 1));
633 result.mCol[2].mValue = _mm_shuffle_ps(tmp3, tmp4, _MM_SHUFFLE(2, 0, 2, 0));
634#elif defined(JPH_USE_NEON)
635 float32x4x2_t tmp1 = vzipq_f32(mCol[0].mValue, mCol[2].mValue);
636 float32x4x2_t tmp2 = vzipq_f32(mCol[1].mValue, vdupq_n_f32(0));
637 float32x4x2_t tmp3 = vzipq_f32(tmp1.val[0], tmp2.val[0]);
638 float32x4x2_t tmp4 = vzipq_f32(tmp1.val[1], tmp2.val[1]);
639
640 Mat44 result;
641 result.mCol[0].mValue = tmp3.val[0];
642 result.mCol[1].mValue = tmp3.val[1];
643 result.mCol[2].mValue = tmp4.val[0];
644#elif defined(JPH_USE_RVV)
645 const float end_col[4] = { 0, 0, 0, 1 };
646 const vfloat32m1_t rvv_end_col = __riscv_vle32_v_f32m1(end_col, 4);
647 const vfloat32m1_t rvv_end_row = __riscv_vfmv_v_f_f32m1(0.0f, 3);
648
649 const vfloat32m1_t row0 = __riscv_vlse32_v_f32m1(&mCol[0].mF32[0], sizeof(Vec4), 3);
650 const vfloat32m1_t row1 = __riscv_vlse32_v_f32m1(&mCol[0].mF32[1], sizeof(Vec4), 3);
651 const vfloat32m1_t row2 = __riscv_vlse32_v_f32m1(&mCol[0].mF32[2], sizeof(Vec4), 3);
652
653 Mat44 result;
654 __riscv_vse32_v_f32m1(result.mCol[0].mF32, row0, 3);
655 __riscv_vse32_v_f32m1(result.mCol[1].mF32, row1, 3);
656 __riscv_vse32_v_f32m1(result.mCol[2].mF32, row2, 3);
657 __riscv_vse32_v_f32m1(result.mCol[3].mF32, rvv_end_col, 4);
658 __riscv_vsse32_v_f32m1(&result.mCol[0].mF32[3], sizeof(Vec4), rvv_end_row, 3);
659 return result;
660#else
661 Mat44 result;
662 for (int c = 0; c < 3; ++c)
663 {
664 for (int r = 0; r < 3; ++r)
665 result.mCol[c].mF32[r] = mCol[r].mF32[c];
666 result.mCol[c].mF32[3] = 0;
667 }
668#endif
669 result.mCol[3] = Vec4(0, 0, 0, 1);
670 return result;
671}
672
673Mat44 Mat44::Inversed() const
674{
675#if defined(JPH_USE_SSE)
676 // Algorithm from: http://download.intel.com/design/PentiumIII/sml/24504301.pdf
677 // Streaming SIMD Extensions - Inverse of 4x4 Matrix
678 // Adapted to load data using _mm_shuffle_ps instead of loading from memory
679 // Replaced _mm_rcp_ps with _mm_div_ps for better accuracy
680
681 __m128 tmp1 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(1, 0, 1, 0));
682 __m128 row1 = _mm_shuffle_ps(mCol[2].mValue, mCol[3].mValue, _MM_SHUFFLE(1, 0, 1, 0));
683 __m128 row0 = _mm_shuffle_ps(tmp1, row1, _MM_SHUFFLE(2, 0, 2, 0));
684 row1 = _mm_shuffle_ps(row1, tmp1, _MM_SHUFFLE(3, 1, 3, 1));
685 tmp1 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(3, 2, 3, 2));
686 __m128 row3 = _mm_shuffle_ps(mCol[2].mValue, mCol[3].mValue, _MM_SHUFFLE(3, 2, 3, 2));
687 __m128 row2 = _mm_shuffle_ps(tmp1, row3, _MM_SHUFFLE(2, 0, 2, 0));
688 row3 = _mm_shuffle_ps(row3, tmp1, _MM_SHUFFLE(3, 1, 3, 1));
689
690 tmp1 = _mm_mul_ps(row2, row3);
691 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
692 __m128 minor0 = _mm_mul_ps(row1, tmp1);
693 __m128 minor1 = _mm_mul_ps(row0, tmp1);
694 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
695 minor0 = _mm_sub_ps(_mm_mul_ps(row1, tmp1), minor0);
696 minor1 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor1);
697 minor1 = _mm_shuffle_ps(minor1, minor1, _MM_SHUFFLE(1, 0, 3, 2));
698
699 tmp1 = _mm_mul_ps(row1, row2);
700 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
701 minor0 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor0);
702 __m128 minor3 = _mm_mul_ps(row0, tmp1);
703 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
704 minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row3, tmp1));
705 minor3 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor3);
706 minor3 = _mm_shuffle_ps(minor3, minor3, _MM_SHUFFLE(1, 0, 3, 2));
707
708 tmp1 = _mm_mul_ps(_mm_shuffle_ps(row1, row1, _MM_SHUFFLE(1, 0, 3, 2)), row3);
709 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
710 row2 = _mm_shuffle_ps(row2, row2, _MM_SHUFFLE(1, 0, 3, 2));
711 minor0 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor0);
712 __m128 minor2 = _mm_mul_ps(row0, tmp1);
713 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
714 minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row2, tmp1));
715 minor2 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor2);
716 minor2 = _mm_shuffle_ps(minor2, minor2, _MM_SHUFFLE(1, 0, 3, 2));
717
718 tmp1 = _mm_mul_ps(row0, row1);
719 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
720 minor2 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor2);
721 minor3 = _mm_sub_ps(_mm_mul_ps(row2, tmp1), minor3);
722 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
723 minor2 = _mm_sub_ps(_mm_mul_ps(row3, tmp1), minor2);
724 minor3 = _mm_sub_ps(minor3, _mm_mul_ps(row2, tmp1));
725
726 tmp1 = _mm_mul_ps(row0, row3);
727 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
728 minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row2, tmp1));
729 minor2 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor2);
730 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
731 minor1 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor1);
732 minor2 = _mm_sub_ps(minor2, _mm_mul_ps(row1, tmp1));
733
734 tmp1 = _mm_mul_ps(row0, row2);
735 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
736 minor1 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor1);
737 minor3 = _mm_sub_ps(minor3, _mm_mul_ps(row1, tmp1));
738 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
739 minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row3, tmp1));
740 minor3 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor3);
741
742 __m128 det = _mm_mul_ps(row0, minor0);
743 det = _mm_add_ps(_mm_shuffle_ps(det, det, _MM_SHUFFLE(2, 3, 0, 1)), det); // Original code did (x + z) + (y + w), changed to (x + y) + (z + w) to match the ARM code below and make the result cross platform deterministic
744 det = _mm_add_ss(_mm_shuffle_ps(det, det, _MM_SHUFFLE(1, 0, 3, 2)), det);
745 det = _mm_div_ss(_mm_set_ss(1.0f), det);
746 det = _mm_shuffle_ps(det, det, _MM_SHUFFLE(0, 0, 0, 0));
747
748 Mat44 result;
749 result.mCol[0].mValue = _mm_mul_ps(det, minor0);
750 result.mCol[1].mValue = _mm_mul_ps(det, minor1);
751 result.mCol[2].mValue = _mm_mul_ps(det, minor2);
752 result.mCol[3].mValue = _mm_mul_ps(det, minor3);
753 return result;
754#elif defined(JPH_USE_NEON)
755 // Adapted from the SSE version, there's surprising few articles about efficient ways of calculating an inverse for ARM on the internet
756 Type tmp1 = JPH_NEON_SHUFFLE_F32x4(mCol[0].mValue, mCol[1].mValue, 0, 1, 4, 5);
757 Type row1 = JPH_NEON_SHUFFLE_F32x4(mCol[2].mValue, mCol[3].mValue, 0, 1, 4, 5);
758 Type row0 = JPH_NEON_SHUFFLE_F32x4(tmp1, row1, 0, 2, 4, 6);
759 row1 = JPH_NEON_SHUFFLE_F32x4(row1, tmp1, 1, 3, 5, 7);
760 tmp1 = JPH_NEON_SHUFFLE_F32x4(mCol[0].mValue, mCol[1].mValue, 2, 3, 6, 7);
761 Type row3 = JPH_NEON_SHUFFLE_F32x4(mCol[2].mValue, mCol[3].mValue, 2, 3, 6, 7);
762 Type row2 = JPH_NEON_SHUFFLE_F32x4(tmp1, row3, 0, 2, 4, 6);
763 row3 = JPH_NEON_SHUFFLE_F32x4(row3, tmp1, 1, 3, 5, 7);
764
765 tmp1 = vmulq_f32(row2, row3);
766 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
767 Type minor0 = vmulq_f32(row1, tmp1);
768 Type minor1 = vmulq_f32(row0, tmp1);
769 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
770 minor0 = vsubq_f32(vmulq_f32(row1, tmp1), minor0);
771 minor1 = vsubq_f32(vmulq_f32(row0, tmp1), minor1);
772 minor1 = JPH_NEON_SHUFFLE_F32x4(minor1, minor1, 2, 3, 0, 1);
773
774 tmp1 = vmulq_f32(row1, row2);
775 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
776 minor0 = vaddq_f32(vmulq_f32(row3, tmp1), minor0);
777 Type minor3 = vmulq_f32(row0, tmp1);
778 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
779 minor0 = vsubq_f32(minor0, vmulq_f32(row3, tmp1));
780 minor3 = vsubq_f32(vmulq_f32(row0, tmp1), minor3);
781 minor3 = JPH_NEON_SHUFFLE_F32x4(minor3, minor3, 2, 3, 0, 1);
782
783 tmp1 = JPH_NEON_SHUFFLE_F32x4(row1, row1, 2, 3, 0, 1);
784 tmp1 = vmulq_f32(tmp1, row3);
785 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
786 row2 = JPH_NEON_SHUFFLE_F32x4(row2, row2, 2, 3, 0, 1);
787 minor0 = vaddq_f32(vmulq_f32(row2, tmp1), minor0);
788 Type minor2 = vmulq_f32(row0, tmp1);
789 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
790 minor0 = vsubq_f32(minor0, vmulq_f32(row2, tmp1));
791 minor2 = vsubq_f32(vmulq_f32(row0, tmp1), minor2);
792 minor2 = JPH_NEON_SHUFFLE_F32x4(minor2, minor2, 2, 3, 0, 1);
793
794 tmp1 = vmulq_f32(row0, row1);
795 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
796 minor2 = vaddq_f32(vmulq_f32(row3, tmp1), minor2);
797 minor3 = vsubq_f32(vmulq_f32(row2, tmp1), minor3);
798 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
799 minor2 = vsubq_f32(vmulq_f32(row3, tmp1), minor2);
800 minor3 = vsubq_f32(minor3, vmulq_f32(row2, tmp1));
801
802 tmp1 = vmulq_f32(row0, row3);
803 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
804 minor1 = vsubq_f32(minor1, vmulq_f32(row2, tmp1));
805 minor2 = vaddq_f32(vmulq_f32(row1, tmp1), minor2);
806 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
807 minor1 = vaddq_f32(vmulq_f32(row2, tmp1), minor1);
808 minor2 = vsubq_f32(minor2, vmulq_f32(row1, tmp1));
809
810 tmp1 = vmulq_f32(row0, row2);
811 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
812 minor1 = vaddq_f32(vmulq_f32(row3, tmp1), minor1);
813 minor3 = vsubq_f32(minor3, vmulq_f32(row1, tmp1));
814 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
815 minor1 = vsubq_f32(minor1, vmulq_f32(row3, tmp1));
816 minor3 = vaddq_f32(vmulq_f32(row1, tmp1), minor3);
817
818 Type det = vmulq_f32(row0, minor0);
819 det = vdupq_n_f32(vaddvq_f32(det));
820 det = vdivq_f32(vdupq_n_f32(1.0f), det);
821
822 Mat44 result;
823 result.mCol[0].mValue = vmulq_f32(det, minor0);
824 result.mCol[1].mValue = vmulq_f32(det, minor1);
825 result.mCol[2].mValue = vmulq_f32(det, minor2);
826 result.mCol[3].mValue = vmulq_f32(det, minor3);
827 return result;
828#elif defined(JPH_USE_RVV)
829 // Implementation mirrored from SSE and NEON implementations
830 const vfloat32m1_t c0 = __riscv_vle32_v_f32m1(mCol[0].mF32, 4);
831 const vfloat32m1_t c1 = __riscv_vle32_v_f32m1(mCol[1].mF32, 4);
832 const vfloat32m1_t c2 = __riscv_vle32_v_f32m1(mCol[2].mF32, 4);
833 const vfloat32m1_t c3 = __riscv_vle32_v_f32m1(mCol[3].mF32, 4);
834
835 vfloat32m1_t minor0, minor1, minor2, minor3;
836 vfloat32m1_t tmp1;
837 vfloat32m1_t row0, row1, row2, row3;
838
839 tmp1 = RVVShuffleFloat32x4<0, 1, 4, 5>(c0, c1);
840 row1 = RVVShuffleFloat32x4<0, 1, 4, 5>(c2, c3);
841 row0 = RVVShuffleFloat32x4<0, 2, 4, 6>(tmp1, row1);
842 row1 = RVVShuffleFloat32x4<1, 3, 5, 7>(row1, tmp1);
843 tmp1 = RVVShuffleFloat32x4<2, 3, 6, 7>(c0, c1);
844 row3 = RVVShuffleFloat32x4<2, 3, 6, 7>(c2, c3);
845 row2 = RVVShuffleFloat32x4<0, 2, 4, 6>(tmp1, row3);
846 row3 = RVVShuffleFloat32x4<1, 3, 5, 7>(row3, tmp1);
847
848 tmp1 = __riscv_vfmul_vv_f32m1(row2, row3, 4);
849 tmp1 = RVVShuffleFloat32x4<1, 0, 3, 2>(tmp1, tmp1);
850 minor0 = __riscv_vfmul_vv_f32m1(row1, tmp1, 4);
851 minor1 = __riscv_vfmul_vv_f32m1(row0, tmp1, 4);
852 tmp1 = RVVShuffleFloat32x4<2, 3, 0, 1>(tmp1, tmp1);
853 minor0 = __riscv_vfsub_vv_f32m1(__riscv_vfmul_vv_f32m1(row1, tmp1, 4), minor0, 4);
854 minor1 = __riscv_vfsub_vv_f32m1(__riscv_vfmul_vv_f32m1(row0, tmp1, 4), minor1, 4);
855 minor1 = RVVShuffleFloat32x4<2, 3, 0, 1>(minor1, minor1);
856
857 tmp1 = __riscv_vfmul_vv_f32m1(row1, row2, 4);
858 tmp1 = RVVShuffleFloat32x4<1, 0, 3, 2>(tmp1, tmp1);
859 minor0 = __riscv_vfadd_vv_f32m1(__riscv_vfmul_vv_f32m1(row3, tmp1, 4), minor0, 4);
860 minor3 = __riscv_vfmul_vv_f32m1(row0, tmp1, 4);
861 tmp1 = RVVShuffleFloat32x4<2, 3, 0, 1>(tmp1, tmp1);
862 minor0 = __riscv_vfsub_vv_f32m1(minor0, __riscv_vfmul_vv_f32m1(row3, tmp1, 4), 4);
863 minor3 = __riscv_vfsub_vv_f32m1(__riscv_vfmul_vv_f32m1(row0, tmp1, 4), minor3, 4);
864 minor3 = RVVShuffleFloat32x4<2, 3, 0, 1>(minor3, minor3);
865
866 tmp1 = RVVShuffleFloat32x4<2, 3, 0, 1>(row1, row1);
867 tmp1 = __riscv_vfmul_vv_f32m1(tmp1, row3, 4);
868 tmp1 = RVVShuffleFloat32x4<1, 0, 3, 2>(tmp1, tmp1);
869 row2 = RVVShuffleFloat32x4<2, 3, 0, 1>(row2, row2);
870 minor0 = __riscv_vfadd_vv_f32m1(__riscv_vfmul_vv_f32m1(row2, tmp1, 4), minor0, 4);
871 minor2 = __riscv_vfmul_vv_f32m1(row0, tmp1, 4);
872 tmp1 = RVVShuffleFloat32x4<2, 3, 0, 1>(tmp1, tmp1);
873 minor0 = __riscv_vfsub_vv_f32m1(minor0, __riscv_vfmul_vv_f32m1(row2, tmp1, 4), 4);
874 minor2 = __riscv_vfsub_vv_f32m1(__riscv_vfmul_vv_f32m1(row0, tmp1, 4), minor2, 4);
875 minor2 = RVVShuffleFloat32x4<2, 3, 0, 1>(minor2, minor2);
876
877 tmp1 = __riscv_vfmul_vv_f32m1(row0, row1, 4);
878 tmp1 = RVVShuffleFloat32x4<1, 0, 3, 2>(tmp1, tmp1);
879 minor2 = __riscv_vfadd_vv_f32m1(__riscv_vfmul_vv_f32m1(row3, tmp1, 4), minor2, 4);
880 minor3 = __riscv_vfsub_vv_f32m1(__riscv_vfmul_vv_f32m1(row2, tmp1, 4), minor3, 4);
881 tmp1 = RVVShuffleFloat32x4<2, 3, 0, 1>(tmp1, tmp1);
882 minor2 = __riscv_vfsub_vv_f32m1(__riscv_vfmul_vv_f32m1(row3, tmp1, 4), minor2, 4);
883 minor3 = __riscv_vfsub_vv_f32m1(minor3, __riscv_vfmul_vv_f32m1(row2, tmp1, 4), 4);
884
885 tmp1 = __riscv_vfmul_vv_f32m1(row0, row3, 4);
886 tmp1 = RVVShuffleFloat32x4<1, 0, 3, 2>(tmp1, tmp1);
887 minor1 = __riscv_vfsub_vv_f32m1(minor1, __riscv_vfmul_vv_f32m1(row2, tmp1, 4), 4);
888 minor2 = __riscv_vfadd_vv_f32m1(__riscv_vfmul_vv_f32m1(row1, tmp1, 4), minor2, 4);
889 tmp1 = RVVShuffleFloat32x4<2, 3, 0, 1>(tmp1, tmp1);
890 minor1 = __riscv_vfadd_vv_f32m1(__riscv_vfmul_vv_f32m1(row2, tmp1, 4), minor1, 4);
891 minor2 = __riscv_vfsub_vv_f32m1(minor2, __riscv_vfmul_vv_f32m1(row1, tmp1, 4), 4);
892
893 tmp1 = __riscv_vfmul_vv_f32m1(row0, row2, 4);
894 tmp1 = RVVShuffleFloat32x4<1, 0, 3, 2>(tmp1, tmp1);
895 minor1 = __riscv_vfadd_vv_f32m1(__riscv_vfmul_vv_f32m1(row3, tmp1, 4), minor1, 4);
896 minor3 = __riscv_vfsub_vv_f32m1(minor3, __riscv_vfmul_vv_f32m1(row1, tmp1, 4), 4);
897 tmp1 = RVVShuffleFloat32x4<2, 3, 0, 1>(tmp1, tmp1);
898 minor1 = __riscv_vfsub_vv_f32m1(minor1, __riscv_vfmul_vv_f32m1(row3, tmp1, 4), 4);
899 minor3 = __riscv_vfadd_vv_f32m1(__riscv_vfmul_vv_f32m1(row1, tmp1, 4), minor3, 4);
900
901 const vfloat32m1_t v_det = __riscv_vfmul_vv_f32m1(row0, minor0, 4);
902 const float s_det = RVVSumElementsFloat32x4(v_det);
903 const vfloat32m1_t det_inv = __riscv_vfmv_v_f_f32m1(1.0f / s_det, 4);
904
905 minor0 = __riscv_vfmul_vv_f32m1(det_inv, minor0, 4);
906 minor1 = __riscv_vfmul_vv_f32m1(det_inv, minor1, 4);
907 minor2 = __riscv_vfmul_vv_f32m1(det_inv, minor2, 4);
908 minor3 = __riscv_vfmul_vv_f32m1(det_inv, minor3, 4);
909
910 Mat44 result;
911 __riscv_vse32_v_f32m1(result.mCol[0].mF32, minor0, 4);
912 __riscv_vse32_v_f32m1(result.mCol[1].mF32, minor1, 4);
913 __riscv_vse32_v_f32m1(result.mCol[2].mF32, minor2, 4);
914 __riscv_vse32_v_f32m1(result.mCol[3].mF32, minor3, 4);
915 return result;
916#else
917 float m00 = JPH_EL(0, 0), m10 = JPH_EL(1, 0), m20 = JPH_EL(2, 0), m30 = JPH_EL(3, 0);
918 float m01 = JPH_EL(0, 1), m11 = JPH_EL(1, 1), m21 = JPH_EL(2, 1), m31 = JPH_EL(3, 1);
919 float m02 = JPH_EL(0, 2), m12 = JPH_EL(1, 2), m22 = JPH_EL(2, 2), m32 = JPH_EL(3, 2);
920 float m03 = JPH_EL(0, 3), m13 = JPH_EL(1, 3), m23 = JPH_EL(2, 3), m33 = JPH_EL(3, 3);
921
922 float m10211120 = m10 * m21 - m11 * m20;
923 float m10221220 = m10 * m22 - m12 * m20;
924 float m10231320 = m10 * m23 - m13 * m20;
925 float m10311130 = m10 * m31 - m11 * m30;
926 float m10321230 = m10 * m32 - m12 * m30;
927 float m10331330 = m10 * m33 - m13 * m30;
928 float m11221221 = m11 * m22 - m12 * m21;
929 float m11231321 = m11 * m23 - m13 * m21;
930 float m11321231 = m11 * m32 - m12 * m31;
931 float m11331331 = m11 * m33 - m13 * m31;
932 float m12231322 = m12 * m23 - m13 * m22;
933 float m12331332 = m12 * m33 - m13 * m32;
934 float m20312130 = m20 * m31 - m21 * m30;
935 float m20322230 = m20 * m32 - m22 * m30;
936 float m20332330 = m20 * m33 - m23 * m30;
937 float m21322231 = m21 * m32 - m22 * m31;
938 float m21332331 = m21 * m33 - m23 * m31;
939 float m22332332 = m22 * m33 - m23 * m32;
940
941 Vec4 col0(m11 * m22332332 - m12 * m21332331 + m13 * m21322231, -m10 * m22332332 + m12 * m20332330 - m13 * m20322230, m10 * m21332331 - m11 * m20332330 + m13 * m20312130, -m10 * m21322231 + m11 * m20322230 - m12 * m20312130);
942 Vec4 col1(-m01 * m22332332 + m02 * m21332331 - m03 * m21322231, m00 * m22332332 - m02 * m20332330 + m03 * m20322230, -m00 * m21332331 + m01 * m20332330 - m03 * m20312130, m00 * m21322231 - m01 * m20322230 + m02 * m20312130);
943 Vec4 col2(m01 * m12331332 - m02 * m11331331 + m03 * m11321231, -m00 * m12331332 + m02 * m10331330 - m03 * m10321230, m00 * m11331331 - m01 * m10331330 + m03 * m10311130, -m00 * m11321231 + m01 * m10321230 - m02 * m10311130);
944 Vec4 col3(-m01 * m12231322 + m02 * m11231321 - m03 * m11221221, m00 * m12231322 - m02 * m10231320 + m03 * m10221220, -m00 * m11231321 + m01 * m10231320 - m03 * m10211120, m00 * m11221221 - m01 * m10221220 + m02 * m10211120);
945
946 float det = m00 * col0.mF32[0] + m01 * col0.mF32[1] + m02 * col0.mF32[2] + m03 * col0.mF32[3];
947
948 return Mat44(col0 / det, col1 / det, col2 / det, col3 / det);
949#endif
950}
951
958
959float Mat44::GetDeterminant3x3() const
960{
961 return GetAxisX().Dot(GetAxisY().Cross(GetAxisZ()));
962}
963
964Mat44 Mat44::Adjointed3x3() const
965{
966 return Mat44(
967 Vec4(JPH_EL(1, 1), JPH_EL(1, 2), JPH_EL(1, 0), 0) * Vec4(JPH_EL(2, 2), JPH_EL(2, 0), JPH_EL(2, 1), 0)
968 - Vec4(JPH_EL(1, 2), JPH_EL(1, 0), JPH_EL(1, 1), 0) * Vec4(JPH_EL(2, 1), JPH_EL(2, 2), JPH_EL(2, 0), 0),
969 Vec4(JPH_EL(0, 2), JPH_EL(0, 0), JPH_EL(0, 1), 0) * Vec4(JPH_EL(2, 1), JPH_EL(2, 2), JPH_EL(2, 0), 0)
970 - Vec4(JPH_EL(0, 1), JPH_EL(0, 2), JPH_EL(0, 0), 0) * Vec4(JPH_EL(2, 2), JPH_EL(2, 0), JPH_EL(2, 1), 0),
971 Vec4(JPH_EL(0, 1), JPH_EL(0, 2), JPH_EL(0, 0), 0) * Vec4(JPH_EL(1, 2), JPH_EL(1, 0), JPH_EL(1, 1), 0)
972 - Vec4(JPH_EL(0, 2), JPH_EL(0, 0), JPH_EL(0, 1), 0) * Vec4(JPH_EL(1, 1), JPH_EL(1, 2), JPH_EL(1, 0), 0),
973 Vec4(0, 0, 0, 1));
974}
975
976Mat44 Mat44::Inversed3x3() const
977{
978 float det = GetDeterminant3x3();
979
980 return Mat44(
981 (Vec4(JPH_EL(1, 1), JPH_EL(1, 2), JPH_EL(1, 0), 0) * Vec4(JPH_EL(2, 2), JPH_EL(2, 0), JPH_EL(2, 1), 0)
982 - Vec4(JPH_EL(1, 2), JPH_EL(1, 0), JPH_EL(1, 1), 0) * Vec4(JPH_EL(2, 1), JPH_EL(2, 2), JPH_EL(2, 0), 0)) / det,
983 (Vec4(JPH_EL(0, 2), JPH_EL(0, 0), JPH_EL(0, 1), 0) * Vec4(JPH_EL(2, 1), JPH_EL(2, 2), JPH_EL(2, 0), 0)
984 - Vec4(JPH_EL(0, 1), JPH_EL(0, 2), JPH_EL(0, 0), 0) * Vec4(JPH_EL(2, 2), JPH_EL(2, 0), JPH_EL(2, 1), 0)) / det,
985 (Vec4(JPH_EL(0, 1), JPH_EL(0, 2), JPH_EL(0, 0), 0) * Vec4(JPH_EL(1, 2), JPH_EL(1, 0), JPH_EL(1, 1), 0)
986 - Vec4(JPH_EL(0, 2), JPH_EL(0, 0), JPH_EL(0, 1), 0) * Vec4(JPH_EL(1, 1), JPH_EL(1, 2), JPH_EL(1, 0), 0)) / det,
987 Vec4(0, 0, 0, 1));
988}
989
990bool Mat44::SetInversed3x3(Mat44Arg inM)
991{
992 float det = inM.GetDeterminant3x3();
993
994 // If the determinant is zero the matrix is singular and we return false
995 if (det == 0.0f)
996 return false;
997
998 // Finish calculating the inverse
999 *this = inM.Adjointed3x3();
1000 mCol[0] /= det;
1001 mCol[1] /= det;
1002 mCol[2] /= det;
1003 return true;
1004}
1005
1006Quat Mat44::GetQuaternion() const
1007{
1008 float tr = mCol[0].mF32[0] + mCol[1].mF32[1] + mCol[2].mF32[2];
1009
1010 if (tr >= 0.0f)
1011 {
1012 float s = sqrt(tr + 1.0f);
1013 float is = 0.5f / s;
1014 return Quat(
1015 (mCol[1].mF32[2] - mCol[2].mF32[1]) * is,
1016 (mCol[2].mF32[0] - mCol[0].mF32[2]) * is,
1017 (mCol[0].mF32[1] - mCol[1].mF32[0]) * is,
1018 0.5f * s);
1019 }
1020 else
1021 {
1022 int i = 0;
1023 if (mCol[1].mF32[1] > mCol[0].mF32[0]) i = 1;
1024 if (mCol[2].mF32[2] > mCol[i].mF32[i]) i = 2;
1025
1026 if (i == 0)
1027 {
1028 float s = sqrt(mCol[0].mF32[0] - (mCol[1].mF32[1] + mCol[2].mF32[2]) + 1);
1029 float is = 0.5f / s;
1030 return Quat(
1031 0.5f * s,
1032 (mCol[1].mF32[0] + mCol[0].mF32[1]) * is,
1033 (mCol[0].mF32[2] + mCol[2].mF32[0]) * is,
1034 (mCol[1].mF32[2] - mCol[2].mF32[1]) * is);
1035 }
1036 else if (i == 1)
1037 {
1038 float s = sqrt(mCol[1].mF32[1] - (mCol[2].mF32[2] + mCol[0].mF32[0]) + 1);
1039 float is = 0.5f / s;
1040 return Quat(
1041 (mCol[1].mF32[0] + mCol[0].mF32[1]) * is,
1042 0.5f * s,
1043 (mCol[2].mF32[1] + mCol[1].mF32[2]) * is,
1044 (mCol[2].mF32[0] - mCol[0].mF32[2]) * is);
1045 }
1046 else
1047 {
1048 JPH_ASSERT(i == 2);
1049
1050 float s = sqrt(mCol[2].mF32[2] - (mCol[0].mF32[0] + mCol[1].mF32[1]) + 1);
1051 float is = 0.5f / s;
1052 return Quat(
1053 (mCol[0].mF32[2] + mCol[2].mF32[0]) * is,
1054 (mCol[2].mF32[1] + mCol[1].mF32[2]) * is,
1055 0.5f * s,
1056 (mCol[0].mF32[1] - mCol[1].mF32[0]) * is);
1057 }
1058 }
1059}
1060
1061Mat44 Mat44::sQuatLeftMultiply(QuatArg inQ)
1062{
1063 return Mat44(
1064 inQ.mValue.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X>().FlipSign<1, 1, -1, -1>(),
1065 inQ.mValue.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y>().FlipSign<-1, 1, 1, -1>(),
1066 inQ.mValue.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>().FlipSign<1, -1, 1, -1>(),
1067 inQ.mValue);
1068}
1069
1070Mat44 Mat44::sQuatRightMultiply(QuatArg inQ)
1071{
1072 return Mat44(
1073 inQ.mValue.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X>().FlipSign<1, -1, 1, -1>(),
1074 inQ.mValue.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y>().FlipSign<1, 1, -1, -1>(),
1075 inQ.mValue.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>().FlipSign<-1, 1, 1, -1>(),
1076 inQ.mValue);
1077}
1078
1079Mat44 Mat44::GetRotation() const
1080{
1081 JPH_ASSERT(mCol[0][3] == 0.0f);
1082 JPH_ASSERT(mCol[1][3] == 0.0f);
1083 JPH_ASSERT(mCol[2][3] == 0.0f);
1084
1085 return Mat44(mCol[0], mCol[1], mCol[2], Vec4(0, 0, 0, 1));
1086}
1087
1088Mat44 Mat44::GetRotationSafe() const
1089{
1090#if defined(JPH_USE_AVX512)
1091 return Mat44(_mm_maskz_mov_ps(0b0111, mCol[0].mValue),
1092 _mm_maskz_mov_ps(0b0111, mCol[1].mValue),
1093 _mm_maskz_mov_ps(0b0111, mCol[2].mValue),
1094 Vec4(0, 0, 0, 1));
1095#elif defined(JPH_USE_SSE4_1)
1096 __m128 zero = _mm_setzero_ps();
1097 return Mat44(_mm_blend_ps(mCol[0].mValue, zero, 8),
1098 _mm_blend_ps(mCol[1].mValue, zero, 8),
1099 _mm_blend_ps(mCol[2].mValue, zero, 8),
1100 Vec4(0, 0, 0, 1));
1101#elif defined(JPH_USE_NEON)
1102 return Mat44(vsetq_lane_f32(0, mCol[0].mValue, 3),
1103 vsetq_lane_f32(0, mCol[1].mValue, 3),
1104 vsetq_lane_f32(0, mCol[2].mValue, 3),
1105 Vec4(0, 0, 0, 1));
1106#elif defined(JPH_USE_RVV)
1107 const float end_col[4] = { 0, 0, 0, 1 };
1108 const vfloat32m1_t rvv_end_col = __riscv_vle32_v_f32m1(end_col, 4);
1109 const vfloat32m1_t rvv_end_row = __riscv_vfmv_v_f_f32m1(0.0f, 3);
1110
1111 Mat44 result(*this);
1112 __riscv_vse32_v_f32m1(result.mCol[3].mF32, rvv_end_col, 4);
1113 __riscv_vsse32_v_f32m1(&result.mCol[0].mF32[3], sizeof(Vec4), rvv_end_row, 3);
1114 return result;
1115#else
1116 return Mat44(Vec4(mCol[0].mF32[0], mCol[0].mF32[1], mCol[0].mF32[2], 0),
1117 Vec4(mCol[1].mF32[0], mCol[1].mF32[1], mCol[1].mF32[2], 0),
1118 Vec4(mCol[2].mF32[0], mCol[2].mF32[1], mCol[2].mF32[2], 0),
1119 Vec4(0, 0, 0, 1));
1120#endif
1121}
1122
1123void Mat44::SetRotation(Mat44Arg inRotation)
1124{
1125 mCol[0] = inRotation.mCol[0];
1126 mCol[1] = inRotation.mCol[1];
1127 mCol[2] = inRotation.mCol[2];
1128}
1129
1130Mat44 Mat44::PreTranslated(Vec3Arg inTranslation) const
1131{
1132 return Mat44(mCol[0], mCol[1], mCol[2], Vec4(GetTranslation() + Multiply3x3(inTranslation), 1));
1133}
1134
1135Mat44 Mat44::PostTranslated(Vec3Arg inTranslation) const
1136{
1137 return Mat44(mCol[0], mCol[1], mCol[2], Vec4(GetTranslation() + inTranslation, 1));
1138}
1139
1140Mat44 Mat44::PreScaled(Vec3Arg inScale) const
1141{
1142 return Mat44(inScale.GetX() * mCol[0], inScale.GetY() * mCol[1], inScale.GetZ() * mCol[2], mCol[3]);
1143}
1144
1145Mat44 Mat44::PostScaled(Vec3Arg inScale) const
1146{
1147 Vec4 scale(inScale, 1);
1148 return Mat44(scale * mCol[0], scale * mCol[1], scale * mCol[2], scale * mCol[3]);
1149}
1150
1151Mat44 Mat44::Decompose(Vec3 &outScale) const
1152{
1153 // Start the modified Gram-Schmidt algorithm
1154 // X axis will just be normalized
1155 Vec3 x = GetAxisX();
1156
1157 // Make Y axis perpendicular to X
1158 Vec3 y = GetAxisY();
1159 float x_dot_x = x.LengthSq();
1160 y -= (x.Dot(y) / x_dot_x) * x;
1161
1162 // Make Z axis perpendicular to X
1163 Vec3 z = GetAxisZ();
1164 z -= (x.Dot(z) / x_dot_x) * x;
1165
1166 // Make Z axis perpendicular to Y
1167 float y_dot_y = y.LengthSq();
1168 z -= (y.Dot(z) / y_dot_y) * y;
1169
1170 // Determine the scale
1171 float z_dot_z = z.LengthSq();
1172 outScale = Vec3(x_dot_x, y_dot_y, z_dot_z).Sqrt();
1173
1174 // If the resulting x, y and z vectors don't form a right handed matrix, flip the z axis.
1175 if (x.Cross(y).Dot(z) < 0.0f)
1176 outScale.SetZ(-outScale.GetZ());
1177
1178 // Determine the rotation and translation
1179 return Mat44(Vec4(x / outScale.GetX(), 0), Vec4(y / outScale.GetY(), 0), Vec4(z / outScale.GetZ(), 0), GetColumn4(3));
1180}
1181
1182#undef JPH_EL
1183
1184JPH_NAMESPACE_END
JPH_NAMESPACE_END
#define JPH_NAMESPACE_END
Definition Core.h:428
JPH_NAMESPACE_BEGIN
#define JPH_NAMESPACE_BEGIN
Definition Core.h:422
xy
#define xy
Definition HLSLToCPP.h:511
JPH_ASSERT
#define JPH_ASSERT(...)
Definition IssueReporting.h:33
JPH_EL
#define JPH_EL(r, c)
Definition Mat44.inl:13
SWIZZLE_Z
@ SWIZZLE_Z
Use the Z component.
Definition Swizzle.h:14
SWIZZLE_W
@ SWIZZLE_W
Use the W component.
Definition Swizzle.h:15
SWIZZLE_X
@ SWIZZLE_X
Use the X component.
Definition Swizzle.h:12
SWIZZLE_Y
@ SWIZZLE_Y
Use the Y component.
Definition Swizzle.h:13
Tan
JPH_INLINE float Tan(float inX)
Tangent of x (input in radians)
Definition Trigonometry.h:28
Float4
Class that holds 4 float values. Convert to Vec4 to perform calculations.
Definition Float4.h:11
Mat44
Holds a 4x4 matrix of floats, but supports also operations on the 3x3 upper left part of the matrix.
Definition Mat44.h:13
Mat44::GetAxisY
JPH_INLINE Vec3 GetAxisY() const
Definition Mat44.h:148
Mat44::PostTranslated
JPH_INLINE Mat44 PostTranslated(Vec3Arg inTranslation) const
Post multiply by translation matrix: result = Mat44::sTranslation(inTranslation) * this (i....
Definition Mat44.inl:1135
Mat44::PreTranslated
JPH_INLINE Mat44 PreTranslated(Vec3Arg inTranslation) const
Pre multiply by translation matrix: result = this * Mat44::sTranslation(inTranslation)
Definition Mat44.inl:1130
Mat44::GetAxisZ
JPH_INLINE Vec3 GetAxisZ() const
Definition Mat44.h:150
Mat44::sIdentity
static JPH_INLINE Mat44 sIdentity()
Identity matrix.
Definition Mat44.inl:35
Mat44::Multiply3x3LeftTransposed
JPH_INLINE Mat44 Multiply3x3LeftTransposed(Mat44Arg inM) const
Multiply transpose of 3x3 matrix by 3x3 matrix ( )
Definition Mat44.inl:490
Mat44::sZero
static JPH_INLINE Mat44 sZero()
Zero matrix.
Definition Mat44.inl:30
Mat44::GetQuaternion
JPH_INLINE Quat GetQuaternion() const
Convert to quaternion.
Definition Mat44.inl:1006
Mat44::StoreFloat4x4
JPH_INLINE void StoreFloat4x4(Float4 *outV) const
Store matrix to memory.
Definition Mat44.inl:568
Mat44::operator-
JPH_INLINE Mat44 operator-() const
Negate.
Definition Mat44.inl:544
Mat44::sCrossProduct
static JPH_INLINE Mat44 sCrossProduct(Vec3Arg inV)
Get matrix that represents a cross product .
Definition Mat44.inl:179
Mat44::Transposed3x3
JPH_INLINE Mat44 Transposed3x3() const
Transpose 3x3 subpart of matrix.
Definition Mat44.inl:621
Mat44::operator*=
JPH_INLINE Mat44 & operator*=(float inV)
Multiply matrix with float.
Definition Mat44.inl:528
Mat44::GetDeterminant3x3
JPH_INLINE float GetDeterminant3x3() const
Get the determinant of a 3x3 matrix.
Definition Mat44.inl:959
Mat44::sQuatRightMultiply
static JPH_INLINE Mat44 sQuatRightMultiply(QuatArg inQ)
Returns matrix MR so that (where p and q are quaternions)
Definition Mat44.inl:1070
Mat44::Transposed
JPH_INLINE Mat44 Transposed() const
Transpose matrix.
Definition Mat44.inl:574
Mat44::Type
Vec4::Type Type
Definition Mat44.h:18
Mat44::Adjointed3x3
JPH_INLINE Mat44 Adjointed3x3() const
Get the adjoint of a 3x3 matrix.
Definition Mat44.inl:964
Mat44::operator==
JPH_INLINE bool operator==(Mat44Arg inM2) const
Comparison.
Definition Mat44.inl:225
Mat44::sRotationZ
static JPH_INLINE Mat44 sRotationZ(float inZ)
Definition Mat44.inl:77
Mat44::sLoadFloat4x4
static JPH_INLINE Mat44 sLoadFloat4x4(const Float4 *inV)
Load 16 floats from memory.
Definition Mat44.inl:45
Mat44::Multiply3x3Transposed
JPH_INLINE Vec3 Multiply3x3Transposed(Vec3Arg inV) const
Multiply vector by only 3x3 part of the transpose of the matrix ( )
Definition Mat44.inl:423
Mat44::sOuterProduct
static JPH_INLINE Mat44 sOuterProduct(Vec3Arg inV1, Vec3Arg inV2)
Get outer product of inV and inV2 (equivalent to )
Definition Mat44.inl:173
Mat44::sLookAt
static JPH_INLINE Mat44 sLookAt(Vec3Arg inPos, Vec3Arg inTarget, Vec3Arg inUp)
Definition Mat44.inl:207
Mat44::GetRotation
JPH_INLINE Mat44 GetRotation() const
Get rotation part only (note: retains the first 3 values from the bottom row)
Definition Mat44.inl:1079
Mat44::GetRotationSafe
JPH_INLINE Mat44 GetRotationSafe() const
Get rotation part only (note: also clears the bottom row)
Definition Mat44.inl:1088
Mat44::Multiply3x3RightTransposed
JPH_INLINE Mat44 Multiply3x3RightTransposed(Mat44Arg inM) const
Multiply 3x3 matrix by the transpose of a 3x3 matrix ( )
Definition Mat44.inl:504
Mat44::sNaN
static JPH_INLINE Mat44 sNaN()
Matrix filled with NaN's.
Definition Mat44.inl:40
Mat44::operator+=
JPH_INLINE Mat44 & operator+=(Mat44Arg inM)
Per element addition of matrix.
Definition Mat44.inl:560
Mat44::PostScaled
JPH_INLINE Mat44 PostScaled(Vec3Arg inScale) const
Scale a matrix: result = Mat44::sScale(inScale) * this.
Definition Mat44.inl:1145
Mat44::IsClose
JPH_INLINE bool IsClose(Mat44Arg inM2, float inMaxDistSq=1.0e-12f) const
Test if two matrices are close.
Definition Mat44.inl:233
Mat44::SetInversed3x3
JPH_INLINE bool SetInversed3x3(Mat44Arg inM)
*this = inM.Inversed3x3(), returns false if the matrix is singular in which case *this is unchanged
Definition Mat44.inl:990
Mat44::sScale
static JPH_INLINE Mat44 sScale(float inScale)
Get matrix that scales uniformly.
Definition Mat44.inl:163
Mat44::sLoadFloat4x4Aligned
static JPH_INLINE Mat44 sLoadFloat4x4Aligned(const Float4 *inV)
Load 16 floats from memory, 16 bytes aligned.
Definition Mat44.inl:53
Mat44::sTranslation
static JPH_INLINE Mat44 sTranslation(Vec3Arg inV)
Get matrix that translates.
Definition Mat44.inl:144
Mat44::GetColumn4
JPH_INLINE Vec4 GetColumn4(uint inCol) const
Definition Mat44.h:160
Mat44::Decompose
JPH_INLINE Mat44 Decompose(Vec3 &outScale) const
Definition Mat44.inl:1151
Mat44::GetAxisX
JPH_INLINE Vec3 GetAxisX() const
Access to the columns.
Definition Mat44.h:146
Mat44::Inversed
JPH_INLINE Mat44 Inversed() const
Inverse 4x4 matrix.
Definition Mat44.inl:673
Mat44::Multiply3x3
JPH_INLINE Vec3 Multiply3x3(Vec3Arg inV) const
Multiply vector by only 3x3 part of the matrix.
Definition Mat44.inl:384
Mat44::sRotationTranslation
static JPH_INLINE Mat44 sRotationTranslation(QuatArg inR, Vec3Arg inT)
Get matrix that rotates and translates.
Definition Mat44.inl:149
Mat44::SetRotation
JPH_INLINE void SetRotation(Mat44Arg inRotation)
Updates the rotation part of this matrix (the first 3 columns)
Definition Mat44.inl:1123
Mat44::GetTranslation
JPH_INLINE Vec3 GetTranslation() const
Definition Mat44.h:152
Mat44::operator+
JPH_INLINE Mat44 operator+(Mat44Arg inM) const
Per element addition of matrix.
Definition Mat44.inl:536
Mat44::sRotation
static JPH_INLINE Mat44 sRotation(Vec3Arg inAxis, float inAngle)
Rotate around arbitrary axis.
Definition Mat44.inl:139
Mat44::sInverseRotationTranslation
static JPH_INLINE Mat44 sInverseRotationTranslation(QuatArg inR, Vec3Arg inT)
Get inverse matrix of sRotationTranslation.
Definition Mat44.inl:156
Mat44::Mat44
Mat44()=default
Constructor.
Mat44::Inversed3x3
JPH_INLINE Mat44 Inversed3x3() const
Inverse 3x3 matrix.
Definition Mat44.inl:976
Mat44::sQuatLeftMultiply
static JPH_INLINE Mat44 sQuatLeftMultiply(QuatArg inQ)
Returns matrix ML so that (where p and q are quaternions)
Definition Mat44.inl:1061
Mat44::SetTranslation
JPH_INLINE void SetTranslation(Vec3Arg inV)
Definition Mat44.h:153
Mat44::sPerspective
static JPH_INLINE Mat44 sPerspective(float inFovY, float inAspect, float inNear, float inFar)
Returns a right-handed perspective projection matrix.
Definition Mat44.inl:216
Mat44::sRotationX
static JPH_INLINE Mat44 sRotationX(float inX)
Rotate around X, Y or Z axis (angle in radians)
Definition Mat44.inl:61
Mat44::InversedRotationTranslation
JPH_INLINE Mat44 InversedRotationTranslation() const
Inverse 4x4 matrix when it only contains rotation and translation.
Definition Mat44.inl:952
Mat44::PreScaled
JPH_INLINE Mat44 PreScaled(Vec3Arg inScale) const
Scale a matrix: result = this * Mat44::sScale(inScale)
Definition Mat44.inl:1140
Mat44::sRotationY
static JPH_INLINE Mat44 sRotationY(float inY)
Definition Mat44.inl:69
Mat44::operator*
friend JPH_INLINE Mat44 operator*(float inV, Mat44Arg inM)
Definition Mat44.h:128
Quat
Definition Quat.h:33
Quat::GetW
JPH_INLINE float GetW() const
Get W component (real part)
Definition Quat.h:79
Quat::GetY
JPH_INLINE float GetY() const
Get Y component (imaginary part j)
Definition Quat.h:73
Quat::GetZ
JPH_INLINE float GetZ() const
Get Z component (imaginary part k)
Definition Quat.h:76
Quat::sRotation
static JPH_INLINE Quat sRotation(Vec3Arg inAxis, float inAngle)
Rotation from axis and angle.
Definition Quat.inl:111
Quat::GetX
JPH_INLINE float GetX() const
Get X component (imaginary part i)
Definition Quat.h:70
Quat::Conjugated
JPH_INLINE Quat Conjugated() const
The conjugate [w, -x, -y, -z] is the same as the inverse for unit quaternions.
Definition Quat.h:185
Quat::IsNormalized
bool IsNormalized(float inTolerance=1.0e-5f) const
If the length of this quaternion is 1 +/- inTolerance.
Definition Quat.h:60
Quat::mValue
Vec4 mValue
4 vector that stores [x, y, z, w] parts of the quaternion
Definition Quat.h:264
UVec4::TestAllTrue
JPH_INLINE bool TestAllTrue() const
Test if all components are true (true is when highest bit of component is set)
Definition UVec4.inl:658
UVec4::sAnd
static JPH_INLINE UVec4 sAnd(UVec4Arg inV1, UVec4Arg inV2)
Logical and (component wise)
Definition UVec4.inl:292
Vec3
Definition Vec3.h:17
Vec3::Dot
JPH_INLINE float Dot(Vec3Arg inV2) const
Dot product.
Definition Vec3.inl:943
Vec3::sFixW
static JPH_INLINE Type sFixW(Type inValue)
Internal helper function that ensures that the Z component is replicated to the W component to preven...
Vec3::sAxisX
static JPH_INLINE Vec3 sAxisX()
Vectors with the principal axis.
Definition Vec3.h:56
Vec3::SplatX
JPH_INLINE Vec4 SplatX() const
Replicate the X component to all components.
Definition Vec3.inl:759
Vec3::Cross
JPH_INLINE Vec3 Cross(Vec3Arg inV2) const
Cross product.
Definition Vec3.inl:841
Vec3::GetX
JPH_INLINE float GetX() const
Get individual components.
Definition Vec3.h:127
Vec3::NormalizedOr
JPH_INLINE Vec3 NormalizedOr(Vec3Arg inZeroValue) const
Normalize vector or return inZeroValue if the length of the vector is zero.
Definition Vec3.inl:1076
Vec3::SplatZ
JPH_INLINE Vec4 SplatZ() const
Replicate the Z component to all components.
Definition Vec3.inl:791
Vec3::SetZ
JPH_INLINE void SetZ(float inZ)
Definition Vec3.h:135
Vec3::sAxisZ
static JPH_INLINE Vec3 sAxisZ()
Definition Vec3.h:58
Vec3::mValue
Type mValue
Definition Vec3.h:299
Vec3::GetY
JPH_INLINE float GetY() const
Definition Vec3.h:128
Vec3::SplatY
JPH_INLINE Vec4 SplatY() const
Replicate the Y component to all components.
Definition Vec3.inl:775
Vec3::LengthSq
JPH_INLINE float LengthSq() const
Squared length of vector.
Definition Vec3.inl:972
Vec3::mF32
float mF32[4]
Definition Vec3.h:300
Vec3::Sqrt
JPH_INLINE Vec3 Sqrt() const
Component wise square root.
Definition Vec3.inl:1027
Vec3::GetZ
JPH_INLINE float GetZ() const
Definition Vec3.h:129
Vec4
Definition Vec4.h:14
Vec4::SplatX
JPH_INLINE Vec4 SplatX() const
Replicate the X component to all components.
Definition Vec4.inl:808
Vec4::mF32
float mF32[4]
Definition Vec4.h:315
Vec4::sLoadFloat4Aligned
static JPH_INLINE Vec4 sLoadFloat4Aligned(const Float4 *inV)
Load 4 floats from memory, 16 bytes aligned.
Definition Vec4.inl:139
Vec4::FlipSign
JPH_INLINE Vec4 FlipSign() const
Flips the signs of the components, e.g. FlipSign<-1, 1, -1, 1>() will flip the signs of the X and Z c...
Definition Vec4.inl:1102
Vec4::sEquals
static JPH_INLINE UVec4 sEquals(Vec4Arg inV1, Vec4Arg inV2)
Equals (component wise)
Definition Vec4.inl:235
Vec4::SplatY
JPH_INLINE Vec4 SplatY() const
Replicate the Y component to all components.
Definition Vec4.inl:824
Vec4::SplatZ
JPH_INLINE Vec4 SplatZ() const
Replicate the Z component to all components.
Definition Vec4.inl:840
Vec4::GetX
JPH_INLINE float GetX() const
Get individual components.
Definition Vec4.h:119
Vec4::sLoadFloat4
static JPH_INLINE Vec4 sLoadFloat4(const Float4 *inV)
Load 4 floats from memory.
Definition Vec4.inl:123
Vec4::sZero
static JPH_INLINE Vec4 sZero()
Vector with all zeros.
Definition Vec4.inl:81
Vec4::Swizzle
JPH_INLINE Vec4 Swizzle() const
Swizzle the elements in inV.
Vec4::mValue
Type mValue
Definition Vec4.h:314
Vec4::sNaN
static JPH_INLINE Vec4 sNaN()
Vector with all NaN's.
Definition Vec4.inl:118
Vec4::sReplicate
static JPH_INLINE Vec4 sReplicate(float inV)
Replicate inV across all components.
Definition Vec4.inl:97
Vec4::SinCos
void SinCos(Vec4 &outSin, Vec4 &outCos) const
Calculate the sine and cosine for each element of this vector (input in radians)
Definition Vec4.inl:1216