Jolt Physics
A multi core friendly Game Physics Engine
Loading...
Searching...
No Matches
ContactConstraintManager.h
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
17
19#include <atomic>
21
23
24struct PhysicsSettings;
25class PhysicsUpdateContext;
26
31{
32public:
34
36 explicit ContactConstraintManager(const PhysicsSettings &inPhysicsSettings);
38
42 void Init(uint inMaxBodyPairs, uint inMaxContactConstraints);
43
45 void SetContactListener(ContactListener *inListener) { mContactListener = inListener; }
46 ContactListener * GetContactListener() const { return mContactListener; }
47
51 using CombineFunction = float (*)(const Body &inBody1, const SubShapeID &inSubShapeID1, const Body &inBody2, const SubShapeID &inSubShapeID2);
52
55 void SetCombineFriction(CombineFunction inCombineFriction) { mCombineFriction = inCombineFriction; }
56 CombineFunction GetCombineFriction() const { return mCombineFriction; }
57
60 void SetCombineRestitution(CombineFunction inCombineRestitution) { mCombineRestitution = inCombineRestitution; }
61 CombineFunction GetCombineRestitution() const { return mCombineRestitution; }
62
64 uint32 GetMaxConstraints() const { return mMaxConstraints; }
65
67 inline ValidateResult ValidateContactPoint(const Body &inBody1, const Body &inBody2, RVec3Arg inBaseOffset, const CollideShapeResult &inCollisionResult) const
68 {
69 if (mContactListener == nullptr)
71
72 return mContactListener->OnContactValidate(inBody1, inBody2, inBaseOffset, inCollisionResult);
73 }
74
76 void PrepareConstraintBuffer(PhysicsUpdateContext *inContext);
77
79 static const int MaxContactPoints = 4;
80
83 {
84 public:
86
87 uint mNumBodyPairs = 0;
88 uint mNumManifolds = 0;
90 };
91
93 ContactAllocator GetContactAllocator() { return mCache[mCacheWriteIdx].GetContactAllocator(); }
94
98 void GetContactsFromCache(ContactAllocator &ioContactAllocator, Body &inBody1, Body &inBody2, bool &outPairHandled, bool &outConstraintCreated);
99
101 using BodyPairHandle = void *;
102
105 BodyPairHandle AddBodyPair(ContactAllocator &ioContactAllocator, const Body &inBody1, const Body &inBody2);
106
148 bool AddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPair, Body &inBody1, Body &inBody2, const ContactManifold &inManifold);
149
154 void FinalizeContactCacheAndCallContactPointRemovedCallbacks(uint inExpectedNumBodyPairs, uint inExpectedNumManifolds);
155
158 bool WereBodiesInContact(const BodyID &inBody1ID, const BodyID &inBody2ID) const;
159
161 uint32 GetNumConstraints() const { return min<uint32>(mNumConstraints, mMaxConstraints); }
162
164 void SortContacts(uint32 *inConstraintIdxBegin, uint32 *inConstraintIdxEnd) const;
165
167 inline void GetAffectedBodies(uint32 inConstraintIdx, const Body *&outBody1, const Body *&outBody2) const
168 {
169 const ContactConstraint &constraint = mConstraints[inConstraintIdx];
170 outBody1 = constraint.mBody1;
171 outBody2 = constraint.mBody2;
172 }
173
175 template <class MotionPropertiesCallback>
176 void WarmStartVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, MotionPropertiesCallback &ioCallback);
177
208 bool SolveVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd);
209
211 void StoreAppliedImpulses(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd) const;
212
229 bool SolvePositionConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd);
230
232 void RecycleConstraintBuffer();
233
235 void FinishConstraintBuffer();
236
243 void OnCCDContactAdded(ContactAllocator &ioContactAllocator, const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &outSettings);
244
245#ifdef JPH_DEBUG_RENDERER
246 // Drawing properties
247 static bool sDrawContactPoint;
251#endif // JPH_DEBUG_RENDERER
252
254 void SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const;
255
257 bool RestoreState(StateRecorder &inStream, const StateRecorderFilter *inFilter);
258
259private:
261 class CachedContactPoint
262 {
263 public:
265 void SaveState(StateRecorder &inStream) const;
266 void RestoreState(StateRecorder &inStream);
267
270 Float3 mPosition1;
271 Float3 mPosition2;
272
274 float mNonPenetrationLambda;
275 Vector<2> mFrictionLambda;
276 };
277
278 static_assert(sizeof(CachedContactPoint) == 36, "Unexpected size");
279 static_assert(alignof(CachedContactPoint) == 4, "Assuming 4 byte aligned");
280
282 class CachedManifold
283 {
284 public:
286 static int sGetRequiredExtraSize(int inNumContactPoints) { return max(0, inNumContactPoints - 1) * sizeof(CachedContactPoint); }
287
289 static int sGetRequiredTotalSize(int inNumContactPoints) { return sizeof(CachedManifold) + sGetRequiredExtraSize(inNumContactPoints); }
290
292 void SaveState(StateRecorder &inStream) const;
293 void RestoreState(StateRecorder &inStream);
294
296 uint32 mNextWithSameBodyPair;
297
300 Float3 mContactNormal;
301
303 enum class EFlags : uint16
304 {
305 ContactPersisted = 1,
306 CCDContact = 2
307 };
308
310 mutable atomic<uint16> mFlags { 0 };
311
313 uint16 mNumContactPoints;
314
316 CachedContactPoint mContactPoints[1];
317 };
318
319 static_assert(sizeof(CachedManifold) == 56, "This structure is expect to not contain any waste due to alignment");
320 static_assert(alignof(CachedManifold) == 4, "Assuming 4 byte aligned");
321
324 using MKeyValue = ManifoldMap::KeyValue;
325 using MKVAndCreated = std::pair<MKeyValue *, bool>;
326
328 class CachedBodyPair
329 {
330 public:
332 void SaveState(StateRecorder &inStream) const;
333 void RestoreState(StateRecorder &inStream);
334
337 Float3 mDeltaPosition;
338
341 Float3 mDeltaRotation;
342
344 uint32 mFirstCachedManifold;
345 };
346
347 static_assert(sizeof(CachedBodyPair) == 28, "Unexpected size");
348 static_assert(alignof(CachedBodyPair) == 4, "Assuming 4 byte aligned");
349
352 using BPKeyValue = BodyPairMap::KeyValue;
353
355 class ManifoldCache
356 {
357 public:
359 void Init(uint inMaxBodyPairs, uint inMaxContactConstraints, uint inCachedManifoldsSize);
360
362 void Clear();
363
366 void Prepare(uint inExpectedNumBodyPairs, uint inExpectedNumManifolds);
367
369 ContactAllocator GetContactAllocator() { return ContactAllocator(mAllocator, cAllocatorBlockSize); }
370
372 const MKeyValue * Find(const SubShapeIDPair &inKey, uint64 inKeyHash) const;
373 MKeyValue * Create(ContactAllocator &ioContactAllocator, const SubShapeIDPair &inKey, uint64 inKeyHash, int inNumContactPoints);
374 MKVAndCreated FindOrCreate(ContactAllocator &ioContactAllocator, const SubShapeIDPair &inKey, uint64 inKeyHash, int inNumContactPoints);
375 uint32 ToHandle(const MKeyValue *inKeyValue) const;
376 const MKeyValue * FromHandle(uint32 inHandle) const;
377
379 const BPKeyValue * Find(const BodyPair &inKey, uint64 inKeyHash) const;
380 BPKeyValue * Create(ContactAllocator &ioContactAllocator, const BodyPair &inKey, uint64 inKeyHash);
381 void GetAllBodyPairsSorted(Array<const BPKeyValue *> &outAll) const;
382 void GetAllManifoldsSorted(const CachedBodyPair &inBodyPair, Array<const MKeyValue *> &outAll) const;
383 void GetAllCCDManifoldsSorted(Array<const MKeyValue *> &outAll) const;
384 void ContactPointRemovedCallbacks(ContactListener *inListener);
385
386#ifdef JPH_ENABLE_ASSERTS
388 uint GetNumManifolds() const { return mCachedManifolds.GetNumKeyValues(); }
389
391 uint GetNumBodyPairs() const { return mCachedBodyPairs.GetNumKeyValues(); }
392
394 void Finalize();
395#endif
396
398 void SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const;
399 bool RestoreState(const ManifoldCache &inReadCache, StateRecorder &inStream, const StateRecorderFilter *inFilter);
400
401 private:
403 static constexpr uint32 cAllocatorBlockSize = 4096;
404
406 LFHMAllocator mAllocator;
407
409 ManifoldMap mCachedManifolds { mAllocator };
410
412 BodyPairMap mCachedBodyPairs { mAllocator };
413
414#ifdef JPH_ENABLE_ASSERTS
415 bool mIsFinalized = false;
416#endif
417 };
418
419 ManifoldCache mCache[2];
420 int mCacheWriteIdx = 0;
421
423 class WorldContactPoint
424 {
425 public:
427 void CalculateNonPenetrationConstraintProperties(const Body &inBody1, float inInvMass1, float inInvInertiaScale1, const Body &inBody2, float inInvMass2, float inInvInertiaScale2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal);
428
429 template <EMotionType Type1, EMotionType Type2>
430 JPH_INLINE void TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(float inDeltaTime, float inGravityDeltaTimeDotNormal, const Body &inBody1, const Body &inBody2, float inInvM1, float inInvM2, Mat44Arg inInvI1, Mat44Arg inInvI2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal, Vec3Arg inWorldSpaceTangent1, Vec3Arg inWorldSpaceTangent2, const ContactSettings &inSettings, float inMinVelocityForRestitution);
431
433 AxisConstraintPart mNonPenetrationConstraint;
434 AxisConstraintPart mFrictionConstraint1;
435 AxisConstraintPart mFrictionConstraint2;
436
438 CachedContactPoint * mContactPoint;
439 };
440
441 using WorldContactPoints = StaticArray<WorldContactPoint, MaxContactPoints>;
442
444 class ContactConstraint
445 {
446 public:
447 #ifdef JPH_DEBUG_RENDERER
449 void Draw(DebugRenderer *inRenderer, ColorArg inManifoldColor) const;
450 #endif // JPH_DEBUG_RENDERER
451
453 JPH_INLINE Vec3 GetWorldSpaceNormal() const
454 {
455 return Vec3::sLoadFloat3Unsafe(mWorldSpaceNormal);
456 }
457
459 JPH_INLINE void GetTangents(Vec3 &outTangent1, Vec3 &outTangent2) const
460 {
461 Vec3 ws_normal = GetWorldSpaceNormal();
462 outTangent1 = ws_normal.GetNormalizedPerpendicular();
463 outTangent2 = ws_normal.Cross(outTangent1);
464 }
465
466 Body * mBody1;
467 Body * mBody2;
468 uint64 mSortKey;
469 Float3 mWorldSpaceNormal;
470 float mCombinedFriction;
471 float mInvMass1;
472 float mInvInertiaScale1;
473 float mInvMass2;
474 float mInvInertiaScale2;
475 WorldContactPoints mContactPoints;
476 };
477
478public:
480 static constexpr uint cMaxContactConstraintsLimit = ~uint(0) / sizeof(ContactConstraint);
481
483 static constexpr uint cMaxBodyPairsLimit = ~uint(0) / sizeof(BodyPairMap::KeyValue);
484
485private:
487 template <EMotionType Type1, EMotionType Type2>
488 JPH_INLINE void TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravityDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2);
489
491 inline void CalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravityDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2);
492
494 template <EMotionType Type1, EMotionType Type2>
495 bool TemplatedAddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPairHandle, Body &inBody1, Body &inBody2, const ContactManifold &inManifold);
496
498 template <EMotionType Type1, EMotionType Type2>
499 JPH_INLINE static void sWarmStartConstraint(ContactConstraint &ioConstraint, MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2, float inWarmStartImpulseRatio);
500
502 template <EMotionType Type1, EMotionType Type2>
503 JPH_INLINE static bool sSolveVelocityConstraint(ContactConstraint &ioConstraint, MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2);
504
506 const PhysicsSettings & mPhysicsSettings;
507
509 ContactListener * mContactListener = nullptr;
510
512 CombineFunction mCombineFriction = [](const Body &inBody1, const SubShapeID &, const Body &inBody2, const SubShapeID &) { return sqrt(inBody1.GetFriction() * inBody2.GetFriction()); };
513 CombineFunction mCombineRestitution = [](const Body &inBody1, const SubShapeID &, const Body &inBody2, const SubShapeID &) { return max(inBody1.GetRestitution(), inBody2.GetRestitution()); };
514
516 ContactConstraint * mConstraints = nullptr;
517 uint32 mMaxConstraints = 0;
518 atomic<uint32> mNumConstraints { 0 };
519
521 PhysicsUpdateContext * mUpdateContext;
522};
523
ValidateResult
Definition ContactListener.h:57
@ AcceptAllContactsForThisBodyPair
Accept this and any further contact points for this body pair.
#define JPH_EXPORT
Definition Core.h:275
#define JPH_SUPPRESS_WARNINGS_STD_BEGIN
Definition Core.h:424
#define JPH_SUPPRESS_WARNINGS_STD_END
Definition Core.h:437
std::uint64_t uint64
Definition Core.h:496
unsigned int uint
Definition Core.h:492
#define JPH_NAMESPACE_END
Definition Core.h:419
std::uint32_t uint32
Definition Core.h:495
#define JPH_NAMESPACE_BEGIN
Definition Core.h:413
std::uint16_t uint16
Definition Core.h:494
EPhysicsUpdateError
Enum used by PhysicsSystem to report error conditions during the PhysicsSystem::Update call....
Definition EPhysicsUpdateError.h:11
#define JPH_OVERRIDE_NEW_DELETE
Macro to override the new and delete functions.
Definition Memory.h:43
Definition Array.h:36
Definition AxisConstraintPart.h:43
Definition Body.h:39
float GetRestitution() const
Restitution (dimensionless number, usually between 0 and 1, 0 = completely inelastic collision respon...
Definition Body.h:146
float GetFriction() const
Friction (dimensionless number, usually between 0 and 1, 0 = no friction, 1 = friction force equals f...
Definition Body.h:142
ID of a body. This is a way of reasoning about bodies in a multithreaded simulation while avoiding ra...
Definition BodyID.h:13
Class that contains all information of two colliding shapes.
Definition CollideShape.h:19
Class that holds an RGBA color with 8-bits per component.
Definition Color.h:16
Contacts are allocated in a lock free hash map.
Definition ContactConstraintManager.h:83
Definition ContactConstraintManager.h:31
static bool sDrawContactPoint
Definition ContactConstraintManager.h:247
CombineFunction GetCombineFriction() const
Definition ContactConstraintManager.h:56
CombineFunction GetCombineRestitution() const
Definition ContactConstraintManager.h:61
ValidateResult ValidateContactPoint(const Body &inBody1, const Body &inBody2, RVec3Arg inBaseOffset, const CollideShapeResult &inCollisionResult) const
Check with the listener if inBody1 and inBody2 could collide, returns false if not.
Definition ContactConstraintManager.h:67
uint32 GetMaxConstraints() const
Get the max number of contact constraints that are allowed.
Definition ContactConstraintManager.h:64
static bool sDrawContactPointReduction
Definition ContactConstraintManager.h:249
void * BodyPairHandle
Handle used to keep track of the current body pair.
Definition ContactConstraintManager.h:101
ContactListener * GetContactListener() const
Definition ContactConstraintManager.h:46
static bool sDrawSupportingFaces
Definition ContactConstraintManager.h:248
void GetAffectedBodies(uint32 inConstraintIdx, const Body *&outBody1, const Body *&outBody2) const
Get the affected bodies for a given constraint.
Definition ContactConstraintManager.h:167
uint32 GetNumConstraints() const
Get the number of contact constraints that were found.
Definition ContactConstraintManager.h:161
void SetCombineFriction(CombineFunction inCombineFriction)
Definition ContactConstraintManager.h:55
void SetContactListener(ContactListener *inListener)
Listener that is notified whenever a contact point between two bodies is added/updated/removed.
Definition ContactConstraintManager.h:45
float(*)(const Body &inBody1, const SubShapeID &inSubShapeID1, const Body &inBody2, const SubShapeID &inSubShapeID2) CombineFunction
Definition ContactConstraintManager.h:51
void SetCombineRestitution(CombineFunction inCombineRestitution)
Definition ContactConstraintManager.h:60
ContactAllocator GetContactAllocator()
Get a new allocator context for storing contacts. Note that you should call this once and then add mu...
Definition ContactConstraintManager.h:93
static bool sDrawContactManifolds
Definition ContactConstraintManager.h:250
Definition ContactListener.h:79
Manifold class, describes the contact surface between two bodies.
Definition ContactListener.h:20
Definition ContactListener.h:41
Definition DebugRenderer.h:47
Class that holds 3 floats. Used as a storage class. Convert to Vec3 for calculations.
Definition Float3.h:13
Definition LockFreeHashMap.h:49
LFHMAllocatorContext(LFHMAllocator &inAllocator, uint32 inBlockSize)
Construct a new allocator context.
Definition LockFreeHashMap.inl:80
Allocator for a lock free hash map.
Definition LockFreeHashMap.h:14
Definition LockFreeHashMap.h:72
Holds a 4x4 matrix of floats, but supports also operations on the 3x3 upper left part of the matrix.
Definition Mat44.h:13
The Body class only keeps track of state for static bodies, the MotionProperties class keeps the addi...
Definition MotionProperties.h:29
Class that makes another class non-copyable. Usage: Inherit from NonCopyable.
Definition NonCopyable.h:11
User callbacks that allow determining which parts of the simulation should be saved by a StateRecorde...
Definition StateRecorder.h:79
Definition StateRecorder.h:110
Simple variable length array backed by a fixed size buffer.
Definition StaticArray.h:14
A sub shape id contains a path to an element (usually a triangle or other primitive type) of a compou...
Definition SubShapeID.h:23
A pair of bodies and their sub shape ID's. Can be used as a key in a map to find a contact point.
Definition SubShapeIDPair.h:15
Definition Vec3.h:17
JPH_INLINE Vec3 Cross(Vec3Arg inV2) const
Cross product.
Definition Vec3.inl:595
JPH_INLINE Vec3 GetNormalizedPerpendicular() const
Get normalized vector that is perpendicular to this vector.
Definition Vec3.inl:827
static JPH_INLINE Vec3 sLoadFloat3Unsafe(const Float3 &inV)
Load 3 floats from memory (reads 32 bits extra which it doesn't use)
Definition Vec3.inl:135
Templatized vector class.
Definition Vector.h:12
Structure that holds a body pair.
Definition BodyPair.h:14
Definition PhysicsSettings.h:28