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 const vfloat32m1_t col0 = __riscv_vle32_v_f32m1(mCol[0].mF32, 4);
266 const vfloat32m1_t col1 = __riscv_vle32_v_f32m1(mCol[1].mF32, 4);
267 const vfloat32m1_t col2 = __riscv_vle32_v_f32m1(mCol[2].mF32, 4);
268 const vfloat32m1_t col3 = __riscv_vle32_v_f32m1(mCol[3].mF32, 4);
269
270 for (int i = 0; i < 4; ++i)
271 {
272 const float *c = inM.mCol[i].mF32;
273 const vfloat32m1_t rep_0 = __riscv_vfmv_v_f_f32m1(c[0], 4);
274 const vfloat32m1_t rep_1 = __riscv_vfmv_v_f_f32m1(c[1], 4);
275 const vfloat32m1_t rep_2 = __riscv_vfmv_v_f_f32m1(c[2], 4);
276 const vfloat32m1_t rep_3 = __riscv_vfmv_v_f_f32m1(c[3], 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 return Transposed3x3().Multiply3x3(inV);
426}
427
428Mat44 Mat44::Multiply3x3(Mat44Arg inM) const
429{
430 JPH_ASSERT(mCol[0][3] == 0.0f);
431 JPH_ASSERT(mCol[1][3] == 0.0f);
432 JPH_ASSERT(mCol[2][3] == 0.0f);
433
434 Mat44 result;
435#if defined(JPH_USE_SSE)
436 for (int i = 0; i < 3; ++i)
437 {
438 __m128 c = inM.mCol[i].mValue;
439 __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(0, 0, 0, 0)));
440 t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(1, 1, 1, 1))));
441 t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(2, 2, 2, 2))));
442 result.mCol[i].mValue = t;
443 }
444#elif defined(JPH_USE_NEON)
445 for (int i = 0; i < 3; ++i)
446 {
447 Type c = inM.mCol[i].mValue;
448 Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(c, 0));
449 t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(c, 1));
450 t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(c, 2));
451 result.mCol[i].mValue = t;
452 }
453#elif defined(JPH_USE_RVV)
454 const vfloat32m1_t col0 = __riscv_vle32_v_f32m1(mCol[0].mF32, 4);
455 const vfloat32m1_t col1 = __riscv_vle32_v_f32m1(mCol[1].mF32, 4);
456 const vfloat32m1_t col2 = __riscv_vle32_v_f32m1(mCol[2].mF32, 4);
457
458 for (int i = 0; i < 3; ++i)
459 {
460 const float* col_i = inM.mCol[i].mF32;
461 const vfloat32m1_t v0 = __riscv_vfmv_v_f_f32m1(col_i[0], 4);
462 const vfloat32m1_t v1 = __riscv_vfmv_v_f_f32m1(col_i[1], 4);
463 const vfloat32m1_t v2 = __riscv_vfmv_v_f_f32m1(col_i[2], 4);
464
465 const vfloat32m1_t mul1 = __riscv_vfmul_vv_f32m1(v1, col1, 4);
466 const vfloat32m1_t mul2 = __riscv_vfmul_vv_f32m1(v2, col2, 4);
467
468 vfloat32m1_t t = __riscv_vfmul_vv_f32m1(v0, col0, 4);
469 t = __riscv_vfadd_vv_f32m1(t, mul1, 4);
470 t = __riscv_vfadd_vv_f32m1(t, mul2, 4);
471 __riscv_vse32_v_f32m1(result.mCol[i].mF32, t, 4);
472 }
473#else
474 for (int i = 0; i < 3; ++i)
475 result.mCol[i] = mCol[0] * inM.mCol[i].mF32[0] + mCol[1] * inM.mCol[i].mF32[1] + mCol[2] * inM.mCol[i].mF32[2];
476#endif
477 result.mCol[3] = Vec4(0, 0, 0, 1);
478 return result;
479}
480
481Mat44 Mat44::Multiply3x3LeftTransposed(Mat44Arg inM) const
482{
483 // Transpose left hand side
484 Mat44 trans = Transposed3x3();
485
486 // Do 3x3 matrix multiply
487 Mat44 result;
488 result.mCol[0] = trans.mCol[0] * inM.mCol[0].SplatX() + trans.mCol[1] * inM.mCol[0].SplatY() + trans.mCol[2] * inM.mCol[0].SplatZ();
489 result.mCol[1] = trans.mCol[0] * inM.mCol[1].SplatX() + trans.mCol[1] * inM.mCol[1].SplatY() + trans.mCol[2] * inM.mCol[1].SplatZ();
490 result.mCol[2] = trans.mCol[0] * inM.mCol[2].SplatX() + trans.mCol[1] * inM.mCol[2].SplatY() + trans.mCol[2] * inM.mCol[2].SplatZ();
491 result.mCol[3] = Vec4(0, 0, 0, 1);
492 return result;
493}
494
495Mat44 Mat44::Multiply3x3RightTransposed(Mat44Arg inM) const
496{
497 JPH_ASSERT(mCol[0][3] == 0.0f);
498 JPH_ASSERT(mCol[1][3] == 0.0f);
499 JPH_ASSERT(mCol[2][3] == 0.0f);
500
501 Mat44 result;
502 result.mCol[0] = mCol[0] * inM.mCol[0].SplatX() + mCol[1] * inM.mCol[1].SplatX() + mCol[2] * inM.mCol[2].SplatX();
503 result.mCol[1] = mCol[0] * inM.mCol[0].SplatY() + mCol[1] * inM.mCol[1].SplatY() + mCol[2] * inM.mCol[2].SplatY();
504 result.mCol[2] = mCol[0] * inM.mCol[0].SplatZ() + mCol[1] * inM.mCol[1].SplatZ() + mCol[2] * inM.mCol[2].SplatZ();
505 result.mCol[3] = Vec4(0, 0, 0, 1);
506 return result;
507}
508
509Mat44 Mat44::operator * (float inV) const
510{
511 Vec4 multiplier = Vec4::sReplicate(inV);
512
513 Mat44 result;
514 for (int c = 0; c < 4; ++c)
515 result.mCol[c] = mCol[c] * multiplier;
516 return result;
517}
518
519Mat44 &Mat44::operator *= (float inV)
520{
521 for (int c = 0; c < 4; ++c)
522 mCol[c] *= inV;
523
524 return *this;
525}
526
527Mat44 Mat44::operator + (Mat44Arg inM) const
528{
529 Mat44 result;
530 for (int i = 0; i < 4; ++i)
531 result.mCol[i] = mCol[i] + inM.mCol[i];
532 return result;
533}
534
535Mat44 Mat44::operator - () const
536{
537 Mat44 result;
538 for (int i = 0; i < 4; ++i)
539 result.mCol[i] = -mCol[i];
540 return result;
541}
542
543Mat44 Mat44::operator - (Mat44Arg inM) const
544{
545 Mat44 result;
546 for (int i = 0; i < 4; ++i)
547 result.mCol[i] = mCol[i] - inM.mCol[i];
548 return result;
549}
550
551Mat44 &Mat44::operator += (Mat44Arg inM)
552{
553 for (int c = 0; c < 4; ++c)
554 mCol[c] += inM.mCol[c];
555
556 return *this;
557}
558
559void Mat44::StoreFloat4x4(Float4 *outV) const
560{
561 for (int c = 0; c < 4; ++c)
562 mCol[c].StoreFloat4(outV + c);
563}
564
565Mat44 Mat44::Transposed() const
566{
567#if defined(JPH_USE_SSE)
568 __m128 tmp1 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(1, 0, 1, 0));
569 __m128 tmp3 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(3, 2, 3, 2));
570 __m128 tmp2 = _mm_shuffle_ps(mCol[2].mValue, mCol[3].mValue, _MM_SHUFFLE(1, 0, 1, 0));
571 __m128 tmp4 = _mm_shuffle_ps(mCol[2].mValue, mCol[3].mValue, _MM_SHUFFLE(3, 2, 3, 2));
572
573 Mat44 result;
574 result.mCol[0].mValue = _mm_shuffle_ps(tmp1, tmp2, _MM_SHUFFLE(2, 0, 2, 0));
575 result.mCol[1].mValue = _mm_shuffle_ps(tmp1, tmp2, _MM_SHUFFLE(3, 1, 3, 1));
576 result.mCol[2].mValue = _mm_shuffle_ps(tmp3, tmp4, _MM_SHUFFLE(2, 0, 2, 0));
577 result.mCol[3].mValue = _mm_shuffle_ps(tmp3, tmp4, _MM_SHUFFLE(3, 1, 3, 1));
578 return result;
579#elif defined(JPH_USE_NEON)
580 float32x4x2_t tmp1 = vzipq_f32(mCol[0].mValue, mCol[2].mValue);
581 float32x4x2_t tmp2 = vzipq_f32(mCol[1].mValue, mCol[3].mValue);
582 float32x4x2_t tmp3 = vzipq_f32(tmp1.val[0], tmp2.val[0]);
583 float32x4x2_t tmp4 = vzipq_f32(tmp1.val[1], tmp2.val[1]);
584
585 Mat44 result;
586 result.mCol[0].mValue = tmp3.val[0];
587 result.mCol[1].mValue = tmp3.val[1];
588 result.mCol[2].mValue = tmp4.val[0];
589 result.mCol[3].mValue = tmp4.val[1];
590 return result;
591#elif defined(JPH_USE_RVV)
592 const vfloat32m1_t row0 = __riscv_vlse32_v_f32m1(&mCol[0].mF32[0], sizeof(Vec4), 4);
593 const vfloat32m1_t row1 = __riscv_vlse32_v_f32m1(&mCol[0].mF32[1], sizeof(Vec4), 4);
594 const vfloat32m1_t row2 = __riscv_vlse32_v_f32m1(&mCol[0].mF32[2], sizeof(Vec4), 4);
595 const vfloat32m1_t row3 = __riscv_vlse32_v_f32m1(&mCol[0].mF32[3], sizeof(Vec4), 4);
596
597 Mat44 result;
598 __riscv_vse32_v_f32m1(result.mCol[0].mF32, row0, 4);
599 __riscv_vse32_v_f32m1(result.mCol[1].mF32, row1, 4);
600 __riscv_vse32_v_f32m1(result.mCol[2].mF32, row2, 4);
601 __riscv_vse32_v_f32m1(result.mCol[3].mF32, row3, 4);
602 return result;
603#else
604 Mat44 result;
605 for (int c = 0; c < 4; ++c)
606 for (int r = 0; r < 4; ++r)
607 result.mCol[r].mF32[c] = mCol[c].mF32[r];
608 return result;
609#endif
610}
611
612Mat44 Mat44::Transposed3x3() const
613{
614#if defined(JPH_USE_SSE)
615 __m128 zero = _mm_setzero_ps();
616 __m128 tmp1 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(1, 0, 1, 0));
617 __m128 tmp3 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(3, 2, 3, 2));
618 __m128 tmp2 = _mm_shuffle_ps(mCol[2].mValue, zero, _MM_SHUFFLE(1, 0, 1, 0));
619 __m128 tmp4 = _mm_shuffle_ps(mCol[2].mValue, zero, _MM_SHUFFLE(3, 2, 3, 2));
620
621 Mat44 result;
622 result.mCol[0].mValue = _mm_shuffle_ps(tmp1, tmp2, _MM_SHUFFLE(2, 0, 2, 0));
623 result.mCol[1].mValue = _mm_shuffle_ps(tmp1, tmp2, _MM_SHUFFLE(3, 1, 3, 1));
624 result.mCol[2].mValue = _mm_shuffle_ps(tmp3, tmp4, _MM_SHUFFLE(2, 0, 2, 0));
625#elif defined(JPH_USE_NEON)
626 float32x4x2_t tmp1 = vzipq_f32(mCol[0].mValue, mCol[2].mValue);
627 float32x4x2_t tmp2 = vzipq_f32(mCol[1].mValue, vdupq_n_f32(0));
628 float32x4x2_t tmp3 = vzipq_f32(tmp1.val[0], tmp2.val[0]);
629 float32x4x2_t tmp4 = vzipq_f32(tmp1.val[1], tmp2.val[1]);
630
631 Mat44 result;
632 result.mCol[0].mValue = tmp3.val[0];
633 result.mCol[1].mValue = tmp3.val[1];
634 result.mCol[2].mValue = tmp4.val[0];
635#elif defined(JPH_USE_RVV)
636 const float end_col[4] = { 0, 0, 0, 1 };
637 const vfloat32m1_t rvv_end_col = __riscv_vle32_v_f32m1(end_col, 4);
638 const vfloat32m1_t rvv_end_row = __riscv_vfmv_v_f_f32m1(0.0f, 3);
639
640 const vfloat32m1_t row0 = __riscv_vlse32_v_f32m1(&mCol[0].mF32[0], sizeof(Vec4), 3);
641 const vfloat32m1_t row1 = __riscv_vlse32_v_f32m1(&mCol[0].mF32[1], sizeof(Vec4), 3);
642 const vfloat32m1_t row2 = __riscv_vlse32_v_f32m1(&mCol[0].mF32[2], sizeof(Vec4), 3);
643
644 Mat44 result;
645 __riscv_vse32_v_f32m1(result.mCol[0].mF32, row0, 3);
646 __riscv_vse32_v_f32m1(result.mCol[1].mF32, row1, 3);
647 __riscv_vse32_v_f32m1(result.mCol[2].mF32, row2, 3);
648 __riscv_vse32_v_f32m1(result.mCol[3].mF32, rvv_end_col, 4);
649 __riscv_vsse32_v_f32m1(&result.mCol[0].mF32[3], sizeof(Vec4), rvv_end_row, 3);
650 return result;
651#else
652 Mat44 result;
653 for (int c = 0; c < 3; ++c)
654 {
655 for (int r = 0; r < 3; ++r)
656 result.mCol[c].mF32[r] = mCol[r].mF32[c];
657 result.mCol[c].mF32[3] = 0;
658 }
659#endif
660 result.mCol[3] = Vec4(0, 0, 0, 1);
661 return result;
662}
663
664Mat44 Mat44::Inversed() const
665{
666#if defined(JPH_USE_SSE)
667 // Algorithm from: http://download.intel.com/design/PentiumIII/sml/24504301.pdf
668 // Streaming SIMD Extensions - Inverse of 4x4 Matrix
669 // Adapted to load data using _mm_shuffle_ps instead of loading from memory
670 // Replaced _mm_rcp_ps with _mm_div_ps for better accuracy
671
672 __m128 tmp1 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(1, 0, 1, 0));
673 __m128 row1 = _mm_shuffle_ps(mCol[2].mValue, mCol[3].mValue, _MM_SHUFFLE(1, 0, 1, 0));
674 __m128 row0 = _mm_shuffle_ps(tmp1, row1, _MM_SHUFFLE(2, 0, 2, 0));
675 row1 = _mm_shuffle_ps(row1, tmp1, _MM_SHUFFLE(3, 1, 3, 1));
676 tmp1 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(3, 2, 3, 2));
677 __m128 row3 = _mm_shuffle_ps(mCol[2].mValue, mCol[3].mValue, _MM_SHUFFLE(3, 2, 3, 2));
678 __m128 row2 = _mm_shuffle_ps(tmp1, row3, _MM_SHUFFLE(2, 0, 2, 0));
679 row3 = _mm_shuffle_ps(row3, tmp1, _MM_SHUFFLE(3, 1, 3, 1));
680
681 tmp1 = _mm_mul_ps(row2, row3);
682 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
683 __m128 minor0 = _mm_mul_ps(row1, tmp1);
684 __m128 minor1 = _mm_mul_ps(row0, tmp1);
685 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
686 minor0 = _mm_sub_ps(_mm_mul_ps(row1, tmp1), minor0);
687 minor1 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor1);
688 minor1 = _mm_shuffle_ps(minor1, minor1, _MM_SHUFFLE(1, 0, 3, 2));
689
690 tmp1 = _mm_mul_ps(row1, row2);
691 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
692 minor0 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor0);
693 __m128 minor3 = _mm_mul_ps(row0, tmp1);
694 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
695 minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row3, tmp1));
696 minor3 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor3);
697 minor3 = _mm_shuffle_ps(minor3, minor3, _MM_SHUFFLE(1, 0, 3, 2));
698
699 tmp1 = _mm_mul_ps(_mm_shuffle_ps(row1, row1, _MM_SHUFFLE(1, 0, 3, 2)), row3);
700 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
701 row2 = _mm_shuffle_ps(row2, row2, _MM_SHUFFLE(1, 0, 3, 2));
702 minor0 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor0);
703 __m128 minor2 = _mm_mul_ps(row0, tmp1);
704 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
705 minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row2, tmp1));
706 minor2 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor2);
707 minor2 = _mm_shuffle_ps(minor2, minor2, _MM_SHUFFLE(1, 0, 3, 2));
708
709 tmp1 = _mm_mul_ps(row0, row1);
710 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
711 minor2 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor2);
712 minor3 = _mm_sub_ps(_mm_mul_ps(row2, tmp1), minor3);
713 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
714 minor2 = _mm_sub_ps(_mm_mul_ps(row3, tmp1), minor2);
715 minor3 = _mm_sub_ps(minor3, _mm_mul_ps(row2, tmp1));
716
717 tmp1 = _mm_mul_ps(row0, row3);
718 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
719 minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row2, tmp1));
720 minor2 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor2);
721 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
722 minor1 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor1);
723 minor2 = _mm_sub_ps(minor2, _mm_mul_ps(row1, tmp1));
724
725 tmp1 = _mm_mul_ps(row0, row2);
726 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
727 minor1 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor1);
728 minor3 = _mm_sub_ps(minor3, _mm_mul_ps(row1, tmp1));
729 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
730 minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row3, tmp1));
731 minor3 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor3);
732
733 __m128 det = _mm_mul_ps(row0, minor0);
734 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
735 det = _mm_add_ss(_mm_shuffle_ps(det, det, _MM_SHUFFLE(1, 0, 3, 2)), det);
736 det = _mm_div_ss(_mm_set_ss(1.0f), det);
737 det = _mm_shuffle_ps(det, det, _MM_SHUFFLE(0, 0, 0, 0));
738
739 Mat44 result;
740 result.mCol[0].mValue = _mm_mul_ps(det, minor0);
741 result.mCol[1].mValue = _mm_mul_ps(det, minor1);
742 result.mCol[2].mValue = _mm_mul_ps(det, minor2);
743 result.mCol[3].mValue = _mm_mul_ps(det, minor3);
744 return result;
745#elif defined(JPH_USE_NEON)
746 // Adapted from the SSE version, there's surprising few articles about efficient ways of calculating an inverse for ARM on the internet
747 Type tmp1 = JPH_NEON_SHUFFLE_F32x4(mCol[0].mValue, mCol[1].mValue, 0, 1, 4, 5);
748 Type row1 = JPH_NEON_SHUFFLE_F32x4(mCol[2].mValue, mCol[3].mValue, 0, 1, 4, 5);
749 Type row0 = JPH_NEON_SHUFFLE_F32x4(tmp1, row1, 0, 2, 4, 6);
750 row1 = JPH_NEON_SHUFFLE_F32x4(row1, tmp1, 1, 3, 5, 7);
751 tmp1 = JPH_NEON_SHUFFLE_F32x4(mCol[0].mValue, mCol[1].mValue, 2, 3, 6, 7);
752 Type row3 = JPH_NEON_SHUFFLE_F32x4(mCol[2].mValue, mCol[3].mValue, 2, 3, 6, 7);
753 Type row2 = JPH_NEON_SHUFFLE_F32x4(tmp1, row3, 0, 2, 4, 6);
754 row3 = JPH_NEON_SHUFFLE_F32x4(row3, tmp1, 1, 3, 5, 7);
755
756 tmp1 = vmulq_f32(row2, row3);
757 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
758 Type minor0 = vmulq_f32(row1, tmp1);
759 Type minor1 = vmulq_f32(row0, tmp1);
760 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
761 minor0 = vsubq_f32(vmulq_f32(row1, tmp1), minor0);
762 minor1 = vsubq_f32(vmulq_f32(row0, tmp1), minor1);
763 minor1 = JPH_NEON_SHUFFLE_F32x4(minor1, minor1, 2, 3, 0, 1);
764
765 tmp1 = vmulq_f32(row1, row2);
766 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
767 minor0 = vaddq_f32(vmulq_f32(row3, tmp1), minor0);
768 Type minor3 = vmulq_f32(row0, tmp1);
769 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
770 minor0 = vsubq_f32(minor0, vmulq_f32(row3, tmp1));
771 minor3 = vsubq_f32(vmulq_f32(row0, tmp1), minor3);
772 minor3 = JPH_NEON_SHUFFLE_F32x4(minor3, minor3, 2, 3, 0, 1);
773
774 tmp1 = JPH_NEON_SHUFFLE_F32x4(row1, row1, 2, 3, 0, 1);
775 tmp1 = vmulq_f32(tmp1, row3);
776 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
777 row2 = JPH_NEON_SHUFFLE_F32x4(row2, row2, 2, 3, 0, 1);
778 minor0 = vaddq_f32(vmulq_f32(row2, tmp1), minor0);
779 Type minor2 = vmulq_f32(row0, tmp1);
780 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
781 minor0 = vsubq_f32(minor0, vmulq_f32(row2, tmp1));
782 minor2 = vsubq_f32(vmulq_f32(row0, tmp1), minor2);
783 minor2 = JPH_NEON_SHUFFLE_F32x4(minor2, minor2, 2, 3, 0, 1);
784
785 tmp1 = vmulq_f32(row0, row1);
786 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
787 minor2 = vaddq_f32(vmulq_f32(row3, tmp1), minor2);
788 minor3 = vsubq_f32(vmulq_f32(row2, tmp1), minor3);
789 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
790 minor2 = vsubq_f32(vmulq_f32(row3, tmp1), minor2);
791 minor3 = vsubq_f32(minor3, vmulq_f32(row2, tmp1));
792
793 tmp1 = vmulq_f32(row0, row3);
794 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
795 minor1 = vsubq_f32(minor1, vmulq_f32(row2, tmp1));
796 minor2 = vaddq_f32(vmulq_f32(row1, tmp1), minor2);
797 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
798 minor1 = vaddq_f32(vmulq_f32(row2, tmp1), minor1);
799 minor2 = vsubq_f32(minor2, vmulq_f32(row1, tmp1));
800
801 tmp1 = vmulq_f32(row0, row2);
802 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
803 minor1 = vaddq_f32(vmulq_f32(row3, tmp1), minor1);
804 minor3 = vsubq_f32(minor3, vmulq_f32(row1, tmp1));
805 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
806 minor1 = vsubq_f32(minor1, vmulq_f32(row3, tmp1));
807 minor3 = vaddq_f32(vmulq_f32(row1, tmp1), minor3);
808
809 Type det = vmulq_f32(row0, minor0);
810 det = vdupq_n_f32(vaddvq_f32(det));
811 det = vdivq_f32(vdupq_n_f32(1.0f), det);
812
813 Mat44 result;
814 result.mCol[0].mValue = vmulq_f32(det, minor0);
815 result.mCol[1].mValue = vmulq_f32(det, minor1);
816 result.mCol[2].mValue = vmulq_f32(det, minor2);
817 result.mCol[3].mValue = vmulq_f32(det, minor3);
818 return result;
819#elif defined(JPH_USE_RVV)
820 // Implementation mirrored from SSE and NEON implementations
821 const vfloat32m1_t c0 = __riscv_vle32_v_f32m1(mCol[0].mF32, 4);
822 const vfloat32m1_t c1 = __riscv_vle32_v_f32m1(mCol[1].mF32, 4);
823 const vfloat32m1_t c2 = __riscv_vle32_v_f32m1(mCol[2].mF32, 4);
824 const vfloat32m1_t c3 = __riscv_vle32_v_f32m1(mCol[3].mF32, 4);
825
826 vfloat32m1_t minor0, minor1, minor2, minor3;
827 vfloat32m1_t tmp1;
828 vfloat32m1_t row0, row1, row2, row3;
829
830 tmp1 = RVVShuffleFloat32x4<0, 1, 4, 5>(c0, c1);
831 row1 = RVVShuffleFloat32x4<0, 1, 4, 5>(c2, c3);
832 row0 = RVVShuffleFloat32x4<0, 2, 4, 6>(tmp1, row1);
833 row1 = RVVShuffleFloat32x4<1, 3, 5, 7>(row1, tmp1);
834 tmp1 = RVVShuffleFloat32x4<2, 3, 6, 7>(c0, c1);
835 row3 = RVVShuffleFloat32x4<2, 3, 6, 7>(c2, c3);
836 row2 = RVVShuffleFloat32x4<0, 2, 4, 6>(tmp1, row3);
837 row3 = RVVShuffleFloat32x4<1, 3, 5, 7>(row3, tmp1);
838
839 tmp1 = __riscv_vfmul_vv_f32m1(row2, row3, 4);
840 tmp1 = RVVShuffleFloat32x4<1, 0, 3, 2>(tmp1, tmp1);
841 minor0 = __riscv_vfmul_vv_f32m1(row1, tmp1, 4);
842 minor1 = __riscv_vfmul_vv_f32m1(row0, tmp1, 4);
843 tmp1 = RVVShuffleFloat32x4<2, 3, 0, 1>(tmp1, tmp1);
844 minor0 = __riscv_vfsub_vv_f32m1(__riscv_vfmul_vv_f32m1(row1, tmp1, 4), minor0, 4);
845 minor1 = __riscv_vfsub_vv_f32m1(__riscv_vfmul_vv_f32m1(row0, tmp1, 4), minor1, 4);
846 minor1 = RVVShuffleFloat32x4<2, 3, 0, 1>(minor1, minor1);
847
848 tmp1 = __riscv_vfmul_vv_f32m1(row1, row2, 4);
849 tmp1 = RVVShuffleFloat32x4<1, 0, 3, 2>(tmp1, tmp1);
850 minor0 = __riscv_vfadd_vv_f32m1(__riscv_vfmul_vv_f32m1(row3, tmp1, 4), minor0, 4);
851 minor3 = __riscv_vfmul_vv_f32m1(row0, tmp1, 4);
852 tmp1 = RVVShuffleFloat32x4<2, 3, 0, 1>(tmp1, tmp1);
853 minor0 = __riscv_vfsub_vv_f32m1(minor0, __riscv_vfmul_vv_f32m1(row3, tmp1, 4), 4);
854 minor3 = __riscv_vfsub_vv_f32m1(__riscv_vfmul_vv_f32m1(row0, tmp1, 4), minor3, 4);
855 minor3 = RVVShuffleFloat32x4<2, 3, 0, 1>(minor3, minor3);
856
857 tmp1 = RVVShuffleFloat32x4<2, 3, 0, 1>(row1, row1);
858 tmp1 = __riscv_vfmul_vv_f32m1(tmp1, row3, 4);
859 tmp1 = RVVShuffleFloat32x4<1, 0, 3, 2>(tmp1, tmp1);
860 row2 = RVVShuffleFloat32x4<2, 3, 0, 1>(row2, row2);
861 minor0 = __riscv_vfadd_vv_f32m1(__riscv_vfmul_vv_f32m1(row2, tmp1, 4), minor0, 4);
862 minor2 = __riscv_vfmul_vv_f32m1(row0, tmp1, 4);
863 tmp1 = RVVShuffleFloat32x4<2, 3, 0, 1>(tmp1, tmp1);
864 minor0 = __riscv_vfsub_vv_f32m1(minor0, __riscv_vfmul_vv_f32m1(row2, tmp1, 4), 4);
865 minor2 = __riscv_vfsub_vv_f32m1(__riscv_vfmul_vv_f32m1(row0, tmp1, 4), minor2, 4);
866 minor2 = RVVShuffleFloat32x4<2, 3, 0, 1>(minor2, minor2);
867
868 tmp1 = __riscv_vfmul_vv_f32m1(row0, row1, 4);
869 tmp1 = RVVShuffleFloat32x4<1, 0, 3, 2>(tmp1, tmp1);
870 minor2 = __riscv_vfadd_vv_f32m1(__riscv_vfmul_vv_f32m1(row3, tmp1, 4), minor2, 4);
871 minor3 = __riscv_vfsub_vv_f32m1(__riscv_vfmul_vv_f32m1(row2, tmp1, 4), minor3, 4);
872 tmp1 = RVVShuffleFloat32x4<2, 3, 0, 1>(tmp1, tmp1);
873 minor2 = __riscv_vfsub_vv_f32m1(__riscv_vfmul_vv_f32m1(row3, tmp1, 4), minor2, 4);
874 minor3 = __riscv_vfsub_vv_f32m1(minor3, __riscv_vfmul_vv_f32m1(row2, tmp1, 4), 4);
875
876 tmp1 = __riscv_vfmul_vv_f32m1(row0, row3, 4);
877 tmp1 = RVVShuffleFloat32x4<1, 0, 3, 2>(tmp1, tmp1);
878 minor1 = __riscv_vfsub_vv_f32m1(minor1, __riscv_vfmul_vv_f32m1(row2, tmp1, 4), 4);
879 minor2 = __riscv_vfadd_vv_f32m1(__riscv_vfmul_vv_f32m1(row1, tmp1, 4), minor2, 4);
880 tmp1 = RVVShuffleFloat32x4<2, 3, 0, 1>(tmp1, tmp1);
881 minor1 = __riscv_vfadd_vv_f32m1(__riscv_vfmul_vv_f32m1(row2, tmp1, 4), minor1, 4);
882 minor2 = __riscv_vfsub_vv_f32m1(minor2, __riscv_vfmul_vv_f32m1(row1, tmp1, 4), 4);
883
884 tmp1 = __riscv_vfmul_vv_f32m1(row0, row2, 4);
885 tmp1 = RVVShuffleFloat32x4<1, 0, 3, 2>(tmp1, tmp1);
886 minor1 = __riscv_vfadd_vv_f32m1(__riscv_vfmul_vv_f32m1(row3, tmp1, 4), minor1, 4);
887 minor3 = __riscv_vfsub_vv_f32m1(minor3, __riscv_vfmul_vv_f32m1(row1, tmp1, 4), 4);
888 tmp1 = RVVShuffleFloat32x4<2, 3, 0, 1>(tmp1, tmp1);
889 minor1 = __riscv_vfsub_vv_f32m1(minor1, __riscv_vfmul_vv_f32m1(row3, tmp1, 4), 4);
890 minor3 = __riscv_vfadd_vv_f32m1(__riscv_vfmul_vv_f32m1(row1, tmp1, 4), minor3, 4);
891
892 const vfloat32m1_t v_det = __riscv_vfmul_vv_f32m1(row0, minor0, 4);
893 const vfloat32m1_t s_det = RVVSumElementsFloat32x4(v_det);
894 const vfloat32m1_t det_splat = __riscv_vrgather_vx_f32m1(s_det, 0, 4);
895 const vfloat32m1_t det_inv = __riscv_vfrdiv_vf_f32m1(det_splat, 1.0f, 4);
896
897 minor0 = __riscv_vfmul_vv_f32m1(det_inv, minor0, 4);
898 minor1 = __riscv_vfmul_vv_f32m1(det_inv, minor1, 4);
899 minor2 = __riscv_vfmul_vv_f32m1(det_inv, minor2, 4);
900 minor3 = __riscv_vfmul_vv_f32m1(det_inv, minor3, 4);
901
902 Mat44 result;
903 __riscv_vse32_v_f32m1(result.mCol[0].mF32, minor0, 4);
904 __riscv_vse32_v_f32m1(result.mCol[1].mF32, minor1, 4);
905 __riscv_vse32_v_f32m1(result.mCol[2].mF32, minor2, 4);
906 __riscv_vse32_v_f32m1(result.mCol[3].mF32, minor3, 4);
907 return result;
908#else
909 float m00 = JPH_EL(0, 0), m10 = JPH_EL(1, 0), m20 = JPH_EL(2, 0), m30 = JPH_EL(3, 0);
910 float m01 = JPH_EL(0, 1), m11 = JPH_EL(1, 1), m21 = JPH_EL(2, 1), m31 = JPH_EL(3, 1);
911 float m02 = JPH_EL(0, 2), m12 = JPH_EL(1, 2), m22 = JPH_EL(2, 2), m32 = JPH_EL(3, 2);
912 float m03 = JPH_EL(0, 3), m13 = JPH_EL(1, 3), m23 = JPH_EL(2, 3), m33 = JPH_EL(3, 3);
913
914 float m10211120 = m10 * m21 - m11 * m20;
915 float m10221220 = m10 * m22 - m12 * m20;
916 float m10231320 = m10 * m23 - m13 * m20;
917 float m10311130 = m10 * m31 - m11 * m30;
918 float m10321230 = m10 * m32 - m12 * m30;
919 float m10331330 = m10 * m33 - m13 * m30;
920 float m11221221 = m11 * m22 - m12 * m21;
921 float m11231321 = m11 * m23 - m13 * m21;
922 float m11321231 = m11 * m32 - m12 * m31;
923 float m11331331 = m11 * m33 - m13 * m31;
924 float m12231322 = m12 * m23 - m13 * m22;
925 float m12331332 = m12 * m33 - m13 * m32;
926 float m20312130 = m20 * m31 - m21 * m30;
927 float m20322230 = m20 * m32 - m22 * m30;
928 float m20332330 = m20 * m33 - m23 * m30;
929 float m21322231 = m21 * m32 - m22 * m31;
930 float m21332331 = m21 * m33 - m23 * m31;
931 float m22332332 = m22 * m33 - m23 * m32;
932
933 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);
934 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);
935 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);
936 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);
937
938 float det = m00 * col0.mF32[0] + m01 * col0.mF32[1] + m02 * col0.mF32[2] + m03 * col0.mF32[3];
939
940 return Mat44(col0 / det, col1 / det, col2 / det, col3 / det);
941#endif
942}
943
950
951float Mat44::GetDeterminant3x3() const
952{
953 return GetAxisX().Dot(GetAxisY().CrossPrecise(GetAxisZ()));
954}
955
956Mat44 Mat44::Adjointed3x3() const
957{
958 return Mat44(
959 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)
960 - 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),
961 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)
962 - 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),
963 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)
964 - 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),
965 Vec4(0, 0, 0, 1));
966}
967
968Mat44 Mat44::Inversed3x3() const
969{
970 float det = GetDeterminant3x3();
971
972 return Mat44(
973 (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)
974 - 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,
975 (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)
976 - 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,
977 (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)
978 - 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,
979 Vec4(0, 0, 0, 1));
980}
981
982bool Mat44::SetInversed3x3(Mat44Arg inM)
983{
984 float det = inM.GetDeterminant3x3();
985
986 // If the determinant is zero the matrix is singular and we return false
987 if (det == 0.0f)
988 return false;
989
990 // Finish calculating the inverse
991 *this = inM.Adjointed3x3();
992 mCol[0] /= det;
993 mCol[1] /= det;
994 mCol[2] /= det;
995 return true;
996}
997
998Quat Mat44::GetQuaternion() const
999{
1000 float tr = mCol[0].mF32[0] + mCol[1].mF32[1] + mCol[2].mF32[2];
1001
1002 if (tr >= 0.0f)
1003 {
1004 float s = Sqrt(tr + 1.0f);
1005 float is = 0.5f / s;
1006 return Quat(
1007 (mCol[1].mF32[2] - mCol[2].mF32[1]) * is,
1008 (mCol[2].mF32[0] - mCol[0].mF32[2]) * is,
1009 (mCol[0].mF32[1] - mCol[1].mF32[0]) * is,
1010 0.5f * s);
1011 }
1012 else
1013 {
1014 int i = 0;
1015 if (mCol[1].mF32[1] > mCol[0].mF32[0]) i = 1;
1016 if (mCol[2].mF32[2] > mCol[i].mF32[i]) i = 2;
1017
1018 if (i == 0)
1019 {
1020 float s = Sqrt(mCol[0].mF32[0] - (mCol[1].mF32[1] + mCol[2].mF32[2]) + 1);
1021 float is = 0.5f / s;
1022 return Quat(
1023 0.5f * s,
1024 (mCol[1].mF32[0] + mCol[0].mF32[1]) * is,
1025 (mCol[0].mF32[2] + mCol[2].mF32[0]) * is,
1026 (mCol[1].mF32[2] - mCol[2].mF32[1]) * is);
1027 }
1028 else if (i == 1)
1029 {
1030 float s = Sqrt(mCol[1].mF32[1] - (mCol[2].mF32[2] + mCol[0].mF32[0]) + 1);
1031 float is = 0.5f / s;
1032 return Quat(
1033 (mCol[1].mF32[0] + mCol[0].mF32[1]) * is,
1034 0.5f * s,
1035 (mCol[2].mF32[1] + mCol[1].mF32[2]) * is,
1036 (mCol[2].mF32[0] - mCol[0].mF32[2]) * is);
1037 }
1038 else
1039 {
1040 JPH_ASSERT(i == 2);
1041
1042 float s = Sqrt(mCol[2].mF32[2] - (mCol[0].mF32[0] + mCol[1].mF32[1]) + 1);
1043 float is = 0.5f / s;
1044 return Quat(
1045 (mCol[0].mF32[2] + mCol[2].mF32[0]) * is,
1046 (mCol[2].mF32[1] + mCol[1].mF32[2]) * is,
1047 0.5f * s,
1048 (mCol[0].mF32[1] - mCol[1].mF32[0]) * is);
1049 }
1050 }
1051}
1052
1053Mat44 Mat44::sQuatLeftMultiply(QuatArg inQ)
1054{
1055 return Mat44(
1056 inQ.mValue.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X>().FlipSign<1, 1, -1, -1>(),
1057 inQ.mValue.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y>().FlipSign<-1, 1, 1, -1>(),
1058 inQ.mValue.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>().FlipSign<1, -1, 1, -1>(),
1059 inQ.mValue);
1060}
1061
1062Mat44 Mat44::sQuatRightMultiply(QuatArg inQ)
1063{
1064 return Mat44(
1065 inQ.mValue.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X>().FlipSign<1, -1, 1, -1>(),
1066 inQ.mValue.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y>().FlipSign<1, 1, -1, -1>(),
1067 inQ.mValue.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>().FlipSign<-1, 1, 1, -1>(),
1068 inQ.mValue);
1069}
1070
1071Mat44 Mat44::GetRotation() const
1072{
1073 JPH_ASSERT(mCol[0][3] == 0.0f);
1074 JPH_ASSERT(mCol[1][3] == 0.0f);
1075 JPH_ASSERT(mCol[2][3] == 0.0f);
1076
1077 return Mat44(mCol[0], mCol[1], mCol[2], Vec4(0, 0, 0, 1));
1078}
1079
1080Mat44 Mat44::GetRotationSafe() const
1081{
1082#if defined(JPH_USE_AVX512)
1083 return Mat44(_mm_maskz_mov_ps(0b0111, mCol[0].mValue),
1084 _mm_maskz_mov_ps(0b0111, mCol[1].mValue),
1085 _mm_maskz_mov_ps(0b0111, mCol[2].mValue),
1086 Vec4(0, 0, 0, 1));
1087#elif defined(JPH_USE_SSE4_1)
1088 __m128 zero = _mm_setzero_ps();
1089 return Mat44(_mm_blend_ps(mCol[0].mValue, zero, 8),
1090 _mm_blend_ps(mCol[1].mValue, zero, 8),
1091 _mm_blend_ps(mCol[2].mValue, zero, 8),
1092 Vec4(0, 0, 0, 1));
1093#elif defined(JPH_USE_NEON)
1094 return Mat44(vsetq_lane_f32(0, mCol[0].mValue, 3),
1095 vsetq_lane_f32(0, mCol[1].mValue, 3),
1096 vsetq_lane_f32(0, mCol[2].mValue, 3),
1097 Vec4(0, 0, 0, 1));
1098#elif defined(JPH_USE_RVV)
1099 const float end_col[4] = { 0, 0, 0, 1 };
1100 const vfloat32m1_t rvv_end_col = __riscv_vle32_v_f32m1(end_col, 4);
1101 const vfloat32m1_t rvv_end_row = __riscv_vfmv_v_f_f32m1(0.0f, 3);
1102
1103 Mat44 result(*this);
1104 __riscv_vse32_v_f32m1(result.mCol[3].mF32, rvv_end_col, 4);
1105 __riscv_vsse32_v_f32m1(&result.mCol[0].mF32[3], sizeof(Vec4), rvv_end_row, 3);
1106 return result;
1107#else
1108 return Mat44(Vec4(mCol[0].mF32[0], mCol[0].mF32[1], mCol[0].mF32[2], 0),
1109 Vec4(mCol[1].mF32[0], mCol[1].mF32[1], mCol[1].mF32[2], 0),
1110 Vec4(mCol[2].mF32[0], mCol[2].mF32[1], mCol[2].mF32[2], 0),
1111 Vec4(0, 0, 0, 1));
1112#endif
1113}
1114
1115void Mat44::SetRotation(Mat44Arg inRotation)
1116{
1117 mCol[0] = inRotation.mCol[0];
1118 mCol[1] = inRotation.mCol[1];
1119 mCol[2] = inRotation.mCol[2];
1120}
1121
1122Mat44 Mat44::PreTranslated(Vec3Arg inTranslation) const
1123{
1124 return Mat44(mCol[0], mCol[1], mCol[2], Vec4(GetTranslation() + Multiply3x3(inTranslation), 1));
1125}
1126
1127Mat44 Mat44::PostTranslated(Vec3Arg inTranslation) const
1128{
1129 return Mat44(mCol[0], mCol[1], mCol[2], Vec4(GetTranslation() + inTranslation, 1));
1130}
1131
1132Mat44 Mat44::PreScaled(Vec3Arg inScale) const
1133{
1134 return Mat44(inScale.GetX() * mCol[0], inScale.GetY() * mCol[1], inScale.GetZ() * mCol[2], mCol[3]);
1135}
1136
1137Mat44 Mat44::PostScaled(Vec3Arg inScale) const
1138{
1139 Vec4 scale(inScale, 1);
1140 return Mat44(scale * mCol[0], scale * mCol[1], scale * mCol[2], scale * mCol[3]);
1141}
1142
1143Mat44 Mat44::Decompose(Vec3 &outScale) const
1144{
1145 // Start the modified Gram-Schmidt algorithm
1146 // X axis will just be normalized
1147 Vec3 x = GetAxisX();
1148
1149 // Make Y axis perpendicular to X
1150 Vec3 y = GetAxisY();
1151 float x_dot_x = x.LengthSq();
1152 y -= (x.Dot(y) / x_dot_x) * x;
1153
1154 // Make Z axis perpendicular to X
1155 Vec3 z = GetAxisZ();
1156 z -= (x.Dot(z) / x_dot_x) * x;
1157
1158 // Make Z axis perpendicular to Y
1159 float y_dot_y = y.LengthSq();
1160 z -= (y.Dot(z) / y_dot_y) * y;
1161
1162 // Determine the scale
1163 float z_dot_z = z.LengthSq();
1164 outScale = Vec3(x_dot_x, y_dot_y, z_dot_z).Sqrt();
1165
1166 // If the resulting x, y and z vectors don't form a right handed matrix, flip the z axis.
1167 if (x.Cross(y).Dot(z) < 0.0f)
1168 outScale.SetZ(-outScale.GetZ());
1169
1170 // Determine the rotation and translation
1171 return Mat44(Vec4(x / outScale.GetX(), 0), Vec4(y / outScale.GetY(), 0), Vec4(z / outScale.GetZ(), 0), GetColumn4(3));
1172}
1173
1174#undef JPH_EL
1175
1176JPH_NAMESPACE_END
JPH_NAMESPACE_END
#define JPH_NAMESPACE_END
Definition Core.h:434
JPH_NAMESPACE_BEGIN
#define JPH_NAMESPACE_BEGIN
Definition Core.h:428
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
Sqrt
JPH_INLINE float Sqrt(float inV)
Take the square root of a float value.
Definition Math.h:76
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:1127
Mat44::PreTranslated
JPH_INLINE Mat44 PreTranslated(Vec3Arg inTranslation) const
Pre multiply by translation matrix: result = this * Mat44::sTranslation(inTranslation)
Definition Mat44.inl:1122
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:481
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:998
Mat44::StoreFloat4x4
JPH_INLINE void StoreFloat4x4(Float4 *outV) const
Store matrix to memory.
Definition Mat44.inl:559
Mat44::operator-
JPH_INLINE Mat44 operator-() const
Negate.
Definition Mat44.inl:535
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:612
Mat44::operator*=
JPH_INLINE Mat44 & operator*=(float inV)
Multiply matrix with float.
Definition Mat44.inl:519
Mat44::GetDeterminant3x3
JPH_INLINE float GetDeterminant3x3() const
Get the determinant of a 3x3 matrix.
Definition Mat44.inl:951
Mat44::sQuatRightMultiply
static JPH_INLINE Mat44 sQuatRightMultiply(QuatArg inQ)
Returns matrix MR so that (where p and q are quaternions)
Definition Mat44.inl:1062
Mat44::Transposed
JPH_INLINE Mat44 Transposed() const
Transpose matrix.
Definition Mat44.inl:565
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:956
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:1071
Mat44::GetRotationSafe
JPH_INLINE Mat44 GetRotationSafe() const
Get rotation part only (note: also clears the bottom row)
Definition Mat44.inl:1080
Mat44::Multiply3x3RightTransposed
JPH_INLINE Mat44 Multiply3x3RightTransposed(Mat44Arg inM) const
Multiply 3x3 matrix by the transpose of a 3x3 matrix ( )
Definition Mat44.inl:495
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:551
Mat44::PostScaled
JPH_INLINE Mat44 PostScaled(Vec3Arg inScale) const
Scale a matrix: result = Mat44::sScale(inScale) * this.
Definition Mat44.inl:1137
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:982
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:1143
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:664
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:1115
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:527
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:968
Mat44::sQuatLeftMultiply
static JPH_INLINE Mat44 sQuatLeftMultiply(QuatArg inQ)
Returns matrix ML so that (where p and q are quaternions)
Definition Mat44.inl:1053
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:944
Mat44::PreScaled
JPH_INLINE Mat44 PreScaled(Vec3Arg inScale) const
Scale a matrix: result = this * Mat44::sScale(inScale)
Definition Mat44.inl:1132
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:160
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:931
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:761
Vec3::Cross
JPH_INLINE Vec3 Cross(Vec3Arg inV2) const
Cross product.
Definition Vec3.inl:855
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:978
Vec3::SplatZ
JPH_INLINE Vec4 SplatZ() const
Replicate the Z component to all components.
Definition Vec3.inl:793
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:308
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:777
Vec3::LengthSq
JPH_INLINE float LengthSq() const
Squared length of vector.
Definition Vec3.inl:946
Vec3::mF32
float mF32[4]
Definition Vec3.h:309
Vec3::Sqrt
JPH_INLINE Vec3 Sqrt() const
Component wise square root.
Definition Vec3.inl:956
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:804
Vec4::mF32
float mF32[4]
Definition Vec4.h:318
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:1078
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:820
Vec4::SplatZ
JPH_INLINE Vec4 SplatZ() const
Replicate the Z component to all components.
Definition Vec4.inl:836
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:317
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:1171