Jolt Physics
A multi core friendly Game Physics Engine
Loading...
Searching...
No Matches
DMat44.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/DVec3.h>
8
10
12 mCol { inC1, inC2, inC3 },
13 mCol3(inC4)
14{
15}
16
17DMat44::DMat44(Type inC1, Type inC2, Type inC3, DTypeArg inC4) :
18 mCol { inC1, inC2, inC3 },
19 mCol3(inC4)
20{
21}
22
24 mCol { inM.GetColumn4(0), inM.GetColumn4(1), inM.GetColumn4(2) },
25 mCol3(inM.GetTranslation())
26{
27}
28
30 mCol { inRot.GetColumn4(0), inRot.GetColumn4(1), inRot.GetColumn4(2) },
31 mCol3(inT)
32{
33}
34
39
41{
42 return DMat44(Vec4(1, 0, 0, 0), Vec4(0, 1, 0, 0), Vec4(0, 0, 1, 0), DVec3::sZero());
43}
44
52
54{
55 return mCol[0] == inM2.mCol[0]
56 && mCol[1] == inM2.mCol[1]
57 && mCol[2] == inM2.mCol[2]
58 && mCol3 == inM2.mCol3;
59}
60
61bool DMat44::IsClose(DMat44Arg inM2, float inMaxDistSq) const
62{
63 for (int i = 0; i < 3; ++i)
64 if (!mCol[i].IsClose(inM2.mCol[i], inMaxDistSq))
65 return false;
66 return mCol3.IsClose(inM2.mCol3, double(inMaxDistSq));
67}
68
70{
71#if defined(JPH_USE_AVX)
72 __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(0, 0, 0, 0)));
73 t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(1, 1, 1, 1))));
74 t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(2, 2, 2, 2))));
75 return DVec3::sFixW(_mm256_add_pd(mCol3.mValue, _mm256_cvtps_pd(t)));
76#elif defined(JPH_USE_SSE)
77 __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(0, 0, 0, 0)));
78 t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(1, 1, 1, 1))));
79 t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(2, 2, 2, 2))));
80 __m128d low = _mm_add_pd(mCol3.mValue.mLow, _mm_cvtps_pd(t));
81 __m128d high = _mm_add_pd(mCol3.mValue.mHigh, _mm_cvtps_pd(_mm_shuffle_ps(t, t, _MM_SHUFFLE(2, 2, 2, 2))));
82 return DVec3({ low, high });
83#elif defined(JPH_USE_NEON)
84 float32x4_t t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(inV.mValue, 0));
85 t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(inV.mValue, 1));
86 t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(inV.mValue, 2));
87 float64x2_t low = vaddq_f64(mCol3.mValue.val[0], vcvt_f64_f32(vget_low_f32(t)));
88 float64x2_t high = vaddq_f64(mCol3.mValue.val[1], vcvt_high_f64_f32(t));
89 return DVec3::sFixW({ low, high });
90#else
91 return DVec3(
92 mCol3.mF64[0] + double(mCol[0].mF32[0] * inV.mF32[0] + mCol[1].mF32[0] * inV.mF32[1] + mCol[2].mF32[0] * inV.mF32[2]),
93 mCol3.mF64[1] + double(mCol[0].mF32[1] * inV.mF32[0] + mCol[1].mF32[1] * inV.mF32[1] + mCol[2].mF32[1] * inV.mF32[2]),
94 mCol3.mF64[2] + double(mCol[0].mF32[2] * inV.mF32[0] + mCol[1].mF32[2] * inV.mF32[1] + mCol[2].mF32[2] * inV.mF32[2]));
95#endif
96}
97
99{
100#if defined(JPH_USE_AVX)
101 __m256d t = _mm256_add_pd(mCol3.mValue, _mm256_mul_pd(_mm256_cvtps_pd(mCol[0].mValue), _mm256_set1_pd(inV.mF64[0])));
102 t = _mm256_add_pd(t, _mm256_mul_pd(_mm256_cvtps_pd(mCol[1].mValue), _mm256_set1_pd(inV.mF64[1])));
103 t = _mm256_add_pd(t, _mm256_mul_pd(_mm256_cvtps_pd(mCol[2].mValue), _mm256_set1_pd(inV.mF64[2])));
104 return DVec3::sFixW(t);
105#elif defined(JPH_USE_SSE)
106 __m128d xxxx = _mm_set1_pd(inV.mF64[0]);
107 __m128d yyyy = _mm_set1_pd(inV.mF64[1]);
108 __m128d zzzz = _mm_set1_pd(inV.mF64[2]);
109 __m128 col0 = mCol[0].mValue;
110 __m128 col1 = mCol[1].mValue;
111 __m128 col2 = mCol[2].mValue;
112 __m128d t_low = _mm_add_pd(mCol3.mValue.mLow, _mm_mul_pd(_mm_cvtps_pd(col0), xxxx));
113 t_low = _mm_add_pd(t_low, _mm_mul_pd(_mm_cvtps_pd(col1), yyyy));
114 t_low = _mm_add_pd(t_low, _mm_mul_pd(_mm_cvtps_pd(col2), zzzz));
115 __m128d t_high = _mm_add_pd(mCol3.mValue.mHigh, _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col0, col0, _MM_SHUFFLE(2, 2, 2, 2))), xxxx));
116 t_high = _mm_add_pd(t_high, _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col1, col1, _MM_SHUFFLE(2, 2, 2, 2))), yyyy));
117 t_high = _mm_add_pd(t_high, _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col2, col2, _MM_SHUFFLE(2, 2, 2, 2))), zzzz));
118 return DVec3({ t_low, t_high });
119#elif defined(JPH_USE_NEON)
120 float64x2_t xxxx = vdupq_laneq_f64(inV.mValue.val[0], 0);
121 float64x2_t yyyy = vdupq_laneq_f64(inV.mValue.val[0], 1);
122 float64x2_t zzzz = vdupq_laneq_f64(inV.mValue.val[1], 0);
123 float32x4_t col0 = mCol[0].mValue;
124 float32x4_t col1 = mCol[1].mValue;
125 float32x4_t col2 = mCol[2].mValue;
126 float64x2_t t_low = vaddq_f64(mCol3.mValue.val[0], vmulq_f64(vcvt_f64_f32(vget_low_f32(col0)), xxxx));
127 t_low = vaddq_f64(t_low, vmulq_f64(vcvt_f64_f32(vget_low_f32(col1)), yyyy));
128 t_low = vaddq_f64(t_low, vmulq_f64(vcvt_f64_f32(vget_low_f32(col2)), zzzz));
129 float64x2_t t_high = vaddq_f64(mCol3.mValue.val[1], vmulq_f64(vcvt_high_f64_f32(col0), xxxx));
130 t_high = vaddq_f64(t_high, vmulq_f64(vcvt_high_f64_f32(col1), yyyy));
131 t_high = vaddq_f64(t_high, vmulq_f64(vcvt_high_f64_f32(col2), zzzz));
132 return DVec3::sFixW({ t_low, t_high });
133#else
134 return DVec3(
135 mCol3.mF64[0] + double(mCol[0].mF32[0]) * inV.mF64[0] + double(mCol[1].mF32[0]) * inV.mF64[1] + double(mCol[2].mF32[0]) * inV.mF64[2],
136 mCol3.mF64[1] + double(mCol[0].mF32[1]) * inV.mF64[0] + double(mCol[1].mF32[1]) * inV.mF64[1] + double(mCol[2].mF32[1]) * inV.mF64[2],
137 mCol3.mF64[2] + double(mCol[0].mF32[2]) * inV.mF64[0] + double(mCol[1].mF32[2]) * inV.mF64[1] + double(mCol[2].mF32[2]) * inV.mF64[2]);
138#endif
139}
140
142{
143#if defined(JPH_USE_AVX)
144 __m256d t = _mm256_mul_pd(_mm256_cvtps_pd(mCol[0].mValue), _mm256_set1_pd(inV.mF64[0]));
145 t = _mm256_add_pd(t, _mm256_mul_pd(_mm256_cvtps_pd(mCol[1].mValue), _mm256_set1_pd(inV.mF64[1])));
146 t = _mm256_add_pd(t, _mm256_mul_pd(_mm256_cvtps_pd(mCol[2].mValue), _mm256_set1_pd(inV.mF64[2])));
147 return DVec3::sFixW(t);
148#elif defined(JPH_USE_SSE)
149 __m128d xxxx = _mm_set1_pd(inV.mF64[0]);
150 __m128d yyyy = _mm_set1_pd(inV.mF64[1]);
151 __m128d zzzz = _mm_set1_pd(inV.mF64[2]);
152 __m128 col0 = mCol[0].mValue;
153 __m128 col1 = mCol[1].mValue;
154 __m128 col2 = mCol[2].mValue;
155 __m128d t_low = _mm_mul_pd(_mm_cvtps_pd(col0), xxxx);
156 t_low = _mm_add_pd(t_low, _mm_mul_pd(_mm_cvtps_pd(col1), yyyy));
157 t_low = _mm_add_pd(t_low, _mm_mul_pd(_mm_cvtps_pd(col2), zzzz));
158 __m128d t_high = _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col0, col0, _MM_SHUFFLE(2, 2, 2, 2))), xxxx);
159 t_high = _mm_add_pd(t_high, _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col1, col1, _MM_SHUFFLE(2, 2, 2, 2))), yyyy));
160 t_high = _mm_add_pd(t_high, _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col2, col2, _MM_SHUFFLE(2, 2, 2, 2))), zzzz));
161 return DVec3({ t_low, t_high });
162#elif defined(JPH_USE_NEON)
163 float64x2_t xxxx = vdupq_laneq_f64(inV.mValue.val[0], 0);
164 float64x2_t yyyy = vdupq_laneq_f64(inV.mValue.val[0], 1);
165 float64x2_t zzzz = vdupq_laneq_f64(inV.mValue.val[1], 0);
166 float32x4_t col0 = mCol[0].mValue;
167 float32x4_t col1 = mCol[1].mValue;
168 float32x4_t col2 = mCol[2].mValue;
169 float64x2_t t_low = vmulq_f64(vcvt_f64_f32(vget_low_f32(col0)), xxxx);
170 t_low = vaddq_f64(t_low, vmulq_f64(vcvt_f64_f32(vget_low_f32(col1)), yyyy));
171 t_low = vaddq_f64(t_low, vmulq_f64(vcvt_f64_f32(vget_low_f32(col2)), zzzz));
172 float64x2_t t_high = vmulq_f64(vcvt_high_f64_f32(col0), xxxx);
173 t_high = vaddq_f64(t_high, vmulq_f64(vcvt_high_f64_f32(col1), yyyy));
174 t_high = vaddq_f64(t_high, vmulq_f64(vcvt_high_f64_f32(col2), zzzz));
175 return DVec3::sFixW({ t_low, t_high });
176#else
177 return DVec3(
178 double(mCol[0].mF32[0]) * inV.mF64[0] + double(mCol[1].mF32[0]) * inV.mF64[1] + double(mCol[2].mF32[0]) * inV.mF64[2],
179 double(mCol[0].mF32[1]) * inV.mF64[0] + double(mCol[1].mF32[1]) * inV.mF64[1] + double(mCol[2].mF32[1]) * inV.mF64[2],
180 double(mCol[0].mF32[2]) * inV.mF64[0] + double(mCol[1].mF32[2]) * inV.mF64[1] + double(mCol[2].mF32[2]) * inV.mF64[2]);
181#endif
182}
183
185{
186 DMat44 result;
187
188 // Rotation part
189#if defined(JPH_USE_SSE)
190 for (int i = 0; i < 3; ++i)
191 {
192 __m128 c = inM.GetColumn4(i).mValue;
193 __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(0, 0, 0, 0)));
194 t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(1, 1, 1, 1))));
195 t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(2, 2, 2, 2))));
196 result.mCol[i].mValue = t;
197 }
198#elif defined(JPH_USE_NEON)
199 for (int i = 0; i < 3; ++i)
200 {
201 Type c = inM.GetColumn4(i).mValue;
202 Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(c, 0));
203 t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(c, 1));
204 t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(c, 2));
205 result.mCol[i].mValue = t;
206 }
207#else
208 for (int i = 0; i < 3; ++i)
209 {
210 Vec4 coli = inM.GetColumn4(i);
211 result.mCol[i] = mCol[0] * coli.mF32[0] + mCol[1] * coli.mF32[1] + mCol[2] * coli.mF32[2];
212 }
213#endif
214
215 // Translation part
216 result.mCol3 = *this * inM.GetTranslation();
217
218 return result;
219}
220
222{
223 DMat44 result;
224
225 // Rotation part
226#if defined(JPH_USE_SSE)
227 for (int i = 0; i < 3; ++i)
228 {
229 __m128 c = inM.mCol[i].mValue;
230 __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(0, 0, 0, 0)));
231 t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(1, 1, 1, 1))));
232 t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(2, 2, 2, 2))));
233 result.mCol[i].mValue = t;
234 }
235#elif defined(JPH_USE_NEON)
236 for (int i = 0; i < 3; ++i)
237 {
238 Type c = inM.GetColumn4(i).mValue;
239 Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(c, 0));
240 t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(c, 1));
241 t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(c, 2));
242 result.mCol[i].mValue = t;
243 }
244#else
245 for (int i = 0; i < 3; ++i)
246 {
247 Vec4 coli = inM.mCol[i];
248 result.mCol[i] = mCol[0] * coli.mF32[0] + mCol[1] * coli.mF32[1] + mCol[2] * coli.mF32[2];
249 }
250#endif
251
252 // Translation part
253 result.mCol3 = *this * inM.GetTranslation();
254
255 return result;
256}
257
259{
260 mCol[0] = inRotation.GetColumn4(0);
261 mCol[1] = inRotation.GetColumn4(1);
262 mCol[2] = inRotation.GetColumn4(2);
263}
264
266{
267 return DMat44(inScale.GetX() * mCol[0], inScale.GetY() * mCol[1], inScale.GetZ() * mCol[2], mCol3);
268}
269
271{
272 Vec4 scale(inScale, 1);
273 return DMat44(scale * mCol[0], scale * mCol[1], scale * mCol[2], DVec3(scale) * mCol3);
274}
275
277{
278 return DMat44(mCol[0], mCol[1], mCol[2], GetTranslation() + Multiply3x3(inTranslation));
279}
280
282{
283 return DMat44(mCol[0], mCol[1], mCol[2], GetTranslation() + Multiply3x3(inTranslation));
284}
285
287{
288 return DMat44(mCol[0], mCol[1], mCol[2], GetTranslation() + inTranslation);
289}
290
292{
293 return DMat44(mCol[0], mCol[1], mCol[2], GetTranslation() + inTranslation);
294}
295
297{
298 DMat44 m(GetRotation().Inversed3x3());
299 m.mCol3 = -m.Multiply3x3(mCol3);
300 return m;
301}
302
304{
306 m.mCol3 = -m.Multiply3x3(mCol3);
307 return m;
308}
309
#define JPH_NAMESPACE_END
Definition Core.h:379
#define JPH_NAMESPACE_BEGIN
Definition Core.h:373
Holds a 4x4 matrix of floats with the last column consisting of doubles.
Definition DMat44.h:13
JPH_INLINE bool operator==(DMat44Arg inM2) const
Comparison.
Definition DMat44.inl:53
DVec3::TypeArg DTypeArg
Definition DMat44.h:20
JPH_INLINE void SetTranslation(DVec3Arg inV)
Definition DMat44.h:112
JPH_INLINE Vec4 GetColumn4(uint inCol) const
Definition DMat44.h:115
JPH_INLINE DMat44 PostTranslated(Vec3Arg inTranslation) const
Post multiply by translation matrix: result = Mat44::sTranslation(inTranslation) * this (i....
Definition DMat44.inl:286
JPH_INLINE Mat44 Transposed3x3() const
Transpose 3x3 subpart of matrix.
Definition DMat44.h:119
JPH_INLINE DMat44 PreTranslated(Vec3Arg inTranslation) const
Pre multiply by translation matrix: result = this * Mat44::sTranslation(inTranslation)
Definition DMat44.inl:276
static JPH_INLINE DMat44 sZero()
Zero matrix.
Definition DMat44.inl:35
DMat44()=default
Constructor.
static JPH_INLINE DMat44 sIdentity()
Identity matrix.
Definition DMat44.inl:40
JPH_INLINE Vec3 Multiply3x3(Vec3Arg inV) const
Multiply vector by only 3x3 part of the matrix.
Definition DMat44.h:78
JPH_INLINE DMat44 PostScaled(Vec3Arg inScale) const
Scale a matrix: result = Mat44::sScale(inScale) * this.
Definition DMat44.inl:270
static JPH_INLINE DMat44 sInverseRotationTranslation(QuatArg inR, DVec3Arg inT)
Get inverse matrix of sRotationTranslation.
Definition DMat44.inl:45
JPH_INLINE void SetRotation(Mat44Arg inRotation)
Updates the rotation part of this matrix (the first 3 columns)
Definition DMat44.inl:258
JPH_INLINE DMat44 PreScaled(Vec3Arg inScale) const
Scale a matrix: result = this * Mat44::sScale(inScale)
Definition DMat44.inl:265
JPH_INLINE DMat44 InversedRotationTranslation() const
Inverse 4x4 matrix when it only contains rotation and translation.
Definition DMat44.inl:303
JPH_INLINE DMat44 Inversed() const
Inverse 4x4 matrix.
Definition DMat44.inl:296
JPH_INLINE DVec3 GetTranslation() const
Definition DMat44.h:111
JPH_INLINE DMat44 operator*(Mat44Arg inM) const
Multiply matrix by matrix.
Definition DMat44.inl:184
JPH_INLINE bool IsClose(DMat44Arg inM2, float inMaxDistSq=1.0e-12f) const
Test if two matrices are close.
Definition DMat44.inl:61
JPH_INLINE Mat44 GetRotation() const
Get rotation part only (note: retains the first 3 values from the bottom row)
Definition DMat44.h:128
Vec4::Type Type
Definition DMat44.h:18
Definition DVec3.h:14
double mF64[4]
Definition DVec3.h:280
static JPH_INLINE Type sFixW(TypeArg inValue)
Internal helper function that ensures that the Z component is replicated to the W component to preven...
Definition DVec3.inl:92
Type mValue
Definition DVec3.h:279
JPH_INLINE bool IsClose(DVec3Arg inV2, double inMaxDistSq=1.0e-24) const
Test if two vectors are close.
Definition DVec3.inl:414
static JPH_INLINE DVec3 sZero()
Vector with all zeros.
Definition DVec3.inl:120
Holds a 4x4 matrix of floats, but supports also operations on the 3x3 upper left part of the matrix.
Definition Mat44.h:13
JPH_INLINE Vec4 GetColumn4(uint inCol) const
Definition Mat44.h:160
JPH_INLINE Vec3 GetTranslation() const
Definition Mat44.h:152
static JPH_INLINE Mat44 sRotation(Vec3Arg inAxis, float inAngle)
Rotate around arbitrary axis.
Definition Mat44.inl:139
Definition Quat.h:33
JPH_INLINE Quat Conjugated() const
The conjugate [w, -x, -y, -z] is the same as the inverse for unit quaternions.
Definition Quat.h:178
Definition Vec3.h:17
JPH_INLINE float GetX() const
Get individual components.
Definition Vec3.h:124
Type mValue
Definition Vec3.h:286
JPH_INLINE float GetY() const
Definition Vec3.h:125
float mF32[4]
Definition Vec3.h:287
JPH_INLINE float GetZ() const
Definition Vec3.h:126
Definition Vec4.h:14
float mF32[4]
Definition Vec4.h:275
static JPH_INLINE Vec4 sZero()
Vector with all zeros.
Definition Vec4.inl:63
Type mValue
Definition Vec4.h:274