UDocumentation UE5.7 10.02.2026 (Source)
API documentation for Unreal Engine 5.7
Utilities.h
Go to the documentation of this file.
1// Copyright Epic Games, Inc. All Rights Reserved.
2
3#pragma once
4
5#include "Chaos/Core.h"
6#include "Chaos/Matrix.h"
7#include "Chaos/Transform.h"
8#include "Chaos/UniformGrid.h"
9#include "Chaos/Vector.h"
10
11#include "Math/NumericLimits.h"
12#include "Templates/IsIntegral.h"
13
14namespace Chaos
15{
16 namespace Utilities
17 {
18 // Get the signed square of the input, i.e. (A * Abs(A)) or (Sign(A) * Square(A))
19 // SignedSquare(2) -> 4
20 // SignedSquare(-2) -> -4
21 template<typename TRealType>
22 inline TRealType SignedSquare(TRealType A)
23 {
24 return A * FMath::Abs(A);
25 }
26
28 template<class TINT = uint64>
29 TINT Factorial(TINT Num)
30 {
31 static_assert(TIsIntegral<TINT>::Value, "Templated type must be integral.");
32 TINT Result = Num;
33 while (Num > 2)
34 {
35 Result *= --Num;
36 }
37 return Result;
38 }
39
41 template<class TINT = uint64>
42 TINT NChooseR(const TINT N, const TINT R)
43 {
44 static_assert(TIsIntegral<TINT>::Value, "Templated type must be integral.");
45 return Factorial(N) / (Factorial(R) * Factorial(N - R));
46 }
47
49 template<class TINT = uint64>
50 TINT NPermuteR(const TINT N, const TINT R)
51 {
52 static_assert(TIsIntegral<TINT>::Value, "Templated type must be integral.");
53 return Factorial(N) / Factorial(N - R);
54 }
55
57 template<class T, class TARRAY = TArray<T>>
58 void GetMinAvgMax(const TARRAY& Values, T& MinV, double& AvgV, T& MaxV)
59 {
60 MinV = TNumericLimits<T>::Max();
61 MaxV = TNumericLimits<T>::Lowest();
62 AvgV = 0.0;
63 for (const T& V : Values)
64 {
65 MinV = V < MinV ? V : MinV;
66 MaxV = V > MaxV ? V : MaxV;
67 AvgV += V;
68 }
69 if (Values.Num())
70 {
71 AvgV /= Values.Num();
72 }
73 else
74 {
75 MinV = MaxV = 0;
76 }
77 }
78
80 template<class T, class TARRAY = TArray<T>>
81 T GetAverage(const TARRAY& Values)
82 {
83 double AvgV = 0.0;
84 for (const T& V : Values)
85 {
86 AvgV += V;
87 }
88 if (Values.Num())
89 {
90 AvgV /= Values.Num();
91 }
92 return static_cast<T>(AvgV);
93 }
94
96 template<class T, class TARRAY = TArray<T>>
97 T GetVariance(const TARRAY& Values, const T Avg)
98 {
99 double Variance = 0.0;
100 for (const T& V : Values)
101 {
102 const T Deviation = V - Avg;
103 Variance += Deviation * Deviation;
104 }
105 if (Values.Num())
106 {
107 Variance /= Values.Num();
108 }
109 return Variance;
110 }
111
113 template<class T, class TARRAY = TArray<T>>
114 T GetVariance(const TARRAY& Values)
115 {
116 return GetVariance(Values, GetAverage(Values));
117 }
118
120 template<class T, class TARRAY = TArray<T>>
121 T GetStandardDeviation(const TARRAY& Values, const T Avg)
122 {
123 const T Variance = GetVariance(Values, Avg);
124 return FMath::Sqrt(Variance);
125 }
126
128 template<class T, class TARRAY = TArray<T>>
129 T GetStandardDeviation(const TARRAY& Values)
130 {
131 const T Variance = GetVariance(Values);
132 return FMath::Sqrt(Variance);
133 }
134
136 template<class T>
137 T GetStandardDeviation(const T Variance)
138 {
139 return FMath::Sqrt(Variance);
140 }
141
142 inline static FMatrix33 CrossProductMatrix(const FVec3& V)
143 {
144 return FMatrix33(
145 0, -V.Z, V.Y,
146 V.Z, 0, -V.X,
147 -V.Y, V.X, 0);
148 }
149
154 inline FMatrix33 Multiply(const FMatrix33& L, const FMatrix33& R)
155 {
156 // @todo(ccaulfield): optimize: simd
157
158 // We want L.R (FMatrix operator* actually calculates R.(L)T; i.e., Right is on the left, and the Left is transposed on the right.)
159 // NOTE: PMatrix constructor takes values in column order
160 return FMatrix33(
161 L.M[0][0] * R.M[0][0] + L.M[1][0] * R.M[0][1] + L.M[2][0] * R.M[0][2], // x00
162 L.M[0][0] * R.M[1][0] + L.M[1][0] * R.M[1][1] + L.M[2][0] * R.M[1][2], // x01
163 L.M[0][0] * R.M[2][0] + L.M[1][0] * R.M[2][1] + L.M[2][0] * R.M[2][2], // x02
164
165 L.M[0][1] * R.M[0][0] + L.M[1][1] * R.M[0][1] + L.M[2][1] * R.M[0][2], // x10
166 L.M[0][1] * R.M[1][0] + L.M[1][1] * R.M[1][1] + L.M[2][1] * R.M[1][2], // x11
167 L.M[0][1] * R.M[2][0] + L.M[1][1] * R.M[2][1] + L.M[2][1] * R.M[2][2], // x12
168
169 L.M[0][2] * R.M[0][0] + L.M[1][2] * R.M[0][1] + L.M[2][2] * R.M[0][2], // x20
170 L.M[0][2] * R.M[1][0] + L.M[1][2] * R.M[1][1] + L.M[2][2] * R.M[1][2], // x21
171 L.M[0][2] * R.M[2][0] + L.M[1][2] * R.M[2][1] + L.M[2][2] * R.M[2][2] // x22
172 );
173 }
174
175 inline FMatrix44 Multiply(const FMatrix44& L, const FMatrix44& R)
176 {
177 // @todo(ccaulfield): optimize: simd
178
179 // We want L.R (FMatrix operator* actually calculates R.(L)T; i.e., Right is on the left, and the Left is transposed on the right.)
180 // NOTE: PMatrix constructor takes values in column order
181 return FMatrix44(
182 L.M[0][0] * R.M[0][0] + L.M[1][0] * R.M[0][1] + L.M[2][0] * R.M[0][2] + L.M[3][0] * R.M[0][3], // x00
183 L.M[0][0] * R.M[1][0] + L.M[1][0] * R.M[1][1] + L.M[2][0] * R.M[1][2] + L.M[3][0] * R.M[1][3], // x01
184 L.M[0][0] * R.M[2][0] + L.M[1][0] * R.M[2][1] + L.M[2][0] * R.M[2][2] + L.M[3][0] * R.M[2][3], // x02
185 L.M[0][0] * R.M[3][0] + L.M[1][0] * R.M[3][1] + L.M[2][0] * R.M[3][2] + L.M[3][0] * R.M[3][3], // x03
186
187 L.M[0][1] * R.M[0][0] + L.M[1][1] * R.M[0][1] + L.M[2][1] * R.M[0][2] + L.M[3][1] * R.M[0][3], // x10
188 L.M[0][1] * R.M[1][0] + L.M[1][1] * R.M[1][1] + L.M[2][1] * R.M[1][2] + L.M[3][1] * R.M[1][3], // x11
189 L.M[0][1] * R.M[2][0] + L.M[1][1] * R.M[2][1] + L.M[2][1] * R.M[2][2] + L.M[3][1] * R.M[2][3], // x12
190 L.M[0][1] * R.M[3][0] + L.M[1][1] * R.M[3][1] + L.M[2][1] * R.M[3][2] + L.M[3][1] * R.M[3][3], // x13
191
192 L.M[0][2] * R.M[0][0] + L.M[1][2] * R.M[0][1] + L.M[2][2] * R.M[0][2] + L.M[3][2] * R.M[0][3], // x20
193 L.M[0][2] * R.M[1][0] + L.M[1][2] * R.M[1][1] + L.M[2][2] * R.M[1][2] + L.M[3][2] * R.M[1][3], // x21
194 L.M[0][2] * R.M[2][0] + L.M[1][2] * R.M[2][1] + L.M[2][2] * R.M[2][2] + L.M[3][2] * R.M[2][3], // x22
195 L.M[0][2] * R.M[3][0] + L.M[1][2] * R.M[3][1] + L.M[2][2] * R.M[3][2] + L.M[3][2] * R.M[3][3], // x23
196
197 L.M[0][3] * R.M[0][0] + L.M[1][3] * R.M[0][1] + L.M[2][3] * R.M[0][2] + L.M[3][3] * R.M[0][3], // x30
198 L.M[0][3] * R.M[1][0] + L.M[1][3] * R.M[1][1] + L.M[2][3] * R.M[1][2] + L.M[3][3] * R.M[1][3], // x31
199 L.M[0][3] * R.M[2][0] + L.M[1][3] * R.M[2][1] + L.M[2][3] * R.M[2][2] + L.M[3][3] * R.M[2][3], // x32
200 L.M[0][3] * R.M[3][0] + L.M[1][3] * R.M[3][1] + L.M[2][3] * R.M[3][2] + L.M[3][3] * R.M[3][3] // x33
201 );
202 }
203
204 inline FMatrix33 MultiplyAB(const FMatrix33& LIn, const FMatrix33& RIn)
205 {
206 return Multiply(LIn, RIn);
207 }
208
209 inline FMatrix33 MultiplyABt(const FMatrix33& L, const FMatrix33& R)
210 {
211 return FMatrix33(
212 L.M[0][0] * R.M[0][0] + L.M[1][0] * R.M[1][0] + L.M[2][0] * R.M[2][0], // x00
213 L.M[0][0] * R.M[0][1] + L.M[1][0] * R.M[1][1] + L.M[2][0] * R.M[2][1], // x01
214 L.M[0][0] * R.M[0][2] + L.M[1][0] * R.M[1][2] + L.M[2][0] * R.M[2][2], // x02
215
216 L.M[0][1] * R.M[0][0] + L.M[1][1] * R.M[1][0] + L.M[2][1] * R.M[2][0], // x10
217 L.M[0][1] * R.M[0][1] + L.M[1][1] * R.M[1][1] + L.M[2][1] * R.M[2][1], // x11
218 L.M[0][1] * R.M[0][2] + L.M[1][1] * R.M[1][2] + L.M[2][1] * R.M[2][2], // x12
219
220 L.M[0][2] * R.M[0][0] + L.M[1][2] * R.M[1][0] + L.M[2][2] * R.M[2][0], // x20
221 L.M[0][2] * R.M[0][1] + L.M[1][2] * R.M[1][1] + L.M[2][2] * R.M[2][1], // x21
222 L.M[0][2] * R.M[0][2] + L.M[1][2] * R.M[1][2] + L.M[2][2] * R.M[2][2] // x22
223 );
224 }
225
226 inline FMatrix33 MultiplyAtB(const FMatrix33& L, const FMatrix33& R)
227 {
228 return FMatrix33(
229 L.M[0][0] * R.M[0][0] + L.M[0][1] * R.M[0][1] + L.M[0][2] * R.M[0][2], // x00
230 L.M[0][0] * R.M[1][0] + L.M[0][1] * R.M[1][1] + L.M[0][2] * R.M[1][2], // x01
231 L.M[0][0] * R.M[2][0] + L.M[0][1] * R.M[2][1] + L.M[0][2] * R.M[2][2], // x02
232
233 L.M[1][0] * R.M[0][0] + L.M[1][1] * R.M[0][1] + L.M[1][2] * R.M[0][2], // x10
234 L.M[1][0] * R.M[1][0] + L.M[1][1] * R.M[1][1] + L.M[1][2] * R.M[1][2], // x11
235 L.M[1][0] * R.M[2][0] + L.M[1][1] * R.M[2][1] + L.M[1][2] * R.M[2][2], // x12
236
237 L.M[2][0] * R.M[0][0] + L.M[2][1] * R.M[0][1] + L.M[2][2] * R.M[0][2], // x20
238 L.M[2][0] * R.M[1][0] + L.M[2][1] * R.M[1][1] + L.M[2][2] * R.M[1][2], // x21
239 L.M[2][0] * R.M[2][0] + L.M[2][1] * R.M[2][1] + L.M[2][2] * R.M[2][2] // x22
240 );
241
242 }
243
248 inline FVec3 Multiply(const FMatrix33& L, const FVec3& R)
249 {
250 // @todo(chaos): optimize: use simd
251 return FVec3(
252 L.M[0][0] * R.X + L.M[1][0] * R.Y + L.M[2][0] * R.Z,
253 L.M[0][1] * R.X + L.M[1][1] * R.Y + L.M[2][1] * R.Z,
254 L.M[0][2] * R.X + L.M[1][2] * R.Y + L.M[2][2] * R.Z);
255 }
256
257 inline TVec3<FRealSingle> Multiply(const TMatrix33<FRealSingle>& L, const TVec3<FRealSingle>& R)
258 {
259 // @todo(chaos): optimize: use simd
260 return TVec3<FRealSingle>(
261 L.M[0][0] * R.X + L.M[1][0] * R.Y + L.M[2][0] * R.Z,
262 L.M[0][1] * R.X + L.M[1][1] * R.Y + L.M[2][1] * R.Z,
263 L.M[0][2] * R.X + L.M[1][2] * R.Y + L.M[2][2] * R.Z);
264 }
265
266 inline FVec4 Multiply(const FMatrix44& L, const FVec4& R)
267 {
268 // @todo(chaos): optimize: use simd
269 return FVec4(
270 L.M[0][0] * R.X + L.M[1][0] * R.Y + L.M[2][0] * R.Z + L.M[3][0] * R.W,
271 L.M[0][1] * R.X + L.M[1][1] * R.Y + L.M[2][1] * R.Z + L.M[3][1] * R.W,
272 L.M[0][2] * R.X + L.M[1][2] * R.Y + L.M[2][2] * R.Z + L.M[3][2] * R.W,
273 L.M[0][3] * R.X + L.M[1][3] * R.Y + L.M[2][3] * R.Z + L.M[3][3] * R.W
274 );
275 }
276
277 // Solves X * A = B (or A.Multiply(X) = B) using Gaussian elimination, writing the result
278 // into X. Returns true if there is a solution, false otherwise.
279 inline bool Solve(FVec3& X, const FMatrix33& A, const FVec3& B)
280 {
281 const FReal Determinant = A.Determinant();
282 if (FMath::IsNearlyZero(Determinant))
283 {
284 return false;
285 }
286
287 // Matrix33 stores in columns...
288 const FVec3 C0 = A.GetColumn(0);
289 const FVec3 C1 = A.GetColumn(1);
290 const FVec3 C2 = A.GetColumn(2);
291
292 // All reference material represents the problem in row form.
293 // Form the 3x4 augmented matrix
294 FVec4 R0(C0.X, C1.X, C2.X, B.X);
295 FVec4 R1(C0.Y, C1.Y, C2.Y, B.Y);
296 FVec4 R2(C0.Z, C1.Z, C2.Z, B.Z);
297
298 // Now apply row operations to reduce it to echelon form: Ones along the diagonal, and
299 // zeros in the bottom left.
300 R0 *= 1 / R0[0]; // Make (0, 0) = 1
301 R1 -= R0 * R1[0]; // Make (1, 0) = 0
302 R2 -= R0 * R2[0]; // Make (2, 0) = 0
303
304 R1 *= 1 / R1[1]; // Make (1, 1) = 1
305 R2 -= R1 * R2[1]; // Make (2, 1) = 0
306
307 R2 *= 1 / R2[2]; // Make (2, 2) = 1
308
309 // R is now in echelon form, so we can work up from the bottom to calculate result
310 X[2] = R2[3];
311 X[1] = R1[3] - R1[2] * X[2];
312 X[0] = R0[3] - (R0[1] * X[1] + R0[2] * X[2]);
313 return true;
314 }
315
319 inline FRigidTransform3 Multiply(const FRigidTransform3 L, const FRigidTransform3& R)
320 {
321 return FRigidTransform3(L.GetTranslation() + L.GetRotation().RotateVector(R.GetTranslation()), L.GetRotation() * R.GetRotation());
322 }
323
327 inline FMatrix33 ComputeWorldSpaceInertia(const FRotation3& CoMRotation, const FMatrix33& I)
328 {
329 FMatrix33 QM = CoMRotation.ToMatrix();
330 return MultiplyAB(QM, MultiplyABt(I, QM));
331 }
332
333 inline FMatrix33 ComputeWorldSpaceInertia(const FRotation3& CoMRotation, const FVec3& I)
334 {
335 // @todo(ccaulfield): optimize ComputeWorldSpaceInertia
336 return ComputeWorldSpaceInertia(CoMRotation, FMatrix33(I.X, I.Y, I.Z));
337 }
338
339 // Given a body with world orientation Q, world angular velocity W, local moments of inertia
340 // I, this calculates the new angular velocity it should have after integrating forwards by
341 // Dt, considering the gyroscopic torques.
342 FVec3 GetAngularVelocityAdjustedForGyroscopicTorques(
343 const FRotation3& Q, const FVec3& I, const FVec3& W, const FReal Dt);
344
348 template<class T>
349 PMatrix<T, 3, 3> ComputeJointFactorMatrix(const TVec3<T>& V, const PMatrix<T, 3, 3>& M, const T& Im)
350 {
351 // Rigid objects rotational contribution to the impulse.
352 // Vx*M*VxT+Im
353 check(Im > FLT_MIN);
354 return PMatrix<T, 3, 3>(
355 -V[2] * (-V[2] * M.M[1][1] + V[1] * M.M[2][1]) + V[1] * (-V[2] * M.M[2][1] + V[1] * M.M[2][2]) + Im,
356 V[2] * (-V[2] * M.M[1][0] + V[1] * M.M[2][0]) - V[0] * (-V[2] * M.M[2][1] + V[1] * M.M[2][2]),
357 -V[1] * (-V[2] * M.M[1][0] + V[1] * M.M[2][0]) + V[0] * (-V[2] * M.M[1][1] + V[1] * M.M[2][1]),
358 V[2] * (V[2] * M.M[0][0] - V[0] * M.M[2][0]) - V[0] * (V[2] * M.M[2][0] - V[0] * M.M[2][2]) + Im,
359 -V[1] * (V[2] * M.M[0][0] - V[0] * M.M[2][0]) + V[0] * (V[2] * M.M[1][0] - V[0] * M.M[2][1]),
360 -V[1] * (-V[1] * M.M[0][0] + V[0] * M.M[1][0]) + V[0] * (-V[1] * M.M[1][0] + V[0] * M.M[1][1]) + Im);
361 }
362
366 template<class T>
367 TVec3<T> ComputeDiagonalJointFactorMatrix(const TVec3<T>& V, const PMatrix<T, 3, 3>& M, const T& Im)
368 {
369 // Rigid objects rotational contribution to the impulse.
370 // Vx*M*VxT+Im
371 check(Im > FLT_MIN);
372 return TVec3<T>(
373 -V[2] * (-V[2] * M.M[1][1] + V[1] * M.M[2][1]) + V[1] * (-V[2] * M.M[2][1] + V[1] * M.M[2][2]) + Im,
374 V[2] * (V[2] * M.M[0][0] - V[0] * M.M[2][0]) - V[0] * (V[2] * M.M[2][0] - V[0] * M.M[2][2]) + Im,
375 -V[1] * (-V[1] * M.M[0][0] + V[0] * M.M[1][0]) + V[0] * (-V[1] * M.M[1][0] + V[0] * M.M[1][1]) + Im);
376 }
377
382 template<typename T>
383 bool IntersectLineSegments2D(const TVec2<T>& InStartA, const TVec2<T>& InEndA, const TVec2<T>& InStartB, const TVec2<T>& InEndB, T& OutTA, T& OutTB)
384 {
385 // Each line can be described as p0 + t(p1 - p0) = P. Set equal to each other and solve for t0 and t1
386 OutTA = OutTB = 0;
387
388 T Divisor = (InEndB[0] - InStartB[0]) * (InStartA[1] - InEndA[1]) - (InStartA[0] - InEndA[0]) * (InEndB[1] - InStartB[1]);
389
390 if (FMath::IsNearlyZero(Divisor))
391 {
392 // The line segments are parallel and will never meet for any values of Ta/Tb
393 return false;
394 }
395
396 OutTA = ((InStartB[1] - InEndB[1]) * (InStartA[0] - InStartB[0]) + (InEndB[0] - InStartB[0]) * (InStartA[1] - InStartB[1])) / Divisor;
397 OutTB = ((InStartA[1] - InEndA[1]) * (InStartA[0] - InStartB[0]) + (InEndA[0] - InStartA[0]) * (InStartA[1] - InStartB[1])) / Divisor;
398
399 return OutTA >= 0 && OutTA <= 1 && OutTB > 0 && OutTB < 1;
400 }
401
406 inline bool ClipLineSegmentToPlane(FVec3& V0, FVec3& V1, const FVec3& PlaneNormal, const FVec3& PlanePos)
407 {
408 FReal Dist0 = FVec3::DotProduct(V0 - PlanePos, PlaneNormal);
409 FReal Dist1 = FVec3::DotProduct(V1 - PlanePos, PlaneNormal);
410 if ((Dist0 > 0.0f) && (Dist1 > 0.0f))
411 {
412 // Whole line segment is outside of face - reject it
413 return false;
414 }
415
416 if ((Dist0 > 0.0f) && (Dist1 < 0.0f))
417 {
418 // We must move vert 0 to the plane
419 FReal ClippedT = -Dist1 / (Dist0 - Dist1);
420 V0 = FMath::Lerp(V1, V0, ClippedT);
421 }
422 else if ((Dist1 > 0.0f) && (Dist0 < 0.0f))
423 {
424 // We must move vert 1 to the plane
425 FReal ClippedT = -Dist0 / (Dist1 - Dist0);
426 V1 = FMath::Lerp(V0, V1, ClippedT);
427 }
428
429 return true;
430 }
431
435 inline bool ClipLineSegmentToAxisAlignedPlane(FVec3& V0, FVec3& V1, const int32 AxisIndex, const FReal PlaneDir, const FReal PlanePos)
436 {
437 FReal Dist0 = (V0[AxisIndex] - PlanePos) * PlaneDir;
438 FReal Dist1 = (V1[AxisIndex] - PlanePos) * PlaneDir;
439 if ((Dist0 > 0.0f) && (Dist1 > 0.0f))
440 {
441 // Whole line segment is outside of face - reject it
442 return false;
443 }
444
445 if ((Dist0 > 0.0f) && (Dist1 < 0.0f))
446 {
447 // We must move vert 0 to the plane
448 FReal ClippedT = -Dist1 / (Dist0 - Dist1);
449 V0 = FMath::Lerp(V1, V0, ClippedT);
450 }
451 else if ((Dist1 > 0.0f) && (Dist0 < 0.0f))
452 {
453 // We must move vert 1 to the plane
454 FReal ClippedT = -Dist0 / (Dist1 - Dist0);
455 V1 = FMath::Lerp(V0, V1, ClippedT);
456 }
457
458 return true;
459 }
460
465 inline void ProjectPointOntoAxisAlignedPlane(FVec3& V, const FVec3& Dir, int32 AxisIndex, FReal PlaneDir, FReal PlanePos)
466 {
467 // V -> V + ((PlanePos - V) | PlaneNormal) / (Dir | PlaneNormal)
468 FReal Denominator = Dir[AxisIndex] * PlaneDir;
469 FReal Numerator = (PlanePos - V[AxisIndex]) * PlaneDir;
470 FReal F = Numerator / Denominator;
471 V = V + F * Dir;
472 }
473
478 inline bool ProjectPointOntoAxisAlignedPlaneSafe(FVec3& V, const FVec3& Dir, int32 AxisIndex, FReal PlaneDir, FReal PlanePos, FReal Epsilon)
479 {
480 // V -> V + ((PlanePos - V) | PlaneNormal) / (Dir | PlaneNormal)
481 FReal Denominator = Dir[AxisIndex] * PlaneDir;
482 if (Denominator > Epsilon)
483 {
484 FReal Numerator = (PlanePos - V[AxisIndex]) * PlaneDir;
485 FReal F = Numerator / Denominator;
486 V = V + F * Dir;
487 return true;
488 }
489 return false;
490 }
491
492 inline bool NormalizeSafe(FVec3& V, FReal EpsilonSq = UE_SMALL_NUMBER)
493 {
494 FReal VLenSq = V.SizeSquared();
495 if (VLenSq > EpsilonSq)
496 {
497 V = V * FMath::InvSqrt(VLenSq);
498 return true;
499 }
500 return false;
501 }
502
503 template <class T>
504 inline T DotProduct(const TArray<T>& X, const TArray<T>& Y)
505 {
506 T Result = T(0);
507 for (int32 i = 0; i < X.Num(); i++)
508 {
509 Result += X[i] * Y[i];
510 }
511 return Result;
512 }
513
518 template<typename T>
519 inline TVec3<T> ScaleInertia(const TVec3<T>& Inertia, const TVec3<T>& Scale, const bool bScaleMass)
520 {
521 // support for negative scale
522 const TVec3<T> AbsScale = Scale.GetAbs();
523
524 TVec3<T> XYZSq = (TVec3<T>(0.5f * (Inertia.X + Inertia.Y + Inertia.Z)) - Inertia) * AbsScale * AbsScale;
525 T XX = XYZSq.Y + XYZSq.Z;
526 T YY = XYZSq.X + XYZSq.Z;
527 T ZZ = XYZSq.X + XYZSq.Y;
528 TVec3<T> ScaledInertia = TVec3<T>(XX, YY, ZZ);
529 T MassScale = (bScaleMass) ? AbsScale.X * AbsScale.Y * AbsScale.Z : 1.0f;
530 return MassScale * ScaledInertia;
531 }
532
537 inline FMatrix33 ScaleInertia(const FMatrix33& Inertia, const FVec3& Scale, const bool bScaleMass)
538 {
539 // support for negative scale
540 const FVec3 AbsScale = Scale.GetAbs();
541
542 // @todo(chaos): do we need to support a rotation of the scale axes?
543 FVec3 D = Inertia.GetDiagonal();
544 FVec3 XYZSq = (FVec3(0.5f * (D.X + D.Y + D.Z)) - D) * AbsScale * AbsScale;
545 FReal XX = XYZSq.Y + XYZSq.Z;
546 FReal YY = XYZSq.X + XYZSq.Z;
547 FReal ZZ = XYZSq.X + XYZSq.Y;
548 FReal XY = Inertia.M[0][1] * AbsScale.X * AbsScale.Y;
549 FReal XZ = Inertia.M[0][2] * AbsScale.X * AbsScale.Z;
550 FReal YZ = Inertia.M[1][2] * AbsScale.Y * AbsScale.Z;
551 FReal MassScale = (bScaleMass) ? AbsScale.X * AbsScale.Y * AbsScale.Z : 1.0f;
552 FMatrix33 ScaledInertia = FMatrix33(
553 MassScale * XX, MassScale * XY, MassScale * XZ,
554 MassScale * XY, MassScale * YY, MassScale * YZ,
555 MassScale * XZ, MassScale * YZ, MassScale * ZZ);
556 return ScaledInertia;
557 }
558
559 // Compute the box size that would generate the given (diagonal) inertia
560 inline bool BoxFromInertia(const FVec3& InInertia, const FReal Mass, FVec3& OutCenter, FVec3& OutSize)
561 {
562 OutSize = FVec3(0);
563 OutCenter = FVec3(0);
564
565 // System of 3 equations in X^2, Y^2, Z^2
566 // Inertia.X = 1/12 M (Size.Y^2 + Size.Z^2)
567 // Inertia.Y = 1/12 M (Size.Z^2 + Size.X^2)
568 // Inertia.Z = 1/12 M (Size.X^2 + Size.Y^2)
569 // Unless the center of mass has been modified, in which case we have
570 // Inertia.X = 1/12 M (Size.Y^2 + Size.Z^2) + M D.X^2
571 // Inertia.Y = 1/12 M (Size.Z^2 + Size.X^2) + M D.Y^2
572 // Inertia.Z = 1/12 M (Size.X^2 + Size.Y^2) + M D.Z^2
573 // Which will not have a unique solution (3 equations in 6 unknowns).
574 // There's no way to know here that the center of mass was modified so we assume it wasn't unless we cannot
575 // solve the equations and then we must make some guesses to recover an equivalent box.
576 if (Mass > 0)
577 {
578 // RInv is the inverse of the coefficient matrix (0,1,1)(1,0,1)(0,1,1)
579 const FMatrix33 RInv = FMatrix33(-0.5f, 0.5f, 0.5f, 0.5f, -0.5f, 0.5f, 0.5f, 0.5f, -0.5f);
580 FVec3 Inertia = InInertia / Mass;
581 FVec3 XYZSq = RInv * (Inertia * 12.0f);
582
583 // If we have a shape with a modified center of mass that is outside the equivalent box, we will end up with negative
584 // coefficients here and cannot calculate a box equivalent. To do this properly we need to know what center of mass offset was applied.
585 // But lets try to do something anyway so that debug draw shows something...we'll pretend that the shifted inertia component would be equal to the
586 // smallest component in the absense of the shift. This works "correctly" for a uniform shape (e.g., a box), but will be wrong for everything else!
587 // Also, there's a sign problem since shifting the center of mass in the opposite direction would have altered the inertia the same way.
588 // Net result: I'm not sure how useful this is - see if we can do something better one day (maybe store the ComNudge)
589 if (XYZSq.X < 0)
590 {
591 FReal DXSq = (Inertia.X - FMath::Min(Inertia.Y, Inertia.Z));
592 if (DXSq > 0)
593 {
594 OutCenter.X = FMath::Sqrt(DXSq);
595 Inertia.X -= DXSq;
596 XYZSq = RInv * (Inertia * 12.0f);
597 }
598 }
599 if (XYZSq.Y < 0)
600 {
601 FReal DYSq = (Inertia.Y - FMath::Min(Inertia.X, Inertia.Z));
602 if (DYSq > 0)
603 {
604 OutCenter.Y = FMath::Sqrt(DYSq);
605 Inertia.Y -= DYSq;
606 XYZSq = RInv * (Inertia * 12.0f);
607 }
608 }
609 if (XYZSq.Z < 0)
610 {
611 FReal DZSq = (Inertia.Z - FMath::Min(Inertia.X, Inertia.Y));
612 if (DZSq > 0)
613 {
614 OutCenter.Z = FMath::Sqrt(DZSq);
615 Inertia.Z -= DZSq;
616 XYZSq = RInv * (Inertia * 12.0f);
617 }
618 }
619
620 OutSize = FVec3(
621 FMath::Sqrt(FMath::Max(XYZSq.X, FReal(0))),
622 FMath::Sqrt(FMath::Max(XYZSq.Y, FReal(0))),
623 FMath::Sqrt(FMath::Max(XYZSq.Z, FReal(0))));
624
625 return true;
626 }
627 return false;
628 }
629
630 // Replacement for FMath::Wrap that works for integers and returns a value in [Begin, End).
631 // Note: this implementation uses a loop to bring the value into range - it should not be used if the value is much larger than the range.
632 inline int32 WrapIndex(int32 V, int32 Begin, int32 End)
633 {
634 int32 Range = End - Begin;
635 while (V < Begin)
636 {
637 V += Range;
638 }
639 while (V >= End)
640 {
641 V -= Range;
642 }
643 return V;
644 }
645
646 // Get the closest point on a line segment between P1 and Q1 and a
647 // second line segment between P2 and Q2
648 // For implementation notes, see "Realtime Collision Detection", Christer Ericson, 2005
649 inline void NearestPointsOnLineSegments(
650 const FVec3& P1, const FVec3& Q1,
651 const FVec3& P2, const FVec3& Q2,
652 FReal& S, FReal& T,
653 FVec3& C1, FVec3& C2,
654 const FReal Epsilon = 1.e-4f)
655 {
656 const FReal EpsilonSq = Epsilon * Epsilon;
657 const FVec3 D1 = Q1 - P1;
658 const FVec3 D2 = Q2 - P2;
659 const FVec3 R = P1 - P2;
660 const FReal A = FVec3::DotProduct(D1, D1);
661 const FReal B = FVec3::DotProduct(D1, D2);
662 const FReal C = FVec3::DotProduct(D1, R);
663 const FReal E = FVec3::DotProduct(D2, D2);
664 const FReal F = FVec3::DotProduct(D2, R);
665 constexpr FReal Min = 0, Max = 1;
666
667 S = 0.0f;
668 T = 0.0f;
669
670 if ((A <= EpsilonSq) && (B <= EpsilonSq))
671 {
672 // Both segments are points
673 }
674 else if (A <= EpsilonSq)
675 {
676 // First segment (only) is a point
677 T = FMath::Clamp<FReal>(F / E, Min, Max);
678 }
679 else if (E <= EpsilonSq)
680 {
681 // Second segment (only) is a point
682 S = FMath::Clamp<FReal>(-C / A, Min, Max);
683 }
684 else
685 {
686 // Non-degenrate case - we have two lines
687 const FReal Denom = A * E - B * B;
688 if (Denom != 0.0f)
689 {
690 S = FMath::Clamp<FReal>((B * F - C * E) / Denom, Min, Max);
691 }
692 T = (B * S + F) / E;
693
694 if (T < 0.0f)
695 {
696 S = FMath::Clamp<FReal>(-C / A, Min, Max);
697 T = 0.0f;
698 }
699 else if (T > 1.0f)
700 {
701 S = FMath::Clamp<FReal>((B - C) / A, Min, Max);
702 T = 1.0f;
703 }
704 }
705
706 C1 = P1 + S * D1;
707 C2 = P2 + T * D2;
708 }
709
710 // Get the closest point on a line segment between SegmentBegin and SegmentEnd to an infinite
711 // line through LinePos along LineVector in both direction.
712 // For implementation notes, see "Realtime Collision Detection", Christer Ericson, 2005
713 inline void NearestPointsOnLineSegmentToLine(
714 const FVec3& SegmentBegin, const FVec3& SegmentEnd,
715 const FVec3& LinePos, const FVec3& LineVector,
716 FReal& S, FReal& T,
717 FVec3& C1, FVec3& C2,
718 const FReal Epsilon = 1.e-4f)
719 {
720 const FReal EpsilonSq = Epsilon * Epsilon;
721 const FVec3& P1 = SegmentBegin;
722 const FVec3& Q1 = SegmentEnd;
723 const FVec3& P2 = LinePos;
724 const FVec3 D1 = Q1 - P1;
725 const FVec3& D2 = LineVector;
726 const FVec3 R = P1 - P2;
727 const FReal A = FVec3::DotProduct(D1, D1);
728 const FReal B = FVec3::DotProduct(D1, D2);
729 const FReal C = FVec3::DotProduct(D1, R);
730 const FReal E = FVec3::DotProduct(D2, D2);
731 const FReal F = FVec3::DotProduct(D2, R);
732 constexpr FReal Min = 0, Max = 1;
733
734 S = 0.0f;
735 T = 0.0f;
736
737 if ((A <= EpsilonSq) && (B <= EpsilonSq))
738 {
739 // Both segments are points
740 }
741 else if (A <= EpsilonSq)
742 {
743 // First segment (only) is a point
744 T = F / E;
745 }
746 else if (E <= EpsilonSq)
747 {
748 // Second line (only) is a point (i.e., LineVector is zero)
749 S = FMath::Clamp<FReal>(-C / A, Min, Max);
750 }
751 else
752 {
753 // Non-degenrate case - we have two lines
754 const FReal Denom = A * E - B * B;
755 if (Denom != 0.0f)
756 {
757 S = FMath::Clamp<FReal>((B * F - C * E) / Denom, Min, Max);
758 }
759 T = (B * S + F) / E;
760 }
761
762 C1 = P1 + S * D1;
763 C2 = P2 + T * D2;
764 }
765
766 namespace
767 {
768 // Use dot product with a barrier to avoid fma to generate inaccuracy results
769 template<typename T>
770 T DotWithoutFMA(const TVec3<T>& A, const TVec3<T>& B)
771 {
772 T X = A.X * B.X;
773 T Y = A.Y * B.Y;
774 T Z = A.Z * B.Z;
775#if defined(_MSC_VER) && !defined(__clang__)
776 _ReadWriteBarrier();
777#endif
778 return X + Y + Z;
779 }
780 }
781
782 template<typename T>
783 inline T ClosestTimeOnLineSegment(const TVec3<T>& Point, const TVec3<T>& StartPoint, const TVec3<T>& EndPoint)
784 {
785 const TVec3<T> Segment = EndPoint - StartPoint;
786 const TVec3<T> VectToPoint = Point - StartPoint;
787
788 // See if closet point is before StartPoint
789 const T Dot1 = DotWithoutFMA<T>(VectToPoint, Segment);
790 if (Dot1 <= T(0))
791 {
792 return T(0);
793 }
794
795 // See if closest point is beyond EndPoint
796 const T Dot2 = DotWithoutFMA<T>(Segment, Segment);
797 if (Dot2 <= Dot1)
798 {
799 return T(1);
800 }
801
802 // Closest Point is within segment
803 return Dot1 / Dot2;
804 }
805
809 template<typename T>
810 inline T RaySphereIntersectionDistance(const TVec3<T>& RayStart, const TVec3<T>& RayDir, const TVec3<T>& SpherePos, const T SphereRadius)
811 {
812 const TVec3<T> EO = SpherePos - RayStart;
813 const T V = TVec3<T>::DotProduct(RayDir, EO);
814 const T Disc = SphereRadius * SphereRadius - (TVec3<T>::DotProduct(EO, EO) - V * V);
815 if (Disc >= 0)
816 {
817 return (V - FMath::Sqrt(Disc));
818 }
819 else
820 {
821 return TNumericLimits<T>::Max();
822 }
823 }
824
829 template<typename T>
830 inline T AsinEst1(const T X)
831 {
832 return X /*+...*/;
833 }
834
835
840 template<typename T>
841 inline T AsinEst3(const T X)
842 {
843 constexpr T C0 = T(1.0);
844 constexpr T C1 = T(1.0 / 6.0);
845 const T X2 = X * X;
846 return X * (C0 + X2 * (C1 /*+...*/));
847 }
848
853 template<typename T>
854 inline T AsinEst5(const T X)
855 {
856 constexpr T C0 = T(1.0);
857 constexpr T C1 = T(1.0 / 6.0);
858 constexpr T C2 = T(3.0 / 40.0);
859 const T X2 = X * X;
860 return X * (C0 + X2 * (C1 + X2 * (C2 /*+...*/)));
861
862 }
863
868 template<typename T>
869 inline T AsinEst7(const T X)
870 {
871 constexpr T C0 = T(1.0);
872 constexpr T C1 = T(1.0 / 6.0);
873 constexpr T C2 = T(3.0 / 40.0);
874 constexpr T C3 = T(15.0 / 336.0);
875 const T X2 = X * X;
876 return X * (C0 + X2 * (C1 + X2 * (C2 + X2 * (C3 /*+...*/))));
877 }
878
882 template<typename T, int Order = 5>
883 inline T AsinEst(const T X)
884 {
885 static_assert((Order == 1) || (Order == 3) || (Order == 5) || (Order == 7), "AsinEst: Only 1, 3, 5, or 7 is supported for the Order");
886 if (Order == 1)
887 {
888 return AsinEst1(X);
889 }
890 else if (Order == 3)
891 {
892 return AsinEst3(X);
893 }
894 else if (Order == 5)
895 {
896 return AsinEst5(X);
897 }
898 else
899 {
900 return AsinEst7(X);
901 }
902 }
903
907 template<typename T, int Order = 5>
908 inline T AsinEstCrossover(const T X, const T Crossover = T(0.7))
909 {
910 if (FMath::Abs(X) < Crossover)
911 {
912 return AsinEst<T, Order>(X);
913 }
914 else
915 {
916 return FMath::Asin(X);
917 }
918 }
919
923 template <class T, class TV, class TV_INT4>
924 void TetMeshFromGrid(const TUniformGrid<T, 3>& Grid, TArray<TV_INT4>& Mesh, TArray<TV>& X)
925 {
926 Mesh.SetNum(20 * Grid.GetNumCells() / 4);
927 int32* MeshPtr = &Mesh[0][0];
928
929 const int32 NumNodes = Grid.GetNumNodes();
930 X.SetNum(NumNodes);
931 for(int32 ii=0; ii < NumNodes; ii++)
932 {
933 X[ii] = Grid.Node(ii);
934 }
935
936 int32 Count = 0;
937 for (int32 i = 0; i < Grid.Counts()[0]; i++)
938 {
939 for (int32 j = 0; j < Grid.Counts()[1]; j++)
940 {
941 for (int32 k = 0; k < Grid.Counts()[2]; k++)
942 {
943 int32 ijk000 = Grid.FlatIndex(TVector<int32, 3>(i, j, k), true);
944 int32 ijk010 = Grid.FlatIndex(TVector<int32, 3>(i, j + 1, k), true);
945 int32 ijk001 = Grid.FlatIndex(TVector<int32, 3>(i, j, k + 1), true);
946 int32 ijk011 = Grid.FlatIndex(TVector<int32, 3>(i, j + 1, k + 1), true);
947 int32 ijk100 = Grid.FlatIndex(TVector<int32, 3>(i + 1, j, k), true);
948 int32 ijk110 = Grid.FlatIndex(TVector<int32, 3>(i + 1, j + 1, k), true);
949 int32 ijk101 = Grid.FlatIndex(TVector<int32, 3>(i + 1, j, k + 1), true);
950 int32 ijk111 = Grid.FlatIndex(TVector<int32, 3>(i + 1, j + 1, k + 1), true);
951 int32 ijk_index = i + j + k;
952 if (ijk_index % 2 == 0)
953 {
954 MeshPtr[20 * Count] = ijk010;
955 MeshPtr[20 * Count + 1] = ijk000;
956 MeshPtr[20 * Count + 2] = ijk110;
957 MeshPtr[20 * Count + 3] = ijk011;
958
959 MeshPtr[20 * Count + 4] = ijk111;
960 MeshPtr[20 * Count + 5] = ijk110;
961 MeshPtr[20 * Count + 6] = ijk101;
962 MeshPtr[20 * Count + 7] = ijk011;
963
964 MeshPtr[20 * Count + 8] = ijk100;
965 MeshPtr[20 * Count + 9] = ijk101;
966 MeshPtr[20 * Count + 10] = ijk110;
967 MeshPtr[20 * Count + 11] = ijk000;
968
969 MeshPtr[20 * Count + 12] = ijk001;
970 MeshPtr[20 * Count + 13] = ijk000;
971 MeshPtr[20 * Count + 14] = ijk011;
972 MeshPtr[20 * Count + 15] = ijk101;
973
974 MeshPtr[20 * Count + 16] = ijk110;
975 MeshPtr[20 * Count + 17] = ijk011;
976 MeshPtr[20 * Count + 18] = ijk000;
977 MeshPtr[20 * Count + 19] = ijk101;
978 }
979 else
980 {
981 MeshPtr[20 * Count] = ijk000;
982 MeshPtr[20 * Count + 1] = ijk100;
983 MeshPtr[20 * Count + 2] = ijk010;
984 MeshPtr[20 * Count + 3] = ijk001;
985
986 MeshPtr[20 * Count + 4] = ijk011;
987 MeshPtr[20 * Count + 5] = ijk010;
988 MeshPtr[20 * Count + 6] = ijk111;
989 MeshPtr[20 * Count + 7] = ijk001;
990
991 MeshPtr[20 * Count + 8] = ijk100;
992 MeshPtr[20 * Count + 9] = ijk111;
993 MeshPtr[20 * Count + 10] = ijk010;
994 MeshPtr[20 * Count + 11] = ijk001;
995
996 MeshPtr[20 * Count + 12] = ijk101;
997 MeshPtr[20 * Count + 13] = ijk111;
998 MeshPtr[20 * Count + 14] = ijk100;
999 MeshPtr[20 * Count + 15] = ijk001;
1000
1001 MeshPtr[20 * Count + 16] = ijk111;
1002 MeshPtr[20 * Count + 17] = ijk010;
1003 MeshPtr[20 * Count + 18] = ijk110;
1004 MeshPtr[20 * Count + 19] = ijk100;
1005 }
1006 Count++;
1007 }
1008 }
1009 }
1010 }
1011
1012 template <int d>
1013 TArray<TArray<int>> ComputeIncidentElements(const TArray<TVector<int32, d>>& Mesh, TArray<TArray<int32>>* LocalIndex=nullptr)
1014 {
1015 int32 MaxIdx = 0;
1016 for(int32 i=0; i < Mesh.Num(); i++)
1017 {
1018 for (int32 j = 0; j < d; j++)
1019 {
1020 const int32 NodeIdx = Mesh[i][j];
1021 MaxIdx = MaxIdx > NodeIdx ? MaxIdx : NodeIdx;
1022 }
1023 }
1024
1025 TArray<TArray<int>> IncidentElements;
1026 IncidentElements.SetNum(MaxIdx + 1);
1027 if (LocalIndex)
1028 LocalIndex->SetNum(MaxIdx + 1);
1029
1030 for (int32 i = 0; i < Mesh.Num(); i++)
1031 {
1032 for (int32 j = 0; j < d; j++)
1033 {
1034 const int32 NodeIdx = Mesh[i][j];
1035 if (NodeIdx >= 0)
1036 {
1037 IncidentElements[NodeIdx].Add(i);
1038 if (LocalIndex)
1039 (*LocalIndex)[NodeIdx].Add(j);
1040 }
1041 }
1042 }
1043
1044 return IncidentElements;
1045 }
1046
1047 inline void DFS_iterative(const TArray<TArray<int32>>& L, int32 v, TArray<bool>& visited, TArray<int32>& component) {
1048 TArray<int32> Stack({ v });
1049 Stack.Heapify();
1050 while (!Stack.IsEmpty()) {
1051 Stack.HeapPop(v);
1052 if (!visited[v]) {
1053 visited[v] = true;
1054 component.Emplace(v);
1055 for (int32 i : L[v]) {
1056 if (!visited[i]) {
1057 Stack.HeapPush(i);
1058 }
1059 }
1060 }
1061 }
1062 }
1063
1064 inline void ConnectedComponentsDFSIterative(const TArray<TArray<int32>>& L, TArray<TArray<int32>>& C) {
1065 //Input: L, the given adjacency list
1066 //Output: C, connected components
1067 TArray<bool> Visited;
1068 Visited.Init(false, L.Num());
1069 for (int32 v = 0; v < L.Num(); ++v) {
1070 if (!Visited[v]) {
1071 TArray<int32> component;
1072 DFS_iterative(L, v, Visited, component);
1073 C.Emplace(component);
1074 }
1075 }
1076 }
1077
1078 inline void FindConnectedRegions(const TArray<FIntVector4>& Elements, TArray<TArray<int32>>& ConnectedComponents)
1079 {
1080 TArray<int32> AllEntries, ElementIndices, Ordering, Ranges;
1081 int32 count = 0;
1082 AllEntries.Init(-1, Elements.Num() * 4);
1083 ElementIndices.Init(-1, Elements.Num()*4);
1084 Ordering.Init(-1, Elements.Num()*4);
1085 Ranges.Init(-1, Elements.Num()*4 + 1);
1086 for (int32 i = 0; i < Elements.Num(); i++)
1087 {
1088 for (int32 j = 0; j < 4; j++)
1089 {
1090 AllEntries[4 * i + j] = Elements[i][j];
1091 ElementIndices[4*i+j] = i;
1092 Ordering[4*i+j] = count;
1093 Ranges[4*i+j] = count++;
1094 }
1095 }
1096 Ranges[Elements.Num() * 4] = count;
1097
1098 if (AllEntries.Num() == 0)
1099 return;
1100
1101 Ordering.Sort([&AllEntries](int32 a, int32 b) { return AllEntries[a] < AllEntries[b]; });
1102 TArray<int32> UniqueRanges({ 0 });
1103
1104 for (int32 i = 0; i < Ranges.Num() - 2; ++i)
1105 {
1106 if (AllEntries[Ordering[i]] != AllEntries[Ordering[i+1]])
1107 {
1108 UniqueRanges.Emplace(i + 1);
1109 }
1110 }
1111
1112 UniqueRanges.Emplace(count);
1113
1114 TArray<TArray<int32>> AdjacencyList;
1115 AdjacencyList.Init(TArray<int32>(), Elements.Num());
1116 for (int32 r = 0; r < UniqueRanges.Num() - 1; r++)
1117 {
1118 if (UniqueRanges[r + 1] - UniqueRanges[r] > 1)
1119 {
1120 for (int32 i = UniqueRanges[r]; i < UniqueRanges[r + 1]; i++)
1121 {
1122 for (int32 j = i + 1; j < UniqueRanges[r + 1]; j++)
1123 {
1124 AdjacencyList[ElementIndices[Ordering[i]]].Emplace(ElementIndices[Ordering[j]]);
1125 AdjacencyList[ElementIndices[Ordering[j]]].Emplace(ElementIndices[Ordering[i]]);
1126 }
1127 }
1128 }
1129 }
1130 ConnectedComponentsDFSIterative(AdjacencyList, ConnectedComponents);
1131 }
1132
1133 inline void FindConnectedRegions(const TArray<FIntVector3>& Elements, TArray<TArray<int32>>& ConnectedComponents)
1134 {
1135 TArray<int32> AllEntries, ElementIndices, Ordering, Ranges;
1136 int32 count = 0;
1137 AllEntries.Init(-1, Elements.Num() * 3);
1138 ElementIndices.Init(-1, Elements.Num() * 3);
1139 Ordering.Init(-1, Elements.Num() * 3);
1140 Ranges.Init(-1, Elements.Num() * 3 + 1);
1141 for (int32 i = 0; i < Elements.Num(); i++)
1142 {
1143 for (int32 j = 0; j < 3; j++)
1144 {
1145 AllEntries[3 * i + j] = Elements[i][j];
1146 ElementIndices[3 * i + j] = i;
1147 Ordering[3 * i + j] = count;
1148 Ranges[3 * i + j] = count++;
1149 }
1150 }
1151 Ranges[Elements.Num() * 3] = count;
1152
1153 if (AllEntries.Num() == 0)
1154 return;
1155
1156 Ordering.Sort([&AllEntries](int32 a, int32 b) { return AllEntries[a] < AllEntries[b]; });
1157 TArray<int32> UniqueRanges({ 0 });
1158
1159 for (int32 i = 0; i < Ranges.Num() - 2; ++i)
1160 {
1161 if (AllEntries[Ordering[i]] != AllEntries[Ordering[i + 1]])
1162 {
1163 UniqueRanges.Emplace(i + 1);
1164 }
1165 }
1166
1167 UniqueRanges.Emplace(count);
1168
1169 TArray<TArray<int32>> AdjacencyList;
1170 AdjacencyList.Init(TArray<int32>(), Elements.Num());
1171 for (int32 r = 0; r < UniqueRanges.Num() - 1; r++)
1172 {
1173 if (UniqueRanges[r + 1] - UniqueRanges[r] > 1)
1174 {
1175 for (int32 i = UniqueRanges[r]; i < UniqueRanges[r + 1]; i++)
1176 {
1177 for (int32 j = i + 1; j < UniqueRanges[r + 1]; j++)
1178 {
1179 AdjacencyList[ElementIndices[Ordering[i]]].Emplace(ElementIndices[Ordering[j]]);
1180 AdjacencyList[ElementIndices[Ordering[j]]].Emplace(ElementIndices[Ordering[i]]);
1181 }
1182 }
1183 }
1184 }
1185 ConnectedComponentsDFSIterative(AdjacencyList, ConnectedComponents);
1186 }
1187
1188 inline TArray<int32> ComputeBoundaryNodes(const TArray<TVec3<int32>>& TriangleMesh)
1189 {
1190 TMap<TPair<int32, int32>, int32> FaceCountPerEdge;
1191
1192 auto AddTriToFaceCountPerEdge = [&FaceCountPerEdge](const TVec3<int32> &Face)
1193 {
1194 auto AddEdge = [&FaceCountPerEdge](int32 A, int32 B)
1195 {
1196 FaceCountPerEdge.FindOrAdd(TPair<int32, int32>(FMath::Min(A, B), FMath::Max(A, B)))++;
1197 };
1198 AddEdge(Face[0], Face[1]);
1199 AddEdge(Face[0], Face[2]);
1200 AddEdge(Face[1], Face[2]);
1201 };
1202
1203 auto CandidateTriWouldMakeNonManifoldEdge = [&FaceCountPerEdge](int32 A, int32 B, int32 C)
1204 {
1205 auto GetCount = [&FaceCountPerEdge](int32 InnerA, int32 InnerB)
1206 {
1207 int32 *Count = FaceCountPerEdge.Find(TPair<int32, int32>(FMath::Min(InnerA, InnerB), FMath::Max(InnerA, InnerB)));
1208 return Count ? *Count : 0;
1209 };
1210 return GetCount(A, B) > 1 || GetCount(B, C) > 1 || GetCount(C, A) > 1;
1211 };
1212
1213 for (const TVec3<int32> &Face : TriangleMesh)
1214 {
1215 AddTriToFaceCountPerEdge(Face);
1216 }
1217
1218 TArray<TPair<int32, int32>> AllEdges;
1219 TSet<int32> BoundaryVerts;
1220 int32 NumKeys = FaceCountPerEdge.GetKeys(AllEdges);
1221
1222 for (const TPair<int32, int32>& Edge: AllEdges)
1223 {
1224 if (FaceCountPerEdge[Edge] == 1)
1225 {
1226 BoundaryVerts.Emplace(Edge.Key);
1227 BoundaryVerts.Emplace(Edge.Value);
1228 }
1229 }
1230
1231 return BoundaryVerts.Array();
1232 }
1233
1234 inline TArray<TArray<int32>> ComputeIncidentElements(const TArray<TArray<int32>>& Constraints, TArray<TArray<int32>>* LocalIndex = nullptr)
1235 {
1236 int32 MaxIdx = 0;
1237 for (int32 i = 0; i < Constraints.Num(); i++)
1238 {
1239 for (int32 j = 0; j < Constraints[i].Num(); j++)
1240 {
1241 const int32 NodeIdx = Constraints[i][j];
1242 MaxIdx = MaxIdx > NodeIdx ? MaxIdx : NodeIdx;
1243 }
1244 }
1245
1246 TArray<TArray<int>> IncidentElements;
1247 IncidentElements.SetNum(MaxIdx + 1);
1248 if (LocalIndex)
1249 {
1250 LocalIndex->SetNum(MaxIdx + 1);
1251 }
1252 for (int32 i = 0; i < Constraints.Num(); i++)
1253 {
1254 for (int32 j = 0; j < Constraints[i].Num(); j++)
1255 {
1256 const int32 NodeIdx = Constraints[i][j];
1257 if (NodeIdx >= 0)
1258 {
1259 IncidentElements[NodeIdx].Add(i);
1260 if (LocalIndex)
1261 (*LocalIndex)[NodeIdx].Add(j);
1262 }
1263 }
1264 }
1265
1266 return IncidentElements;
1267 }
1268
1269 inline void MergeIncidentElements(const TArray<TArray<int32>>& ExtraIncidentElements, const TArray<TArray<int32>>& ExtraIncidentElementsLocal, TArray<TArray<int32>>& IncidentElements, TArray<TArray<int32>>& IncidentElementsLocal)
1270 {
1271 if (ensureMsgf(IncidentElements.Num() == ExtraIncidentElements.Num() && IncidentElementsLocal.Num() == ExtraIncidentElementsLocal.Num(), TEXT("Input incident elements are of different size")))
1272 {
1273 for (int32 i = 0; i < IncidentElements.Num(); i++)
1274 {
1275 IncidentElements[i] += ExtraIncidentElements[i];
1276 IncidentElementsLocal[i] += ExtraIncidentElementsLocal[i];
1277 }
1278 }
1279 }
1280
1281 inline FIntVector3 SortFIntVector3(FIntVector3& V)
1282 {
1283 if (V[1] < V[0])
1284 {
1285 int32 V0 = V[0];
1286 V[0] = V[1];
1287 V[1] = V0;
1288 }
1289 if (V[2] < V[0])
1290 {
1291 int32 V2 = V[2];
1292 V[2] = V[1];
1293 V[1] = V[0];
1294 V[0] = V2;
1295 }
1296 else if (V[2] < V[1])
1297 {
1298 int32 V1 = V[1];
1299 V[1] = V[2];
1300 V[2] = V1;
1301 }
1302 return V;
1303 }
1304
1305 // For each of the 4 triangle faces in a tet, return local vertex indices
1306 // right hand rule, triangle normal pointing outwards
1307 inline FIntVector3 TetFace(int32 f) {
1308 switch (f) {
1309 case 0:
1310 return { 1, 2, 3 };
1311 case 1:
1312 return { 0, 3, 2 };
1313 case 2:
1314 return { 0, 1, 3 };
1315 case 3:
1316 return { 0, 2, 1 };
1317 default:
1318 return { -1, -1, -1 };
1319 }
1320 }
1321
1322 template <typename Func1, typename Func2>
1323 TArray<FIntVector2> ComputeMeshFacePairs(const TArray<FIntVector4>& Mesh, Func1 GreaterThan, Func2 Equal) {
1324 int32 NumFacesPerElement = 4;
1325 int32 face_number = Mesh.Num() * NumFacesPerElement;
1326 TArray<int32> AllFaces, ranges;
1327 AllFaces.SetNum(face_number);
1328 for (int32 f = 0; f < face_number; f++) {
1329 AllFaces[f] = f;
1330 }
1331
1332 AllFaces.Sort([&GreaterThan, &Mesh](int32 face_a, int32 face_b) { return GreaterThan(Mesh, face_a, face_b); });
1333 TArray<FIntVector2> CommonFacePairs;
1334 int32 f1 = 0, f2 = 1;
1335 while (f2 < AllFaces.Num())
1336 {
1337 if (Equal(Mesh, AllFaces[f1], AllFaces[f2])) //common face from two tetrahedra
1338 {
1339 CommonFacePairs.Add(FIntVector2(AllFaces[f1], AllFaces[f2]));
1340 f1 += 2;
1341 f2 += 2;
1342 }
1343 else //boundary face
1344 {
1345 CommonFacePairs.Add(FIntVector2(AllFaces[f1], -1));
1346 f1 += 1;
1347 f2 += 1;
1348 }
1349 }
1350 if (f1 < AllFaces.Num())
1351 {
1352 CommonFacePairs.Add(FIntVector2(AllFaces[f1], -1));
1353 }
1354 return CommonFacePairs;
1355 }
1356
1357 //returns pairs of face indices {FaceA, FaceB} that are shared by two adjacent tetrahedra or boundary face {FaceA, -1}
1358 inline TArray<FIntVector2> ComputeTetMeshFacePairs(const TArray<FIntVector4>& TetMesh) {
1359 auto FaceGreaterThan = [](FIntVector3& i1, FIntVector3& i2)
1360 {
1361 SortFIntVector3(i1);
1362 SortFIntVector3(i2);
1363 if (i1[0] > i2[0])
1364 return true;
1365 else if (i1[0] == i2[0] && i1[1] > i2[1])
1366 return true;
1367 return (i1[0] == i2[0] && i1[1] == i2[1] && i1[2] > i2[2]);
1368 };
1369 auto FaceEqual = [](FIntVector3& i1, FIntVector3& i2)
1370 {
1371 SortFIntVector3(i1);
1372 SortFIntVector3(i2);
1373 return i1 == i2;
1374 };
1375 auto GreaterThan = [&FaceGreaterThan](const TArray<FIntVector4>& Mesh, int32 e1, int32 e2)
1376 {
1377 int32 Local1 = e1 % 4;
1378 int32 Element1 = e1 / 4;
1379 FIntVector3 LocalFace1 = TetFace(Local1);
1380 FIntVector3 Edge1 = FIntVector3(Mesh[Element1][LocalFace1[0]], Mesh[Element1][LocalFace1[1]], Mesh[Element1][LocalFace1[2]]);
1381 int32 Local2 = e2 % 4;
1382 int32 Element2 = e2 / 4;
1383 FIntVector3 LocalFace2 = TetFace(Local2);
1384 FIntVector3 Edge2 = FIntVector3(Mesh[Element2][LocalFace2[0]], Mesh[Element2][LocalFace2[1]], Mesh[Element2][LocalFace2[2]]);
1385 return FaceGreaterThan(Edge1, Edge2);
1386 };
1387 auto Equal = [&FaceEqual](const TArray<FIntVector4>& Mesh, int32 e1, int32 e2)
1388 {
1389 int32 Local1 = e1 % 4;
1390 int32 Element1 = e1 / 4;
1391 FIntVector3 LocalFace1 = TetFace(Local1);
1392 FIntVector3 Edge1 = FIntVector3(Mesh[Element1][LocalFace1[0]], Mesh[Element1][LocalFace1[1]], Mesh[Element1][LocalFace1[2]]);
1393 int32 Local2 = e2 % 4;
1394 int32 Element2 = e2 / 4;
1395 FIntVector3 LocalFace2 = TetFace(Local2);
1396 FIntVector3 Edge2 = FIntVector3(Mesh[Element2][LocalFace2[0]], Mesh[Element2][LocalFace2[1]], Mesh[Element2][LocalFace2[2]]);
1397 return FaceEqual(Edge1, Edge2);
1398 };
1399 return ComputeMeshFacePairs(TetMesh, GreaterThan, Equal);
1400 }
1401
1402 //Return a randomly sampled point in selected tetrahedra following uniform distribution
1403 inline TArray<TArray<FVector3f>> RandomPointsInTet(const TArray<FVector3f>& x, const TArray<FIntVector4>& TetMesh, const TArray<int32>& SampleElements, int32 NumRandomPointsPerElement = 1) {
1404 srand(1);
1405 TArray<TArray<FVector3f>> SampledPoints;
1406 SampledPoints.SetNum(SampleElements.Num());
1407 for (int32 ElemIdx = 0; ElemIdx < SampleElements.Num(); ++ElemIdx)
1408 {
1409 int32 e = SampleElements[ElemIdx];
1410 for (int32 i = 0; i < NumRandomPointsPerElement; ++i) {
1411 FVector4f Weights(0);
1412 FVector3f Point(0);
1413 float TotalWeight = 0;
1414 for (int32 j = 0; j < 4; ++j) {
1415 Weights[j] = float(rand()) / float(RAND_MAX);
1416 TotalWeight += Weights[j];
1417 }
1418 for (int32 j = 0; j < 4; ++j) {
1419 Weights[j] /= TotalWeight;
1420 Point += Weights[j] * x[TetMesh[e][j]];
1421 }
1422 SampledPoints[ElemIdx].Add(Point);
1423 }
1424 }
1425 return SampledPoints;
1426 }
1427 } // namespace Utilities
1428} // namespace Chaos
ESplineBoneAxis::Z
@ Z
check
#define check(expr)
Definition AssertionMacros.h:314
ensureMsgf
#define ensureMsgf( InExpression, InFormat,...)
Definition AssertionMacros.h:465
TEXT
#define TEXT(x)
Definition Platform.h:1272
int32
FPlatformTypes::int32 int32
A 32-bit signed integer.
Definition Platform.h:1125
StaticCastSharedRef
UE_FORCEINLINE_HINT TSharedRef< CastToType, Mode > StaticCastSharedRef(TSharedRef< CastFromType, Mode > const &InSharedRef)
Definition SharedPointer.h:127
FIntVector2
UE::Math::TIntVector2< int32 > FIntVector2
Definition MathFwd.h:91
FIntVector3
UE::Math::TIntVector3< int32 > FIntVector3
Definition MathFwd.h:92
Num
@ Num
Definition MetalRHIPrivate.h:234
float
USkinnedMeshComponent float
Definition SkinnedMeshComponent.h:60
UE_SMALL_NUMBER
#define UE_SMALL_NUMBER
Definition UnrealMathUtility.h:130
Chaos::TUniformGrid
Definition UniformGrid.h:267
Chaos::TVector< FReal, 4 >
Definition Vector.h:386
Chaos::TVector< T, 3 >
Definition Vector.h:1000
Chaos::TVector< T, 3 >::X
T X
Definition Vector.h:1168
Chaos::TVector< T, 3 >::Z
T Z
Definition Vector.h:1170
Chaos::TVector< T, 3 >::Y
T Y
Definition Vector.h:1169
Constraints
Definition Constraints.Build.cs:6
TArray
Definition Array.h:670
TArray::Num
UE_REWRITE SizeType Num() const
Definition Array.h:1144
TArray::Emplace
UE_FORCEINLINE_HINT SizeType Emplace(ArgsType &&... Args)
Definition Array.h:2561
TArray::SetNum
void SetNum(SizeType NewNum, EAllowShrinking AllowShrinking=UE::Core::Private::AllowShrinkingByDefault< AllocatorType >())
Definition Array.h:2308
TArray::Add
UE_NODEBUG UE_FORCEINLINE_HINT SizeType Add(ElementType &&Item)
Definition Array.h:2696
TArray::Init
void Init(const ElementType &Element, SizeType Number)
Definition Array.h:3043
TArray::Heapify
UE_NODEBUG UE_FORCEINLINE_HINT void Heapify(const PREDICATE_CLASS &Predicate)
Definition Array.h:3640
TArray::Sort
UE_NODEBUG void Sort()
Definition Array.h:3418
TMap
Definition UnrealString.h.inl:34
Chaos::Utilities::AsinEst3
T AsinEst3(const T X)
Definition Utilities.h:841
Chaos::Utilities::NormalizeSafe
bool NormalizeSafe(FVec3 &V, FReal EpsilonSq=UE_SMALL_NUMBER)
Definition Utilities.h:492
Chaos::Utilities::AsinEst7
T AsinEst7(const T X)
Definition Utilities.h:869
Chaos::Utilities::ComputeDiagonalJointFactorMatrix
TVec3< T > ComputeDiagonalJointFactorMatrix(const TVec3< T > &V, const PMatrix< T, 3, 3 > &M, const T &Im)
Definition Utilities.h:367
Chaos::Utilities::FindConnectedRegions
void FindConnectedRegions(const TArray< FIntVector4 > &Elements, TArray< TArray< int32 > > &ConnectedComponents)
Definition Utilities.h:1078
Chaos::Utilities::GetAverage
T GetAverage(const TARRAY &Values)
Compute the average value.
Definition Utilities.h:81
Chaos::Utilities::NearestPointsOnLineSegmentToLine
void NearestPointsOnLineSegmentToLine(const FVec3 &SegmentBegin, const FVec3 &SegmentEnd, const FVec3 &LinePos, const FVec3 &LineVector, FReal &S, FReal &T, FVec3 &C1, FVec3 &C2, const FReal Epsilon=1.e-4f)
Definition Utilities.h:713
Chaos::Utilities::TetMeshFromGrid
void TetMeshFromGrid(const TUniformGrid< T, 3 > &Grid, TArray< TV_INT4 > &Mesh, TArray< TV > &X)
Generate a tetrahedral mesh from a Freudenthal lattice defined on a 3 dimensional grid.
Definition Utilities.h:924
Chaos::Utilities::ScaleInertia
TVec3< T > ScaleInertia(const TVec3< T > &Inertia, const TVec3< T > &Scale, const bool bScaleMass)
Definition Utilities.h:519
Chaos::Utilities::ComputeMeshFacePairs
TArray< FIntVector2 > ComputeMeshFacePairs(const TArray< FIntVector4 > &Mesh, Func1 GreaterThan, Func2 Equal)
Definition Utilities.h:1323
Chaos::Utilities::ComputeIncidentElements
TArray< TArray< int > > ComputeIncidentElements(const TArray< TVector< int32, d > > &Mesh, TArray< TArray< int32 > > *LocalIndex=nullptr)
Definition Utilities.h:1013
Chaos::Utilities::ProjectPointOntoAxisAlignedPlane
void ProjectPointOntoAxisAlignedPlane(FVec3 &V, const FVec3 &Dir, int32 AxisIndex, FReal PlaneDir, FReal PlanePos)
Definition Utilities.h:465
Chaos::Utilities::AsinEst5
T AsinEst5(const T X)
Definition Utilities.h:854
Chaos::Utilities::MultiplyAB
FMatrix33 MultiplyAB(const FMatrix33 &LIn, const FMatrix33 &RIn)
Definition Utilities.h:204
Chaos::Utilities::MultiplyAtB
FMatrix33 MultiplyAtB(const FMatrix33 &L, const FMatrix33 &R)
Definition Utilities.h:226
Chaos::Utilities::Factorial
TINT Factorial(TINT Num)
Take the factorial of Num, which should be of integral type.
Definition Utilities.h:29
Chaos::Utilities::ProjectPointOntoAxisAlignedPlaneSafe
bool ProjectPointOntoAxisAlignedPlaneSafe(FVec3 &V, const FVec3 &Dir, int32 AxisIndex, FReal PlaneDir, FReal PlanePos, FReal Epsilon)
Definition Utilities.h:478
Chaos::Utilities::DotProduct
T DotProduct(const TArray< T > &X, const TArray< T > &Y)
Definition Utilities.h:504
Chaos::Utilities::TetFace
FIntVector3 TetFace(int32 f)
Definition Utilities.h:1307
Chaos::Utilities::SignedSquare
TRealType SignedSquare(TRealType A)
Definition Utilities.h:22
Chaos::Utilities::DFS_iterative
void DFS_iterative(const TArray< TArray< int32 > > &L, int32 v, TArray< bool > &visited, TArray< int32 > &component)
Definition Utilities.h:1047
Chaos::Utilities::SortFIntVector3
FIntVector3 SortFIntVector3(FIntVector3 &V)
Definition Utilities.h:1281
Chaos::Utilities::MergeIncidentElements
void MergeIncidentElements(const TArray< TArray< int32 > > &ExtraIncidentElements, const TArray< TArray< int32 > > &ExtraIncidentElementsLocal, TArray< TArray< int32 > > &IncidentElements, TArray< TArray< int32 > > &IncidentElementsLocal)
Definition Utilities.h:1269
Chaos::Utilities::RaySphereIntersectionDistance
T RaySphereIntersectionDistance(const TVec3< T > &RayStart, const TVec3< T > &RayDir, const TVec3< T > &SpherePos, const T SphereRadius)
The distance from Start to the sphere along the vector Dir. Returns numeric max if no intersection.
Definition Utilities.h:810
Chaos::Utilities::ClipLineSegmentToAxisAlignedPlane
bool ClipLineSegmentToAxisAlignedPlane(FVec3 &V0, FVec3 &V1, const int32 AxisIndex, const FReal PlaneDir, const FReal PlanePos)
Definition Utilities.h:435
Chaos::Utilities::AsinEst
T AsinEst(const T X)
Definition Utilities.h:883
Chaos::Utilities::NearestPointsOnLineSegments
void NearestPointsOnLineSegments(const FVec3 &P1, const FVec3 &Q1, const FVec3 &P2, const FVec3 &Q2, FReal &S, FReal &T, FVec3 &C1, FVec3 &C2, const FReal Epsilon=1.e-4f)
Definition Utilities.h:649
Chaos::Utilities::MultiplyABt
FMatrix33 MultiplyABt(const FMatrix33 &L, const FMatrix33 &R)
Definition Utilities.h:209
Chaos::Utilities::AsinEst1
T AsinEst1(const T X)
Definition Utilities.h:830
Chaos::Utilities::ComputeWorldSpaceInertia
FMatrix33 ComputeWorldSpaceInertia(const FRotation3 &CoMRotation, const FMatrix33 &I)
Definition Utilities.h:327
Chaos::Utilities::NPermuteR
TINT NPermuteR(const TINT N, const TINT R)
Number of ways to choose of R elements from a set of size N, with repetitions.
Definition Utilities.h:50
Chaos::Utilities::GetStandardDeviation
T GetStandardDeviation(const TARRAY &Values, const T Avg)
Compute the standard deviation of Values, given the average value of Avg.
Definition Utilities.h:121
Chaos::Utilities::ClosestTimeOnLineSegment
T ClosestTimeOnLineSegment(const TVec3< T > &Point, const TVec3< T > &StartPoint, const TVec3< T > &EndPoint)
Definition Utilities.h:783
Chaos::Utilities::NChooseR
TINT NChooseR(const TINT N, const TINT R)
Number of ways to choose of R elements from a set of size N, with no repetitions.
Definition Utilities.h:42
Chaos::Utilities::IntersectLineSegments2D
bool IntersectLineSegments2D(const TVec2< T > &InStartA, const TVec2< T > &InEndA, const TVec2< T > &InStartB, const TVec2< T > &InEndB, T &OutTA, T &OutTB)
Definition Utilities.h:383
Chaos::Utilities::GetAngularVelocityAdjustedForGyroscopicTorques
FVec3 GetAngularVelocityAdjustedForGyroscopicTorques(const FRotation3 &Q, const FVec3 &I, const FVec3 &W, const FReal Dt)
Definition Utilities.cpp:27
Chaos::Utilities::ComputeTetMeshFacePairs
TArray< FIntVector2 > ComputeTetMeshFacePairs(const TArray< FIntVector4 > &TetMesh)
Definition Utilities.h:1358
Chaos::Utilities::GetMinAvgMax
void GetMinAvgMax(const TARRAY &Values, T &MinV, double &AvgV, T &MaxV)
Compute the minimum, average, and maximum values of Values.
Definition Utilities.h:58
Chaos::Utilities::BoxFromInertia
bool BoxFromInertia(const FVec3 &InInertia, const FReal Mass, FVec3 &OutCenter, FVec3 &OutSize)
Definition Utilities.h:560
Chaos::Utilities::AsinEstCrossover
T AsinEstCrossover(const T X, const T Crossover=T(0.7))
Definition Utilities.h:908
Chaos::Utilities::RandomPointsInTet
TArray< TArray< FVector3f > > RandomPointsInTet(const TArray< FVector3f > &x, const TArray< FIntVector4 > &TetMesh, const TArray< int32 > &SampleElements, int32 NumRandomPointsPerElement=1)
Definition Utilities.h:1403
Chaos::Utilities::ComputeJointFactorMatrix
PMatrix< T, 3, 3 > ComputeJointFactorMatrix(const TVec3< T > &V, const PMatrix< T, 3, 3 > &M, const T &Im)
Definition Utilities.h:349
Chaos::Utilities::ConnectedComponentsDFSIterative
void ConnectedComponentsDFSIterative(const TArray< TArray< int32 > > &L, TArray< TArray< int32 > > &C)
Definition Utilities.h:1064
Chaos::Utilities::Solve
bool Solve(FVec3 &X, const FMatrix33 &A, const FVec3 &B)
Definition Utilities.h:279
Chaos::Utilities::GetVariance
T GetVariance(const TARRAY &Values, const T Avg)
Compute the variance of Values, given the average value of Avg.
Definition Utilities.h:97
Chaos::Utilities::ComputeBoundaryNodes
TArray< int32 > ComputeBoundaryNodes(const TArray< TVec3< int32 > > &TriangleMesh)
Definition Utilities.h:1188
Chaos::Utilities::ClipLineSegmentToPlane
bool ClipLineSegmentToPlane(FVec3 &V0, FVec3 &V1, const FVec3 &PlaneNormal, const FVec3 &PlanePos)
Definition Utilities.h:406
Chaos::Utilities::WrapIndex
int32 WrapIndex(int32 V, int32 Begin, int32 End)
Definition Utilities.h:632
Chaos
Definition SkeletalMeshComponent.h:307
Chaos::FRigidTransform3
TRigidTransform< FReal, 3 > FRigidTransform3
Definition Core.h:22
Chaos::Y
@ Y
Definition SimulationModuleBase.h:153
Chaos::X
@ X
Definition SimulationModuleBase.h:152
Chaos::FVec4
TVector< FReal, 4 > FVec4
Definition Core.h:18
Chaos::FReal
FRealDouble FReal
Definition Real.h:22
Chaos::Order
void Order(int32 &A, int32 &B)
Definition TriangleMesh.cpp:792
Chaos::FMatrix33
PMatrix< FReal, 3, 3 > FMatrix33
Definition Core.h:20
Chaos::FMatrix44
PMatrix< FReal, 4, 4 > FMatrix44
Definition Core.h:21
Chaos::FVec3
TVector< FReal, 3 > FVec3
Definition Core.h:17
v
float v
Definition radaudio_mdct.cpp:62
FMath::Lerp
static constexpr UE_FORCEINLINE_HINT T Lerp(const T &A, const T &B, const U &Alpha)
Definition UnrealMathUtility.h:1116
FMath::IsNearlyZero
static UE_FORCEINLINE_HINT bool IsNearlyZero(float Value, float ErrorTolerance=UE_SMALL_NUMBER)
Definition UnrealMathUtility.h:407
TIsIntegral
Definition IsIntegral.h:12
TNumericLimits
Definition NumericLimits.h:41
TTuple
Definition Tuple.h:652