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
216bool Mat44::operator == (Mat44Arg inM2) const
217{
218 return UVec4::sAnd(
219 UVec4::sAnd(Vec4::sEquals(mCol[0], inM2.mCol[0]), Vec4::sEquals(mCol[1], inM2.mCol[1])),
220 UVec4::sAnd(Vec4::sEquals(mCol[2], inM2.mCol[2]), Vec4::sEquals(mCol[3], inM2.mCol[3]))
221 ).TestAllTrue();
222}
223
224bool Mat44::IsClose(Mat44Arg inM2, float inMaxDistSq) const
225{
226 for (int i = 0; i < 4; ++i)
227 if (!mCol[i].IsClose(inM2.mCol[i], inMaxDistSq))
228 return false;
229 return true;
230}
231
232Mat44 Mat44::operator * (Mat44Arg inM) const
233{
234 Mat44 result;
235#if defined(JPH_USE_SSE)
236 for (int i = 0; i < 4; ++i)
237 {
238 __m128 c = inM.mCol[i].mValue;
239 __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(0, 0, 0, 0)));
240 t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(1, 1, 1, 1))));
241 t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(2, 2, 2, 2))));
242 t = _mm_add_ps(t, _mm_mul_ps(mCol[3].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(3, 3, 3, 3))));
243 result.mCol[i].mValue = t;
244 }
245#elif defined(JPH_USE_NEON)
246 for (int i = 0; i < 4; ++i)
247 {
248 Type c = inM.mCol[i].mValue;
249 Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(c, 0));
250 t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(c, 1));
251 t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(c, 2));
252 t = vmlaq_f32(t, mCol[3].mValue, vdupq_laneq_f32(c, 3));
253 result.mCol[i].mValue = t;
254 }
255#else
256 for (int i = 0; i < 4; ++i)
257 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];
258#endif
259 return result;
260}
261
262Vec3 Mat44::operator * (Vec3Arg inV) const
263{
264#if defined(JPH_USE_SSE)
265 __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(0, 0, 0, 0)));
266 t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(1, 1, 1, 1))));
267 t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(2, 2, 2, 2))));
268 t = _mm_add_ps(t, mCol[3].mValue);
269 return Vec3::sFixW(t);
270#elif defined(JPH_USE_NEON)
271 Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(inV.mValue, 0));
272 t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(inV.mValue, 1));
273 t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(inV.mValue, 2));
274 t = vaddq_f32(t, mCol[3].mValue); // Don't combine this with the first mul into a fused multiply add, causes precision issues
275 return Vec3::sFixW(t);
276#else
277 return Vec3(
278 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],
279 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],
280 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]);
281#endif
282}
283
284Vec4 Mat44::operator * (Vec4Arg inV) const
285{
286#if defined(JPH_USE_SSE)
287 __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(0, 0, 0, 0)));
288 t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(1, 1, 1, 1))));
289 t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(2, 2, 2, 2))));
290 t = _mm_add_ps(t, _mm_mul_ps(mCol[3].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(3, 3, 3, 3))));
291 return t;
292#elif defined(JPH_USE_NEON)
293 Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(inV.mValue, 0));
294 t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(inV.mValue, 1));
295 t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(inV.mValue, 2));
296 t = vmlaq_f32(t, mCol[3].mValue, vdupq_laneq_f32(inV.mValue, 3));
297 return t;
298#else
299 return Vec4(
300 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],
301 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],
302 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],
303 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]);
304#endif
305}
306
307Vec3 Mat44::Multiply3x3(Vec3Arg inV) const
308{
309#if defined(JPH_USE_SSE)
310 __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(0, 0, 0, 0)));
311 t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(1, 1, 1, 1))));
312 t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(2, 2, 2, 2))));
313 return Vec3::sFixW(t);
314#elif defined(JPH_USE_NEON)
315 Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(inV.mValue, 0));
316 t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(inV.mValue, 1));
317 t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(inV.mValue, 2));
318 return Vec3::sFixW(t);
319#else
320 return Vec3(
321 mCol[0].mF32[0] * inV.mF32[0] + mCol[1].mF32[0] * inV.mF32[1] + mCol[2].mF32[0] * inV.mF32[2],
322 mCol[0].mF32[1] * inV.mF32[0] + mCol[1].mF32[1] * inV.mF32[1] + mCol[2].mF32[1] * inV.mF32[2],
323 mCol[0].mF32[2] * inV.mF32[0] + mCol[1].mF32[2] * inV.mF32[1] + mCol[2].mF32[2] * inV.mF32[2]);
324#endif
325}
326
327Vec3 Mat44::Multiply3x3Transposed(Vec3Arg inV) const
328{
329#if defined(JPH_USE_SSE4_1)
330 __m128 x = _mm_dp_ps(mCol[0].mValue, inV.mValue, 0x7f);
331 __m128 y = _mm_dp_ps(mCol[1].mValue, inV.mValue, 0x7f);
332 __m128 xy = _mm_blend_ps(x, y, 0b0010);
333 __m128 z = _mm_dp_ps(mCol[2].mValue, inV.mValue, 0x7f);
334 __m128 xyzz = _mm_blend_ps(xy, z, 0b1100);
335 return xyzz;
336#else
337 return Transposed3x3().Multiply3x3(inV);
338#endif
339}
340
341Mat44 Mat44::Multiply3x3(Mat44Arg inM) const
342{
343 JPH_ASSERT(mCol[0][3] == 0.0f);
344 JPH_ASSERT(mCol[1][3] == 0.0f);
345 JPH_ASSERT(mCol[2][3] == 0.0f);
346
347 Mat44 result;
348#if defined(JPH_USE_SSE)
349 for (int i = 0; i < 3; ++i)
350 {
351 __m128 c = inM.mCol[i].mValue;
352 __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(0, 0, 0, 0)));
353 t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(1, 1, 1, 1))));
354 t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(2, 2, 2, 2))));
355 result.mCol[i].mValue = t;
356 }
357#elif defined(JPH_USE_NEON)
358 for (int i = 0; i < 3; ++i)
359 {
360 Type c = inM.mCol[i].mValue;
361 Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(c, 0));
362 t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(c, 1));
363 t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(c, 2));
364 result.mCol[i].mValue = t;
365 }
366#else
367 for (int i = 0; i < 3; ++i)
368 result.mCol[i] = mCol[0] * inM.mCol[i].mF32[0] + mCol[1] * inM.mCol[i].mF32[1] + mCol[2] * inM.mCol[i].mF32[2];
369#endif
370 result.mCol[3] = Vec4(0, 0, 0, 1);
371 return result;
372}
373
374Mat44 Mat44::Multiply3x3LeftTransposed(Mat44Arg inM) const
375{
376 // Transpose left hand side
377 Mat44 trans = Transposed3x3();
378
379 // Do 3x3 matrix multiply
380 Mat44 result;
381 result.mCol[0] = trans.mCol[0] * inM.mCol[0].SplatX() + trans.mCol[1] * inM.mCol[0].SplatY() + trans.mCol[2] * inM.mCol[0].SplatZ();
382 result.mCol[1] = trans.mCol[0] * inM.mCol[1].SplatX() + trans.mCol[1] * inM.mCol[1].SplatY() + trans.mCol[2] * inM.mCol[1].SplatZ();
383 result.mCol[2] = trans.mCol[0] * inM.mCol[2].SplatX() + trans.mCol[1] * inM.mCol[2].SplatY() + trans.mCol[2] * inM.mCol[2].SplatZ();
384 result.mCol[3] = Vec4(0, 0, 0, 1);
385 return result;
386}
387
388Mat44 Mat44::Multiply3x3RightTransposed(Mat44Arg inM) const
389{
390 JPH_ASSERT(mCol[0][3] == 0.0f);
391 JPH_ASSERT(mCol[1][3] == 0.0f);
392 JPH_ASSERT(mCol[2][3] == 0.0f);
393
394 Mat44 result;
395 result.mCol[0] = mCol[0] * inM.mCol[0].SplatX() + mCol[1] * inM.mCol[1].SplatX() + mCol[2] * inM.mCol[2].SplatX();
396 result.mCol[1] = mCol[0] * inM.mCol[0].SplatY() + mCol[1] * inM.mCol[1].SplatY() + mCol[2] * inM.mCol[2].SplatY();
397 result.mCol[2] = mCol[0] * inM.mCol[0].SplatZ() + mCol[1] * inM.mCol[1].SplatZ() + mCol[2] * inM.mCol[2].SplatZ();
398 result.mCol[3] = Vec4(0, 0, 0, 1);
399 return result;
400}
401
402Mat44 Mat44::operator * (float inV) const
403{
404 Vec4 multiplier = Vec4::sReplicate(inV);
405
406 Mat44 result;
407 for (int c = 0; c < 4; ++c)
408 result.mCol[c] = mCol[c] * multiplier;
409 return result;
410}
411
412Mat44 &Mat44::operator *= (float inV)
413{
414 for (int c = 0; c < 4; ++c)
415 mCol[c] *= inV;
416
417 return *this;
418}
419
420Mat44 Mat44::operator + (Mat44Arg inM) const
421{
422 Mat44 result;
423 for (int i = 0; i < 4; ++i)
424 result.mCol[i] = mCol[i] + inM.mCol[i];
425 return result;
426}
427
428Mat44 Mat44::operator - () const
429{
430 Mat44 result;
431 for (int i = 0; i < 4; ++i)
432 result.mCol[i] = -mCol[i];
433 return result;
434}
435
436Mat44 Mat44::operator - (Mat44Arg inM) const
437{
438 Mat44 result;
439 for (int i = 0; i < 4; ++i)
440 result.mCol[i] = mCol[i] - inM.mCol[i];
441 return result;
442}
443
444Mat44 &Mat44::operator += (Mat44Arg inM)
445{
446 for (int c = 0; c < 4; ++c)
447 mCol[c] += inM.mCol[c];
448
449 return *this;
450}
451
452void Mat44::StoreFloat4x4(Float4 *outV) const
453{
454 for (int c = 0; c < 4; ++c)
455 mCol[c].StoreFloat4(outV + c);
456}
457
458Mat44 Mat44::Transposed() const
459{
460#if defined(JPH_USE_SSE)
461 __m128 tmp1 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(1, 0, 1, 0));
462 __m128 tmp3 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(3, 2, 3, 2));
463 __m128 tmp2 = _mm_shuffle_ps(mCol[2].mValue, mCol[3].mValue, _MM_SHUFFLE(1, 0, 1, 0));
464 __m128 tmp4 = _mm_shuffle_ps(mCol[2].mValue, mCol[3].mValue, _MM_SHUFFLE(3, 2, 3, 2));
465
466 Mat44 result;
467 result.mCol[0].mValue = _mm_shuffle_ps(tmp1, tmp2, _MM_SHUFFLE(2, 0, 2, 0));
468 result.mCol[1].mValue = _mm_shuffle_ps(tmp1, tmp2, _MM_SHUFFLE(3, 1, 3, 1));
469 result.mCol[2].mValue = _mm_shuffle_ps(tmp3, tmp4, _MM_SHUFFLE(2, 0, 2, 0));
470 result.mCol[3].mValue = _mm_shuffle_ps(tmp3, tmp4, _MM_SHUFFLE(3, 1, 3, 1));
471 return result;
472#elif defined(JPH_USE_NEON)
473 float32x4x2_t tmp1 = vzipq_f32(mCol[0].mValue, mCol[2].mValue);
474 float32x4x2_t tmp2 = vzipq_f32(mCol[1].mValue, mCol[3].mValue);
475 float32x4x2_t tmp3 = vzipq_f32(tmp1.val[0], tmp2.val[0]);
476 float32x4x2_t tmp4 = vzipq_f32(tmp1.val[1], tmp2.val[1]);
477
478 Mat44 result;
479 result.mCol[0].mValue = tmp3.val[0];
480 result.mCol[1].mValue = tmp3.val[1];
481 result.mCol[2].mValue = tmp4.val[0];
482 result.mCol[3].mValue = tmp4.val[1];
483 return result;
484#else
485 Mat44 result;
486 for (int c = 0; c < 4; ++c)
487 for (int r = 0; r < 4; ++r)
488 result.mCol[r].mF32[c] = mCol[c].mF32[r];
489 return result;
490#endif
491}
492
493Mat44 Mat44::Transposed3x3() const
494{
495#if defined(JPH_USE_SSE)
496 __m128 zero = _mm_setzero_ps();
497 __m128 tmp1 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(1, 0, 1, 0));
498 __m128 tmp3 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(3, 2, 3, 2));
499 __m128 tmp2 = _mm_shuffle_ps(mCol[2].mValue, zero, _MM_SHUFFLE(1, 0, 1, 0));
500 __m128 tmp4 = _mm_shuffle_ps(mCol[2].mValue, zero, _MM_SHUFFLE(3, 2, 3, 2));
501
502 Mat44 result;
503 result.mCol[0].mValue = _mm_shuffle_ps(tmp1, tmp2, _MM_SHUFFLE(2, 0, 2, 0));
504 result.mCol[1].mValue = _mm_shuffle_ps(tmp1, tmp2, _MM_SHUFFLE(3, 1, 3, 1));
505 result.mCol[2].mValue = _mm_shuffle_ps(tmp3, tmp4, _MM_SHUFFLE(2, 0, 2, 0));
506#elif defined(JPH_USE_NEON)
507 float32x4x2_t tmp1 = vzipq_f32(mCol[0].mValue, mCol[2].mValue);
508 float32x4x2_t tmp2 = vzipq_f32(mCol[1].mValue, vdupq_n_f32(0));
509 float32x4x2_t tmp3 = vzipq_f32(tmp1.val[0], tmp2.val[0]);
510 float32x4x2_t tmp4 = vzipq_f32(tmp1.val[1], tmp2.val[1]);
511
512 Mat44 result;
513 result.mCol[0].mValue = tmp3.val[0];
514 result.mCol[1].mValue = tmp3.val[1];
515 result.mCol[2].mValue = tmp4.val[0];
516#else
517 Mat44 result;
518 for (int c = 0; c < 3; ++c)
519 {
520 for (int r = 0; r < 3; ++r)
521 result.mCol[c].mF32[r] = mCol[r].mF32[c];
522 result.mCol[c].mF32[3] = 0;
523 }
524#endif
525 result.mCol[3] = Vec4(0, 0, 0, 1);
526 return result;
527}
528
529Mat44 Mat44::Inversed() const
530{
531#if defined(JPH_USE_SSE)
532 // Algorithm from: http://download.intel.com/design/PentiumIII/sml/24504301.pdf
533 // Streaming SIMD Extensions - Inverse of 4x4 Matrix
534 // Adapted to load data using _mm_shuffle_ps instead of loading from memory
535 // Replaced _mm_rcp_ps with _mm_div_ps for better accuracy
536
537 __m128 tmp1 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(1, 0, 1, 0));
538 __m128 row1 = _mm_shuffle_ps(mCol[2].mValue, mCol[3].mValue, _MM_SHUFFLE(1, 0, 1, 0));
539 __m128 row0 = _mm_shuffle_ps(tmp1, row1, _MM_SHUFFLE(2, 0, 2, 0));
540 row1 = _mm_shuffle_ps(row1, tmp1, _MM_SHUFFLE(3, 1, 3, 1));
541 tmp1 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(3, 2, 3, 2));
542 __m128 row3 = _mm_shuffle_ps(mCol[2].mValue, mCol[3].mValue, _MM_SHUFFLE(3, 2, 3, 2));
543 __m128 row2 = _mm_shuffle_ps(tmp1, row3, _MM_SHUFFLE(2, 0, 2, 0));
544 row3 = _mm_shuffle_ps(row3, tmp1, _MM_SHUFFLE(3, 1, 3, 1));
545
546 tmp1 = _mm_mul_ps(row2, row3);
547 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
548 __m128 minor0 = _mm_mul_ps(row1, tmp1);
549 __m128 minor1 = _mm_mul_ps(row0, tmp1);
550 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
551 minor0 = _mm_sub_ps(_mm_mul_ps(row1, tmp1), minor0);
552 minor1 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor1);
553 minor1 = _mm_shuffle_ps(minor1, minor1, _MM_SHUFFLE(1, 0, 3, 2));
554
555 tmp1 = _mm_mul_ps(row1, row2);
556 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
557 minor0 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor0);
558 __m128 minor3 = _mm_mul_ps(row0, tmp1);
559 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
560 minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row3, tmp1));
561 minor3 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor3);
562 minor3 = _mm_shuffle_ps(minor3, minor3, _MM_SHUFFLE(1, 0, 3, 2));
563
564 tmp1 = _mm_mul_ps(_mm_shuffle_ps(row1, row1, _MM_SHUFFLE(1, 0, 3, 2)), row3);
565 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
566 row2 = _mm_shuffle_ps(row2, row2, _MM_SHUFFLE(1, 0, 3, 2));
567 minor0 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor0);
568 __m128 minor2 = _mm_mul_ps(row0, tmp1);
569 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
570 minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row2, tmp1));
571 minor2 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor2);
572 minor2 = _mm_shuffle_ps(minor2, minor2, _MM_SHUFFLE(1, 0, 3, 2));
573
574 tmp1 = _mm_mul_ps(row0, row1);
575 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
576 minor2 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor2);
577 minor3 = _mm_sub_ps(_mm_mul_ps(row2, tmp1), minor3);
578 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
579 minor2 = _mm_sub_ps(_mm_mul_ps(row3, tmp1), minor2);
580 minor3 = _mm_sub_ps(minor3, _mm_mul_ps(row2, tmp1));
581
582 tmp1 = _mm_mul_ps(row0, row3);
583 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
584 minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row2, tmp1));
585 minor2 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor2);
586 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
587 minor1 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor1);
588 minor2 = _mm_sub_ps(minor2, _mm_mul_ps(row1, tmp1));
589
590 tmp1 = _mm_mul_ps(row0, row2);
591 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
592 minor1 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor1);
593 minor3 = _mm_sub_ps(minor3, _mm_mul_ps(row1, tmp1));
594 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
595 minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row3, tmp1));
596 minor3 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor3);
597
598 __m128 det = _mm_mul_ps(row0, minor0);
599 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
600 det = _mm_add_ss(_mm_shuffle_ps(det, det, _MM_SHUFFLE(1, 0, 3, 2)), det);
601 det = _mm_div_ss(_mm_set_ss(1.0f), det);
602 det = _mm_shuffle_ps(det, det, _MM_SHUFFLE(0, 0, 0, 0));
603
604 Mat44 result;
605 result.mCol[0].mValue = _mm_mul_ps(det, minor0);
606 result.mCol[1].mValue = _mm_mul_ps(det, minor1);
607 result.mCol[2].mValue = _mm_mul_ps(det, minor2);
608 result.mCol[3].mValue = _mm_mul_ps(det, minor3);
609 return result;
610#elif defined(JPH_USE_NEON)
611 // Adapted from the SSE version, there's surprising few articles about efficient ways of calculating an inverse for ARM on the internet
612 Type tmp1 = JPH_NEON_SHUFFLE_F32x4(mCol[0].mValue, mCol[1].mValue, 0, 1, 4, 5);
613 Type row1 = JPH_NEON_SHUFFLE_F32x4(mCol[2].mValue, mCol[3].mValue, 0, 1, 4, 5);
614 Type row0 = JPH_NEON_SHUFFLE_F32x4(tmp1, row1, 0, 2, 4, 6);
615 row1 = JPH_NEON_SHUFFLE_F32x4(row1, tmp1, 1, 3, 5, 7);
616 tmp1 = JPH_NEON_SHUFFLE_F32x4(mCol[0].mValue, mCol[1].mValue, 2, 3, 6, 7);
617 Type row3 = JPH_NEON_SHUFFLE_F32x4(mCol[2].mValue, mCol[3].mValue, 2, 3, 6, 7);
618 Type row2 = JPH_NEON_SHUFFLE_F32x4(tmp1, row3, 0, 2, 4, 6);
619 row3 = JPH_NEON_SHUFFLE_F32x4(row3, tmp1, 1, 3, 5, 7);
620
621 tmp1 = vmulq_f32(row2, row3);
622 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
623 Type minor0 = vmulq_f32(row1, tmp1);
624 Type minor1 = vmulq_f32(row0, tmp1);
625 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
626 minor0 = vsubq_f32(vmulq_f32(row1, tmp1), minor0);
627 minor1 = vsubq_f32(vmulq_f32(row0, tmp1), minor1);
628 minor1 = JPH_NEON_SHUFFLE_F32x4(minor1, minor1, 2, 3, 0, 1);
629
630 tmp1 = vmulq_f32(row1, row2);
631 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
632 minor0 = vaddq_f32(vmulq_f32(row3, tmp1), minor0);
633 Type minor3 = vmulq_f32(row0, tmp1);
634 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
635 minor0 = vsubq_f32(minor0, vmulq_f32(row3, tmp1));
636 minor3 = vsubq_f32(vmulq_f32(row0, tmp1), minor3);
637 minor3 = JPH_NEON_SHUFFLE_F32x4(minor3, minor3, 2, 3, 0, 1);
638
639 tmp1 = JPH_NEON_SHUFFLE_F32x4(row1, row1, 2, 3, 0, 1);
640 tmp1 = vmulq_f32(tmp1, row3);
641 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
642 row2 = JPH_NEON_SHUFFLE_F32x4(row2, row2, 2, 3, 0, 1);
643 minor0 = vaddq_f32(vmulq_f32(row2, tmp1), minor0);
644 Type minor2 = vmulq_f32(row0, tmp1);
645 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
646 minor0 = vsubq_f32(minor0, vmulq_f32(row2, tmp1));
647 minor2 = vsubq_f32(vmulq_f32(row0, tmp1), minor2);
648 minor2 = JPH_NEON_SHUFFLE_F32x4(minor2, minor2, 2, 3, 0, 1);
649
650 tmp1 = vmulq_f32(row0, row1);
651 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
652 minor2 = vaddq_f32(vmulq_f32(row3, tmp1), minor2);
653 minor3 = vsubq_f32(vmulq_f32(row2, tmp1), minor3);
654 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
655 minor2 = vsubq_f32(vmulq_f32(row3, tmp1), minor2);
656 minor3 = vsubq_f32(minor3, vmulq_f32(row2, tmp1));
657
658 tmp1 = vmulq_f32(row0, row3);
659 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
660 minor1 = vsubq_f32(minor1, vmulq_f32(row2, tmp1));
661 minor2 = vaddq_f32(vmulq_f32(row1, tmp1), minor2);
662 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
663 minor1 = vaddq_f32(vmulq_f32(row2, tmp1), minor1);
664 minor2 = vsubq_f32(minor2, vmulq_f32(row1, tmp1));
665
666 tmp1 = vmulq_f32(row0, row2);
667 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
668 minor1 = vaddq_f32(vmulq_f32(row3, tmp1), minor1);
669 minor3 = vsubq_f32(minor3, vmulq_f32(row1, tmp1));
670 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
671 minor1 = vsubq_f32(minor1, vmulq_f32(row3, tmp1));
672 minor3 = vaddq_f32(vmulq_f32(row1, tmp1), minor3);
673
674 Type det = vmulq_f32(row0, minor0);
675 det = vdupq_n_f32(vaddvq_f32(det));
676 det = vdivq_f32(vdupq_n_f32(1.0f), det);
677
678 Mat44 result;
679 result.mCol[0].mValue = vmulq_f32(det, minor0);
680 result.mCol[1].mValue = vmulq_f32(det, minor1);
681 result.mCol[2].mValue = vmulq_f32(det, minor2);
682 result.mCol[3].mValue = vmulq_f32(det, minor3);
683 return result;
684#else
685 float m00 = JPH_EL(0, 0), m10 = JPH_EL(1, 0), m20 = JPH_EL(2, 0), m30 = JPH_EL(3, 0);
686 float m01 = JPH_EL(0, 1), m11 = JPH_EL(1, 1), m21 = JPH_EL(2, 1), m31 = JPH_EL(3, 1);
687 float m02 = JPH_EL(0, 2), m12 = JPH_EL(1, 2), m22 = JPH_EL(2, 2), m32 = JPH_EL(3, 2);
688 float m03 = JPH_EL(0, 3), m13 = JPH_EL(1, 3), m23 = JPH_EL(2, 3), m33 = JPH_EL(3, 3);
689
690 float m10211120 = m10 * m21 - m11 * m20;
691 float m10221220 = m10 * m22 - m12 * m20;
692 float m10231320 = m10 * m23 - m13 * m20;
693 float m10311130 = m10 * m31 - m11 * m30;
694 float m10321230 = m10 * m32 - m12 * m30;
695 float m10331330 = m10 * m33 - m13 * m30;
696 float m11221221 = m11 * m22 - m12 * m21;
697 float m11231321 = m11 * m23 - m13 * m21;
698 float m11321231 = m11 * m32 - m12 * m31;
699 float m11331331 = m11 * m33 - m13 * m31;
700 float m12231322 = m12 * m23 - m13 * m22;
701 float m12331332 = m12 * m33 - m13 * m32;
702 float m20312130 = m20 * m31 - m21 * m30;
703 float m20322230 = m20 * m32 - m22 * m30;
704 float m20332330 = m20 * m33 - m23 * m30;
705 float m21322231 = m21 * m32 - m22 * m31;
706 float m21332331 = m21 * m33 - m23 * m31;
707 float m22332332 = m22 * m33 - m23 * m32;
708
709 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);
710 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);
711 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);
712 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);
713
714 float det = m00 * col0.mF32[0] + m01 * col0.mF32[1] + m02 * col0.mF32[2] + m03 * col0.mF32[3];
715
716 return Mat44(col0 / det, col1 / det, col2 / det, col3 / det);
717#endif
718}
719
726
727float Mat44::GetDeterminant3x3() const
728{
729 return GetAxisX().Dot(GetAxisY().Cross(GetAxisZ()));
730}
731
732Mat44 Mat44::Adjointed3x3() const
733{
734 // Adapted from Inversed() to remove 4th column and the division by the determinant
735 // Note: This can be optimized.
736
737 JPH_ASSERT(mCol[0][3] == 0.0f);
738 JPH_ASSERT(mCol[1][3] == 0.0f);
739 JPH_ASSERT(mCol[2][3] == 0.0f);
740
741#if defined(JPH_USE_SSE)
742 __m128 tmp1 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(1, 0, 1, 0));
743 __m128 row1 = _mm_shuffle_ps(mCol[2].mValue, _mm_setzero_ps(), _MM_SHUFFLE(1, 0, 1, 0));
744 __m128 row0 = _mm_shuffle_ps(tmp1, row1, _MM_SHUFFLE(2, 0, 2, 0));
745 row1 = _mm_shuffle_ps(row1, tmp1, _MM_SHUFFLE(3, 1, 3, 1));
746 tmp1 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(3, 2, 3, 2));
747 __m128 row3 = _mm_shuffle_ps(mCol[2].mValue, _mm_set_ps(1, 0, 0, 0), _MM_SHUFFLE(3, 2, 3, 2));
748 __m128 row2 = _mm_shuffle_ps(tmp1, row3, _MM_SHUFFLE(2, 0, 2, 0));
749 row3 = _mm_shuffle_ps(row3, tmp1, _MM_SHUFFLE(3, 1, 3, 1));
750
751 tmp1 = _mm_mul_ps(row2, row3);
752 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
753 __m128 minor0 = _mm_mul_ps(row1, tmp1);
754 __m128 minor1 = _mm_mul_ps(row0, tmp1);
755 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
756 minor0 = _mm_sub_ps(_mm_mul_ps(row1, tmp1), minor0);
757 minor1 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor1);
758 minor1 = _mm_shuffle_ps(minor1, minor1, _MM_SHUFFLE(1, 0, 3, 2));
759
760 tmp1 = _mm_mul_ps(row1, row2);
761 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
762 minor0 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor0);
763 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
764 minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row3, tmp1));
765
766 tmp1 = _mm_mul_ps(_mm_shuffle_ps(row1, row1, _MM_SHUFFLE(1, 0, 3, 2)), row3);
767 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
768 row2 = _mm_shuffle_ps(row2, row2, _MM_SHUFFLE(1, 0, 3, 2));
769 minor0 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor0);
770 __m128 minor2 = _mm_mul_ps(row0, tmp1);
771 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
772 minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row2, tmp1));
773 minor2 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor2);
774 minor2 = _mm_shuffle_ps(minor2, minor2, _MM_SHUFFLE(1, 0, 3, 2));
775
776 tmp1 = _mm_mul_ps(row0, row1);
777 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
778 minor2 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor2);
779 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
780 minor2 = _mm_sub_ps(_mm_mul_ps(row3, tmp1), minor2);
781
782 tmp1 = _mm_mul_ps(row0, row3);
783 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
784 minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row2, tmp1));
785 minor2 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor2);
786 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
787 minor1 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor1);
788 minor2 = _mm_sub_ps(minor2, _mm_mul_ps(row1, tmp1));
789
790 tmp1 = _mm_mul_ps(row0, row2);
791 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
792 minor1 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor1);
793 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
794 minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row3, tmp1));
795
796 Mat44 result;
797 result.mCol[0].mValue = minor0;
798 result.mCol[1].mValue = minor1;
799 result.mCol[2].mValue = minor2;
800 result.mCol[3] = Vec4(0, 0, 0, 1);
801 return result;
802#elif defined(JPH_USE_NEON)
803 Type v0001 = vsetq_lane_f32(1, vdupq_n_f32(0), 3);
804 Type tmp1 = JPH_NEON_SHUFFLE_F32x4(mCol[0].mValue, mCol[1].mValue, 0, 1, 4, 5);
805 Type row1 = JPH_NEON_SHUFFLE_F32x4(mCol[2].mValue, v0001, 0, 1, 4, 5);
806 Type row0 = JPH_NEON_SHUFFLE_F32x4(tmp1, row1, 0, 2, 4, 6);
807 row1 = JPH_NEON_SHUFFLE_F32x4(row1, tmp1, 1, 3, 5, 7);
808 tmp1 = JPH_NEON_SHUFFLE_F32x4(mCol[0].mValue, mCol[1].mValue, 2, 3, 6, 7);
809 Type row3 = JPH_NEON_SHUFFLE_F32x4(mCol[2].mValue, v0001, 2, 3, 6, 7);
810 Type row2 = JPH_NEON_SHUFFLE_F32x4(tmp1, row3, 0, 2, 4, 6);
811 row3 = JPH_NEON_SHUFFLE_F32x4(row3, tmp1, 1, 3, 5, 7);
812
813 tmp1 = vmulq_f32(row2, row3);
814 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
815 Type minor0 = vmulq_f32(row1, tmp1);
816 Type minor1 = vmulq_f32(row0, tmp1);
817 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
818 minor0 = vsubq_f32(vmulq_f32(row1, tmp1), minor0);
819 minor1 = vsubq_f32(vmulq_f32(row0, tmp1), minor1);
820 minor1 = JPH_NEON_SHUFFLE_F32x4(minor1, minor1, 2, 3, 0, 1);
821
822 tmp1 = vmulq_f32(row1, row2);
823 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
824 minor0 = vaddq_f32(vmulq_f32(row3, tmp1), minor0);
825 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
826 minor0 = vsubq_f32(minor0, vmulq_f32(row3, tmp1));
827
828 tmp1 = JPH_NEON_SHUFFLE_F32x4(row1, row1, 2, 3, 0, 1);
829 tmp1 = vmulq_f32(tmp1, row3);
830 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
831 row2 = JPH_NEON_SHUFFLE_F32x4(row2, row2, 2, 3, 0, 1);
832 minor0 = vaddq_f32(vmulq_f32(row2, tmp1), minor0);
833 Type minor2 = vmulq_f32(row0, tmp1);
834 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
835 minor0 = vsubq_f32(minor0, vmulq_f32(row2, tmp1));
836 minor2 = vsubq_f32(vmulq_f32(row0, tmp1), minor2);
837 minor2 = JPH_NEON_SHUFFLE_F32x4(minor2, minor2, 2, 3, 0, 1);
838
839 tmp1 = vmulq_f32(row0, row1);
840 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
841 minor2 = vaddq_f32(vmulq_f32(row3, tmp1), minor2);
842 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
843 minor2 = vsubq_f32(vmulq_f32(row3, tmp1), minor2);
844
845 tmp1 = vmulq_f32(row0, row3);
846 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
847 minor1 = vsubq_f32(minor1, vmulq_f32(row2, tmp1));
848 minor2 = vaddq_f32(vmulq_f32(row1, tmp1), minor2);
849 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
850 minor1 = vaddq_f32(vmulq_f32(row2, tmp1), minor1);
851 minor2 = vsubq_f32(minor2, vmulq_f32(row1, tmp1));
852
853 tmp1 = vmulq_f32(row0, row2);
854 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
855 minor1 = vaddq_f32(vmulq_f32(row3, tmp1), minor1);
856 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
857 minor1 = vsubq_f32(minor1, vmulq_f32(row3, tmp1));
858
859 Mat44 result;
860 result.mCol[0].mValue = minor0;
861 result.mCol[1].mValue = minor1;
862 result.mCol[2].mValue = minor2;
863 result.mCol[3].mValue = v0001;
864 return result;
865#else
866 return Mat44(
867 Vec4(JPH_EL(1, 1) * JPH_EL(2, 2) - JPH_EL(1, 2) * JPH_EL(2, 1),
868 JPH_EL(1, 2) * JPH_EL(2, 0) - JPH_EL(1, 0) * JPH_EL(2, 2),
869 JPH_EL(1, 0) * JPH_EL(2, 1) - JPH_EL(1, 1) * JPH_EL(2, 0),
870 0),
871 Vec4(JPH_EL(0, 2) * JPH_EL(2, 1) - JPH_EL(0, 1) * JPH_EL(2, 2),
872 JPH_EL(0, 0) * JPH_EL(2, 2) - JPH_EL(0, 2) * JPH_EL(2, 0),
873 JPH_EL(0, 1) * JPH_EL(2, 0) - JPH_EL(0, 0) * JPH_EL(2, 1),
874 0),
875 Vec4(JPH_EL(0, 1) * JPH_EL(1, 2) - JPH_EL(0, 2) * JPH_EL(1, 1),
876 JPH_EL(0, 2) * JPH_EL(1, 0) - JPH_EL(0, 0) * JPH_EL(1, 2),
877 JPH_EL(0, 0) * JPH_EL(1, 1) - JPH_EL(0, 1) * JPH_EL(1, 0),
878 0),
879 Vec4(0, 0, 0, 1));
880#endif
881}
882
883Mat44 Mat44::Inversed3x3() const
884{
885 // Adapted from Inversed() to remove 4th column
886 // Note: This can be optimized.
887
888 JPH_ASSERT(mCol[0][3] == 0.0f);
889 JPH_ASSERT(mCol[1][3] == 0.0f);
890 JPH_ASSERT(mCol[2][3] == 0.0f);
891
892#if defined(JPH_USE_SSE)
893 __m128 tmp1 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(1, 0, 1, 0));
894 __m128 row1 = _mm_shuffle_ps(mCol[2].mValue, _mm_setzero_ps(), _MM_SHUFFLE(1, 0, 1, 0));
895 __m128 row0 = _mm_shuffle_ps(tmp1, row1, _MM_SHUFFLE(2, 0, 2, 0));
896 row1 = _mm_shuffle_ps(row1, tmp1, _MM_SHUFFLE(3, 1, 3, 1));
897 tmp1 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(3, 2, 3, 2));
898 __m128 row3 = _mm_shuffle_ps(mCol[2].mValue, _mm_set_ps(1, 0, 0, 0), _MM_SHUFFLE(3, 2, 3, 2));
899 __m128 row2 = _mm_shuffle_ps(tmp1, row3, _MM_SHUFFLE(2, 0, 2, 0));
900 row3 = _mm_shuffle_ps(row3, tmp1, _MM_SHUFFLE(3, 1, 3, 1));
901
902 tmp1 = _mm_mul_ps(row2, row3);
903 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
904 __m128 minor0 = _mm_mul_ps(row1, tmp1);
905 __m128 minor1 = _mm_mul_ps(row0, tmp1);
906 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
907 minor0 = _mm_sub_ps(_mm_mul_ps(row1, tmp1), minor0);
908 minor1 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor1);
909 minor1 = _mm_shuffle_ps(minor1, minor1, _MM_SHUFFLE(1, 0, 3, 2));
910
911 tmp1 = _mm_mul_ps(row1, row2);
912 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
913 minor0 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor0);
914 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
915 minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row3, tmp1));
916
917 tmp1 = _mm_mul_ps(_mm_shuffle_ps(row1, row1, _MM_SHUFFLE(1, 0, 3, 2)), row3);
918 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
919 row2 = _mm_shuffle_ps(row2, row2, _MM_SHUFFLE(1, 0, 3, 2));
920 minor0 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor0);
921 __m128 minor2 = _mm_mul_ps(row0, tmp1);
922 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
923 minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row2, tmp1));
924 minor2 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor2);
925 minor2 = _mm_shuffle_ps(minor2, minor2, _MM_SHUFFLE(1, 0, 3, 2));
926
927 tmp1 = _mm_mul_ps(row0, row1);
928 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
929 minor2 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor2);
930 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
931 minor2 = _mm_sub_ps(_mm_mul_ps(row3, tmp1), minor2);
932
933 tmp1 = _mm_mul_ps(row0, row3);
934 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
935 minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row2, tmp1));
936 minor2 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor2);
937 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
938 minor1 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor1);
939 minor2 = _mm_sub_ps(minor2, _mm_mul_ps(row1, tmp1));
940
941 tmp1 = _mm_mul_ps(row0, row2);
942 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
943 minor1 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor1);
944 tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
945 minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row3, tmp1));
946
947 __m128 det = _mm_mul_ps(row0, minor0);
948 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
949 det = _mm_add_ss(_mm_shuffle_ps(det, det, _MM_SHUFFLE(1, 0, 3, 2)), det);
950 det = _mm_div_ss(_mm_set_ss(1.0f), det);
951 det = _mm_shuffle_ps(det, det, _MM_SHUFFLE(0, 0, 0, 0));
952
953 Mat44 result;
954 result.mCol[0].mValue = _mm_mul_ps(det, minor0);
955 result.mCol[1].mValue = _mm_mul_ps(det, minor1);
956 result.mCol[2].mValue = _mm_mul_ps(det, minor2);
957 result.mCol[3] = Vec4(0, 0, 0, 1);
958 return result;
959#elif defined(JPH_USE_NEON)
960 Type v0001 = vsetq_lane_f32(1, vdupq_n_f32(0), 3);
961 Type tmp1 = JPH_NEON_SHUFFLE_F32x4(mCol[0].mValue, mCol[1].mValue, 0, 1, 4, 5);
962 Type row1 = JPH_NEON_SHUFFLE_F32x4(mCol[2].mValue, v0001, 0, 1, 4, 5);
963 Type row0 = JPH_NEON_SHUFFLE_F32x4(tmp1, row1, 0, 2, 4, 6);
964 row1 = JPH_NEON_SHUFFLE_F32x4(row1, tmp1, 1, 3, 5, 7);
965 tmp1 = JPH_NEON_SHUFFLE_F32x4(mCol[0].mValue, mCol[1].mValue, 2, 3, 6, 7);
966 Type row3 = JPH_NEON_SHUFFLE_F32x4(mCol[2].mValue, v0001, 2, 3, 6, 7);
967 Type row2 = JPH_NEON_SHUFFLE_F32x4(tmp1, row3, 0, 2, 4, 6);
968 row3 = JPH_NEON_SHUFFLE_F32x4(row3, tmp1, 1, 3, 5, 7);
969
970 tmp1 = vmulq_f32(row2, row3);
971 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
972 Type minor0 = vmulq_f32(row1, tmp1);
973 Type minor1 = vmulq_f32(row0, tmp1);
974 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
975 minor0 = vsubq_f32(vmulq_f32(row1, tmp1), minor0);
976 minor1 = vsubq_f32(vmulq_f32(row0, tmp1), minor1);
977 minor1 = JPH_NEON_SHUFFLE_F32x4(minor1, minor1, 2, 3, 0, 1);
978
979 tmp1 = vmulq_f32(row1, row2);
980 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
981 minor0 = vaddq_f32(vmulq_f32(row3, tmp1), minor0);
982 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
983 minor0 = vsubq_f32(minor0, vmulq_f32(row3, tmp1));
984
985 tmp1 = JPH_NEON_SHUFFLE_F32x4(row1, row1, 2, 3, 0, 1);
986 tmp1 = vmulq_f32(tmp1, row3);
987 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
988 row2 = JPH_NEON_SHUFFLE_F32x4(row2, row2, 2, 3, 0, 1);
989 minor0 = vaddq_f32(vmulq_f32(row2, tmp1), minor0);
990 Type minor2 = vmulq_f32(row0, tmp1);
991 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
992 minor0 = vsubq_f32(minor0, vmulq_f32(row2, tmp1));
993 minor2 = vsubq_f32(vmulq_f32(row0, tmp1), minor2);
994 minor2 = JPH_NEON_SHUFFLE_F32x4(minor2, minor2, 2, 3, 0, 1);
995
996 tmp1 = vmulq_f32(row0, row1);
997 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
998 minor2 = vaddq_f32(vmulq_f32(row3, tmp1), minor2);
999 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
1000 minor2 = vsubq_f32(vmulq_f32(row3, tmp1), minor2);
1001
1002 tmp1 = vmulq_f32(row0, row3);
1003 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
1004 minor1 = vsubq_f32(minor1, vmulq_f32(row2, tmp1));
1005 minor2 = vaddq_f32(vmulq_f32(row1, tmp1), minor2);
1006 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
1007 minor1 = vaddq_f32(vmulq_f32(row2, tmp1), minor1);
1008 minor2 = vsubq_f32(minor2, vmulq_f32(row1, tmp1));
1009
1010 tmp1 = vmulq_f32(row0, row2);
1011 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
1012 minor1 = vaddq_f32(vmulq_f32(row3, tmp1), minor1);
1013 tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
1014 minor1 = vsubq_f32(minor1, vmulq_f32(row3, tmp1));
1015
1016 Type det = vmulq_f32(row0, minor0);
1017 det = vdupq_n_f32(vaddvq_f32(det));
1018 det = vdivq_f32(vdupq_n_f32(1.0f), det);
1019
1020 Mat44 result;
1021 result.mCol[0].mValue = vmulq_f32(det, minor0);
1022 result.mCol[1].mValue = vmulq_f32(det, minor1);
1023 result.mCol[2].mValue = vmulq_f32(det, minor2);
1024 result.mCol[3].mValue = v0001;
1025 return result;
1026#else
1027 float det = GetDeterminant3x3();
1028
1029 return Mat44(
1030 Vec4((JPH_EL(1, 1) * JPH_EL(2, 2) - JPH_EL(1, 2) * JPH_EL(2, 1)) / det,
1031 (JPH_EL(1, 2) * JPH_EL(2, 0) - JPH_EL(1, 0) * JPH_EL(2, 2)) / det,
1032 (JPH_EL(1, 0) * JPH_EL(2, 1) - JPH_EL(1, 1) * JPH_EL(2, 0)) / det,
1033 0),
1034 Vec4((JPH_EL(0, 2) * JPH_EL(2, 1) - JPH_EL(0, 1) * JPH_EL(2, 2)) / det,
1035 (JPH_EL(0, 0) * JPH_EL(2, 2) - JPH_EL(0, 2) * JPH_EL(2, 0)) / det,
1036 (JPH_EL(0, 1) * JPH_EL(2, 0) - JPH_EL(0, 0) * JPH_EL(2, 1)) / det,
1037 0),
1038 Vec4((JPH_EL(0, 1) * JPH_EL(1, 2) - JPH_EL(0, 2) * JPH_EL(1, 1)) / det,
1039 (JPH_EL(0, 2) * JPH_EL(1, 0) - JPH_EL(0, 0) * JPH_EL(1, 2)) / det,
1040 (JPH_EL(0, 0) * JPH_EL(1, 1) - JPH_EL(0, 1) * JPH_EL(1, 0)) / det,
1041 0),
1042 Vec4(0, 0, 0, 1));
1043#endif
1044}
1045
1046Quat Mat44::GetQuaternion() const
1047{
1048 JPH_ASSERT(mCol[3] == Vec4(0, 0, 0, 1));
1049
1050 float tr = mCol[0].mF32[0] + mCol[1].mF32[1] + mCol[2].mF32[2];
1051
1052 if (tr >= 0.0f)
1053 {
1054 float s = sqrt(tr + 1.0f);
1055 float is = 0.5f / s;
1056 return Quat(
1057 (mCol[1].mF32[2] - mCol[2].mF32[1]) * is,
1058 (mCol[2].mF32[0] - mCol[0].mF32[2]) * is,
1059 (mCol[0].mF32[1] - mCol[1].mF32[0]) * is,
1060 0.5f * s);
1061 }
1062 else
1063 {
1064 int i = 0;
1065 if (mCol[1].mF32[1] > mCol[0].mF32[0]) i = 1;
1066 if (mCol[2].mF32[2] > mCol[i].mF32[i]) i = 2;
1067
1068 if (i == 0)
1069 {
1070 float s = sqrt(mCol[0].mF32[0] - (mCol[1].mF32[1] + mCol[2].mF32[2]) + 1);
1071 float is = 0.5f / s;
1072 return Quat(
1073 0.5f * s,
1074 (mCol[1].mF32[0] + mCol[0].mF32[1]) * is,
1075 (mCol[0].mF32[2] + mCol[2].mF32[0]) * is,
1076 (mCol[1].mF32[2] - mCol[2].mF32[1]) * is);
1077 }
1078 else if (i == 1)
1079 {
1080 float s = sqrt(mCol[1].mF32[1] - (mCol[2].mF32[2] + mCol[0].mF32[0]) + 1);
1081 float is = 0.5f / s;
1082 return Quat(
1083 (mCol[1].mF32[0] + mCol[0].mF32[1]) * is,
1084 0.5f * s,
1085 (mCol[2].mF32[1] + mCol[1].mF32[2]) * is,
1086 (mCol[2].mF32[0] - mCol[0].mF32[2]) * is);
1087 }
1088 else
1089 {
1090 JPH_ASSERT(i == 2);
1091
1092 float s = sqrt(mCol[2].mF32[2] - (mCol[0].mF32[0] + mCol[1].mF32[1]) + 1);
1093 float is = 0.5f / s;
1094 return Quat(
1095 (mCol[0].mF32[2] + mCol[2].mF32[0]) * is,
1096 (mCol[2].mF32[1] + mCol[1].mF32[2]) * is,
1097 0.5f * s,
1098 (mCol[0].mF32[1] - mCol[1].mF32[0]) * is);
1099 }
1100 }
1101}
1102
1103Mat44 Mat44::sQuatLeftMultiply(QuatArg inQ)
1104{
1105 return Mat44(
1106 Vec4(1, 1, -1, -1) * inQ.mValue.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X>(),
1107 Vec4(-1, 1, 1, -1) * inQ.mValue.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y>(),
1108 Vec4(1, -1, 1, -1) * inQ.mValue.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>(),
1109 inQ.mValue);
1110}
1111
1112Mat44 Mat44::sQuatRightMultiply(QuatArg inQ)
1113{
1114 return Mat44(
1115 Vec4(1, -1, 1, -1) * inQ.mValue.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X>(),
1116 Vec4(1, 1, -1, -1) * inQ.mValue.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y>(),
1117 Vec4(-1, 1, 1, -1) * inQ.mValue.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>(),
1118 inQ.mValue);
1119}
1120
1121Mat44 Mat44::GetRotation() const
1122{
1123 JPH_ASSERT(mCol[0][3] == 0.0f);
1124 JPH_ASSERT(mCol[1][3] == 0.0f);
1125 JPH_ASSERT(mCol[2][3] == 0.0f);
1126
1127 return Mat44(mCol[0], mCol[1], mCol[2], Vec4(0, 0, 0, 1));
1128}
1129
1130Mat44 Mat44::GetRotationSafe() const
1131{
1132#if defined(JPH_USE_AVX512)
1133 return Mat44(_mm_maskz_mov_ps(0b0111, mCol[0].mValue),
1134 _mm_maskz_mov_ps(0b0111, mCol[1].mValue),
1135 _mm_maskz_mov_ps(0b0111, mCol[2].mValue),
1136 Vec4(0, 0, 0, 1));
1137#elif defined(JPH_USE_SSE4_1)
1138 __m128 zero = _mm_setzero_ps();
1139 return Mat44(_mm_blend_ps(mCol[0].mValue, zero, 8),
1140 _mm_blend_ps(mCol[1].mValue, zero, 8),
1141 _mm_blend_ps(mCol[2].mValue, zero, 8),
1142 Vec4(0, 0, 0, 1));
1143#elif defined(JPH_USE_NEON)
1144 return Mat44(vsetq_lane_f32(0, mCol[0].mValue, 3),
1145 vsetq_lane_f32(0, mCol[1].mValue, 3),
1146 vsetq_lane_f32(0, mCol[2].mValue, 3),
1147 Vec4(0, 0, 0, 1));
1148#else
1149 return Mat44(Vec4(mCol[0].mF32[0], mCol[0].mF32[1], mCol[0].mF32[2], 0),
1150 Vec4(mCol[1].mF32[0], mCol[1].mF32[1], mCol[1].mF32[2], 0),
1151 Vec4(mCol[2].mF32[0], mCol[2].mF32[1], mCol[2].mF32[2], 0),
1152 Vec4(0, 0, 0, 1));
1153#endif
1154}
1155
1156void Mat44::SetRotation(Mat44Arg inRotation)
1157{
1158 mCol[0] = inRotation.mCol[0];
1159 mCol[1] = inRotation.mCol[1];
1160 mCol[2] = inRotation.mCol[2];
1161}
1162
1163Mat44 Mat44::PreTranslated(Vec3Arg inTranslation) const
1164{
1165 return Mat44(mCol[0], mCol[1], mCol[2], Vec4(GetTranslation() + Multiply3x3(inTranslation), 1));
1166}
1167
1168Mat44 Mat44::PostTranslated(Vec3Arg inTranslation) const
1169{
1170 return Mat44(mCol[0], mCol[1], mCol[2], Vec4(GetTranslation() + inTranslation, 1));
1171}
1172
1173Mat44 Mat44::PreScaled(Vec3Arg inScale) const
1174{
1175 return Mat44(inScale.GetX() * mCol[0], inScale.GetY() * mCol[1], inScale.GetZ() * mCol[2], mCol[3]);
1176}
1177
1178Mat44 Mat44::PostScaled(Vec3Arg inScale) const
1179{
1180 Vec4 scale(inScale, 1);
1181 return Mat44(scale * mCol[0], scale * mCol[1], scale * mCol[2], scale * mCol[3]);
1182}
1183
1184Mat44 Mat44::Decompose(Vec3 &outScale) const
1185{
1186 // Start the modified Gram-Schmidt algorithm
1187 // X axis will just be normalized
1188 Vec3 x = GetAxisX();
1189
1190 // Make Y axis perpendicular to X
1191 Vec3 y = GetAxisY();
1192 float x_dot_x = x.LengthSq();
1193 y -= (x.Dot(y) / x_dot_x) * x;
1194
1195 // Make Z axis perpendicular to X
1196 Vec3 z = GetAxisZ();
1197 z -= (x.Dot(z) / x_dot_x) * x;
1198
1199 // Make Z axis perpendicular to Y
1200 float y_dot_y = y.LengthSq();
1201 z -= (y.Dot(z) / y_dot_y) * y;
1202
1203 // Determine the scale
1204 float z_dot_z = z.LengthSq();
1205 outScale = Vec3(x_dot_x, y_dot_y, z_dot_z).Sqrt();
1206
1207 // If the resulting x, y and z vectors don't form a right handed matrix, flip the z axis.
1208 if (x.Cross(y).Dot(z) < 0.0f)
1209 outScale.SetZ(-outScale.GetZ());
1210
1211 // Determine the rotation and translation
1212 return Mat44(Vec4(x / outScale.GetX(), 0), Vec4(y / outScale.GetY(), 0), Vec4(z / outScale.GetZ(), 0), GetColumn4(3));
1213}
1214
1215#undef JPH_EL
1216
1217JPH_NAMESPACE_END
JPH_NAMESPACE_END
#define JPH_NAMESPACE_END
Definition Core.h:240
JPH_NAMESPACE_BEGIN
#define JPH_NAMESPACE_BEGIN
Definition Core.h:234
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
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:144
Mat44::PostTranslated
JPH_INLINE Mat44 PostTranslated(Vec3Arg inTranslation) const
Post multiply by translation matrix: result = Mat44::sTranslation(inTranslation) * this (i....
Definition Mat44.inl:1168
Mat44::PreTranslated
JPH_INLINE Mat44 PreTranslated(Vec3Arg inTranslation) const
Pre multiply by translation matrix: result = this * Mat44::sTranslation(inTranslation)
Definition Mat44.inl:1163
Mat44::GetAxisZ
JPH_INLINE Vec3 GetAxisZ() const
Definition Mat44.h:146
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:374
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:1046
Mat44::StoreFloat4x4
JPH_INLINE void StoreFloat4x4(Float4 *outV) const
Store matrix to memory.
Definition Mat44.inl:452
Mat44::operator-
JPH_INLINE Mat44 operator-() const
Negate.
Definition Mat44.inl:428
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:493
Mat44::operator*=
JPH_INLINE Mat44 & operator*=(float inV)
Multiply matrix with float.
Definition Mat44.inl:412
Mat44::GetDeterminant3x3
JPH_INLINE float GetDeterminant3x3() const
Get the determinant of a 3x3 matrix.
Definition Mat44.inl:727
Mat44::sQuatRightMultiply
static JPH_INLINE Mat44 sQuatRightMultiply(QuatArg inQ)
Returns matrix MR so that (where p and q are quaternions)
Definition Mat44.inl:1112
Mat44::Transposed
JPH_INLINE Mat44 Transposed() const
Transpose matrix.
Definition Mat44.inl:458
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:732
Mat44::operator==
JPH_INLINE bool operator==(Mat44Arg inM2) const
Comparsion.
Definition Mat44.inl:216
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:327
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:1121
Mat44::GetRotationSafe
JPH_INLINE Mat44 GetRotationSafe() const
Get rotation part only (note: also clears the bottom row)
Definition Mat44.inl:1130
Mat44::Multiply3x3RightTransposed
JPH_INLINE Mat44 Multiply3x3RightTransposed(Mat44Arg inM) const
Multiply 3x3 matrix by the transpose of a 3x3 matrix ( )
Definition Mat44.inl:388
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:444
Mat44::PostScaled
JPH_INLINE Mat44 PostScaled(Vec3Arg inScale) const
Scale a matrix: result = Mat44::sScale(inScale) * this.
Definition Mat44.inl:1178
Mat44::IsClose
JPH_INLINE bool IsClose(Mat44Arg inM2, float inMaxDistSq=1.0e-12f) const
Test if two matrices are close.
Definition Mat44.inl:224
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:156
Mat44::Decompose
JPH_INLINE Mat44 Decompose(Vec3 &outScale) const
Definition Mat44.inl:1184
Mat44::GetAxisX
JPH_INLINE Vec3 GetAxisX() const
Access to the columns.
Definition Mat44.h:142
Mat44::Inversed
JPH_INLINE Mat44 Inversed() const
Inverse 4x4 matrix.
Definition Mat44.inl:529
Mat44::Multiply3x3
JPH_INLINE Vec3 Multiply3x3(Vec3Arg inV) const
Multiply vector by only 3x3 part of the matrix.
Definition Mat44.inl:307
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:1156
Mat44::GetTranslation
JPH_INLINE Vec3 GetTranslation() const
Definition Mat44.h:148
Mat44::operator+
JPH_INLINE Mat44 operator+(Mat44Arg inM) const
Per element addition of matrix.
Definition Mat44.inl:420
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:883
Mat44::sQuatLeftMultiply
static JPH_INLINE Mat44 sQuatLeftMultiply(QuatArg inQ)
Returns matrix ML so that (where p and q are quaternions)
Definition Mat44.inl:1103
Mat44::SetTranslation
JPH_INLINE void SetTranslation(Vec3Arg inV)
Definition Mat44.h:149
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:720
Mat44::PreScaled
JPH_INLINE Mat44 PreScaled(Vec3Arg inScale) const
Scale a matrix: result = this * Mat44::sScale(inScale)
Definition Mat44.inl:1173
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:124
Quat
Definition Quat.h:33
Quat::GetW
JPH_INLINE float GetW() const
Get W component (real part)
Definition Quat.h:77
Quat::GetY
JPH_INLINE float GetY() const
Get Y component (imaginary part j)
Definition Quat.h:71
Quat::GetZ
JPH_INLINE float GetZ() const
Get Z component (imaginary part k)
Definition Quat.h:74
Quat::sRotation
static JPH_INLINE Quat sRotation(Vec3Arg inAxis, float inAngle)
Rotation from axis and angle.
Definition Quat.inl:74
Quat::GetX
JPH_INLINE float GetX() const
Get X component (imaginary part i)
Definition Quat.h:68
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:168
Quat::IsNormalized
bool IsNormalized(float inTolerance=1.0e-5f) const
If the length of this quaternion is 1 +/- inTolerance.
Definition Quat.h:58
Quat::mValue
Vec4 mValue
4 vector that stores [x, y, z, w] parts of the quaternion
Definition Quat.h:238
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:400
UVec4::sAnd
static JPH_INLINE UVec4 sAnd(UVec4Arg inV1, UVec4Arg inV2)
Logical and (component wise)
Definition UVec4.inl:194
Vec3
Definition Vec3.h:16
Vec3::Dot
JPH_INLINE float Dot(Vec3Arg inV2) const
Dot product.
Definition Vec3.inl:637
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:51
Vec3::SplatX
JPH_INLINE Vec4 SplatX() const
Replicate the X component to all components.
Definition Vec3.inl:521
Vec3::Cross
JPH_INLINE Vec3 Cross(Vec3Arg inV2) const
Cross product.
Definition Vec3.inl:582
Vec3::GetX
JPH_INLINE float GetX() const
Get individual components.
Definition Vec3.h:122
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:708
Vec3::SplatZ
JPH_INLINE Vec4 SplatZ() const
Replicate the Z component to all components.
Definition Vec3.inl:543
Vec3::SetZ
JPH_INLINE void SetZ(float inZ)
Definition Vec3.h:130
Vec3::sAxisZ
static JPH_INLINE Vec3 sAxisZ()
Definition Vec3.h:53
Vec3::mValue
Type mValue
Definition Vec3.h:281
Vec3::GetY
JPH_INLINE float GetY() const
Definition Vec3.h:123
Vec3::SplatY
JPH_INLINE Vec4 SplatY() const
Replicate the Y component to all components.
Definition Vec3.inl:532
Vec3::LengthSq
JPH_INLINE float LengthSq() const
Squared length of vector.
Definition Vec3.inl:653
Vec3::mF32
float mF32[4]
Definition Vec3.h:282
Vec3::Sqrt
JPH_INLINE Vec3 Sqrt() const
Component wise square root.
Definition Vec3.inl:683
Vec3::GetZ
JPH_INLINE float GetZ() const
Definition Vec3.h:124
Vec4
Definition Vec4.h:14
Vec4::SplatX
JPH_INLINE Vec4 SplatX() const
Replicate the X component to all components.
Definition Vec4.inl:547
Vec4::mF32
float mF32[4]
Definition Vec4.h:271
Vec4::sLoadFloat4Aligned
static JPH_INLINE Vec4 sLoadFloat4Aligned(const Float4 *inV)
Load 4 floats from memory, 16 bytes aligned.
Definition Vec4.inl:101
Vec4::sEquals
static JPH_INLINE UVec4 sEquals(Vec4Arg inV1, Vec4Arg inV2)
Equals (component wise)
Definition Vec4.inl:166
Vec4::SplatY
JPH_INLINE Vec4 SplatY() const
Replicate the Y component to all components.
Definition Vec4.inl:558
Vec4::SplatZ
JPH_INLINE Vec4 SplatZ() const
Replicate the Z component to all components.
Definition Vec4.inl:569
Vec4::GetX
JPH_INLINE float GetX() const
Get individual components.
Definition Vec4.h:112
Vec4::sLoadFloat4
static JPH_INLINE Vec4 sLoadFloat4(const Float4 *inV)
Load 4 floats from memory.
Definition Vec4.inl:90
Vec4::sZero
static JPH_INLINE Vec4 sZero()
Vector with all zeros.
Definition Vec4.inl:63
Vec4::Swizzle
JPH_INLINE Vec4 Swizzle() const
Swizzle the elements in inV.
Vec4::mValue
Type mValue
Definition Vec4.h:270
Vec4::sNaN
static JPH_INLINE Vec4 sNaN()
Vector with all NaN's.
Definition Vec4.inl:85
Vec4::sReplicate
static JPH_INLINE Vec4 sReplicate(float inV)
Replicate inV across all components.
Definition Vec4.inl:74
Vec4::SinCos
void SinCos(Vec4 &outSin, Vec4 &outCos) const
Calcluate the sine and cosine for each element of this vector (input in radians)
Definition Vec4.inl:772