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;
26
28{
29public:
31
33 explicit ContactConstraintManager(const PhysicsSettings &inPhysicsSettings);
35
39 void Init(uint inMaxBodyPairs, uint inMaxContactConstraints);
40
42 void SetContactListener(ContactListener *inListener) { mContactListener = inListener; }
43 ContactListener * GetContactListener() const { return mContactListener; }
44
48 using CombineFunction = float (*)(const Body &inBody1, const SubShapeID &inSubShapeID1, const Body &inBody2, const SubShapeID &inSubShapeID2);
49
52 void SetCombineFriction(CombineFunction inCombineFriction) { mCombineFriction = inCombineFriction; }
53 CombineFunction GetCombineFriction() const { return mCombineFriction; }
54
57 void SetCombineRestitution(CombineFunction inCombineRestitution) { mCombineRestitution = inCombineRestitution; }
58 CombineFunction GetCombineRestitution() const { return mCombineRestitution; }
59
61 uint32 GetMaxConstraints() const { return mMaxConstraints; }
62
64 inline ValidateResult ValidateContactPoint(const Body &inBody1, const Body &inBody2, RVec3Arg inBaseOffset, const CollideShapeResult &inCollisionResult) const
65 {
66 if (mContactListener == nullptr)
67 return ValidateResult::AcceptAllContactsForThisBodyPair;
68
69 return mContactListener->OnContactValidate(inBody1, inBody2, inBaseOffset, inCollisionResult);
70 }
71
73 void PrepareConstraintBuffer(PhysicsUpdateContext *inContext);
74
76 static const int MaxContactPoints = 4;
77
80 {
81 public:
83
84 uint mNumBodyPairs = 0;
85 uint mNumManifolds = 0;
86 EPhysicsUpdateError mErrors = EPhysicsUpdateError::None;
87 };
88
90 ContactAllocator GetContactAllocator() { return mCache[mCacheWriteIdx].GetContactAllocator(); }
91
95 void GetContactsFromCache(ContactAllocator &ioContactAllocator, Body &inBody1, Body &inBody2, bool &outPairHandled, bool &outConstraintCreated);
96
98 using BodyPairHandle = void *;
99
102 BodyPairHandle AddBodyPair(ContactAllocator &ioContactAllocator, const Body &inBody1, const Body &inBody2);
103
145 bool AddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPair, Body &inBody1, Body &inBody2, const ContactManifold &inManifold);
146
151 void FinalizeContactCacheAndCallContactPointRemovedCallbacks(uint inExpectedNumBodyPairs, uint inExpectedNumManifolds);
152
155 bool WereBodiesInContact(const BodyID &inBody1ID, const BodyID &inBody2ID) const;
156
158 uint32 GetNumConstraints() const { return min<uint32>(mNumConstraints, mMaxConstraints); }
159
161 void SortContacts(uint32 *inConstraintIdxBegin, uint32 *inConstraintIdxEnd) const;
162
164 inline void GetAffectedBodies(uint32 inConstraintIdx, const Body *&outBody1, const Body *&outBody2) const
165 {
166 const ContactConstraint &constraint = mConstraints[inConstraintIdx];
167 outBody1 = constraint.mBody1;
168 outBody2 = constraint.mBody2;
169 }
170
172 template <class MotionPropertiesCallback>
173 void WarmStartVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, MotionPropertiesCallback &ioCallback);
174
205 bool SolveVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd);
206
208 void StoreAppliedImpulses(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd) const;
209
226 bool SolvePositionConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd);
227
229 void RecycleConstraintBuffer();
230
232 void FinishConstraintBuffer();
233
240 void OnCCDContactAdded(ContactAllocator &ioContactAllocator, const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &outSettings);
241
242#ifdef JPH_DEBUG_RENDERER
243 // Drawing properties
244 static bool sDrawContactPoint;
248#endif // JPH_DEBUG_RENDERER
249
251 void SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const;
252
254 bool RestoreState(StateRecorder &inStream);
255
256private:
258 class CachedContactPoint
259 {
260 public:
262 void SaveState(StateRecorder &inStream) const;
263 void RestoreState(StateRecorder &inStream);
264
267 Float3 mPosition1;
268 Float3 mPosition2;
269
271 float mNonPenetrationLambda;
272 Vector<2> mFrictionLambda;
273 };
274
275 static_assert(sizeof(CachedContactPoint) == 36, "Unexpected size");
276 static_assert(alignof(CachedContactPoint) == 4, "Assuming 4 byte aligned");
277
279 class CachedManifold
280 {
281 public:
283 static int sGetRequiredExtraSize(int inNumContactPoints) { return max(0, inNumContactPoints - 1) * sizeof(CachedContactPoint); }
284
286 static int sGetRequiredTotalSize(int inNumContactPoints) { return sizeof(CachedManifold) + sGetRequiredExtraSize(inNumContactPoints); }
287
289 void SaveState(StateRecorder &inStream) const;
290 void RestoreState(StateRecorder &inStream);
291
293 uint32 mNextWithSameBodyPair;
294
297 Float3 mContactNormal;
298
300 enum class EFlags : uint16
301 {
302 ContactPersisted = 1,
303 CCDContact = 2
304 };
305
307 mutable atomic<uint16> mFlags { 0 };
308
310 uint16 mNumContactPoints;
311
313 CachedContactPoint mContactPoints[1];
314 };
315
316 static_assert(sizeof(CachedManifold) == 56, "This structure is expect to not contain any waste due to alignment");
317 static_assert(alignof(CachedManifold) == 4, "Assuming 4 byte aligned");
318
321 using MKeyValue = ManifoldMap::KeyValue;
322 using MKVAndCreated = pair<MKeyValue *, bool>;
323
325 class CachedBodyPair
326 {
327 public:
329 void SaveState(StateRecorder &inStream) const;
330 void RestoreState(StateRecorder &inStream);
331
334 Float3 mDeltaPosition;
335
338 Float3 mDeltaRotation;
339
341 uint32 mFirstCachedManifold;
342 };
343
344 static_assert(sizeof(CachedBodyPair) == 28, "Unexpected size");
345 static_assert(alignof(CachedBodyPair) == 4, "Assuming 4 byte aligned");
346
349 using BPKeyValue = BodyPairMap::KeyValue;
350
352 class ManifoldCache
353 {
354 public:
356 void Init(uint inMaxBodyPairs, uint inMaxContactConstraints, uint inCachedManifoldsSize);
357
359 void Clear();
360
363 void Prepare(uint inExpectedNumBodyPairs, uint inExpectedNumManifolds);
364
366 ContactAllocator GetContactAllocator() { return ContactAllocator(mAllocator, cAllocatorBlockSize); }
367
369 const MKeyValue * Find(const SubShapeIDPair &inKey, uint64 inKeyHash) const;
370 MKeyValue * Create(ContactAllocator &ioContactAllocator, const SubShapeIDPair &inKey, uint64 inKeyHash, int inNumContactPoints);
371 MKVAndCreated FindOrCreate(ContactAllocator &ioContactAllocator, const SubShapeIDPair &inKey, uint64 inKeyHash, int inNumContactPoints);
372 uint32 ToHandle(const MKeyValue *inKeyValue) const;
373 const MKeyValue * FromHandle(uint32 inHandle) const;
374
376 const BPKeyValue * Find(const BodyPair &inKey, uint64 inKeyHash) const;
377 BPKeyValue * Create(ContactAllocator &ioContactAllocator, const BodyPair &inKey, uint64 inKeyHash);
378 void GetAllBodyPairsSorted(Array<const BPKeyValue *> &outAll) const;
379 void GetAllManifoldsSorted(const CachedBodyPair &inBodyPair, Array<const MKeyValue *> &outAll) const;
380 void GetAllCCDManifoldsSorted(Array<const MKeyValue *> &outAll) const;
381 void ContactPointRemovedCallbacks(ContactListener *inListener);
382
383#ifdef JPH_ENABLE_ASSERTS
385 uint GetNumManifolds() const { return mCachedManifolds.GetNumKeyValues(); }
386
388 uint GetNumBodyPairs() const { return mCachedBodyPairs.GetNumKeyValues(); }
389
391 void Finalize();
392#endif
393
395 void SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const;
396 bool RestoreState(const ManifoldCache &inReadCache, StateRecorder &inStream);
397
398 private:
400 static constexpr uint32 cAllocatorBlockSize = 4096;
401
403 LFHMAllocator mAllocator;
404
406 ManifoldMap mCachedManifolds { mAllocator };
407
409 BodyPairMap mCachedBodyPairs { mAllocator };
410
411#ifdef JPH_ENABLE_ASSERTS
412 bool mIsFinalized = false;
413#endif
414 };
415
416 ManifoldCache mCache[2];
417 int mCacheWriteIdx = 0;
418
420 class WorldContactPoint
421 {
422 public:
424 void CalculateNonPenetrationConstraintProperties(const Body &inBody1, float inInvMass1, float inInvInertiaScale1, const Body &inBody2, float inInvMass2, float inInvInertiaScale2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal);
425
426 template <EMotionType Type1, EMotionType Type2>
427 JPH_INLINE void TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(float inDeltaTime, 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);
428
430 AxisConstraintPart mNonPenetrationConstraint;
431 AxisConstraintPart mFrictionConstraint1;
432 AxisConstraintPart mFrictionConstraint2;
433
435 CachedContactPoint * mContactPoint;
436 };
437
438 using WorldContactPoints = StaticArray<WorldContactPoint, MaxContactPoints>;
439
441 class ContactConstraint
442 {
443 public:
444 #ifdef JPH_DEBUG_RENDERER
446 void Draw(DebugRenderer *inRenderer, ColorArg inManifoldColor) const;
447 #endif // JPH_DEBUG_RENDERER
448
450 JPH_INLINE Vec3 GetWorldSpaceNormal() const
451 {
452 return Vec3::sLoadFloat3Unsafe(mWorldSpaceNormal);
453 }
454
456 JPH_INLINE void GetTangents(Vec3 &outTangent1, Vec3 &outTangent2) const
457 {
458 Vec3 ws_normal = GetWorldSpaceNormal();
459 outTangent1 = ws_normal.GetNormalizedPerpendicular();
460 outTangent2 = ws_normal.Cross(outTangent1);
461 }
462
463 Body * mBody1;
464 Body * mBody2;
465 uint64 mSortKey;
466 Float3 mWorldSpaceNormal;
467 float mCombinedFriction;
468 float mInvMass1;
469 float mInvInertiaScale1;
470 float mInvMass2;
471 float mInvInertiaScale2;
472 WorldContactPoints mContactPoints;
473 };
474
476 template <EMotionType Type1, EMotionType Type2>
477 JPH_INLINE void TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2);
478
480 inline void CalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2);
481
483 template <EMotionType Type1, EMotionType Type2>
484 bool TemplatedAddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPairHandle, Body &inBody1, Body &inBody2, const ContactManifold &inManifold);
485
487 template <EMotionType Type1, EMotionType Type2>
488 JPH_INLINE static void sWarmStartConstraint(ContactConstraint &ioConstraint, MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2, float inWarmStartImpulseRatio);
489
491 template <EMotionType Type1, EMotionType Type2>
492 JPH_INLINE static bool sSolveVelocityConstraint(ContactConstraint &ioConstraint, MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2);
493
495 const PhysicsSettings & mPhysicsSettings;
496
498 ContactListener * mContactListener = nullptr;
499
501 CombineFunction mCombineFriction = [](const Body &inBody1, const SubShapeID &, const Body &inBody2, const SubShapeID &) { return sqrt(inBody1.GetFriction() * inBody2.GetFriction()); };
502 CombineFunction mCombineRestitution = [](const Body &inBody1, const SubShapeID &, const Body &inBody2, const SubShapeID &) { return max(inBody1.GetRestitution(), inBody2.GetRestitution()); };
503
505 ContactConstraint * mConstraints = nullptr;
506 uint32 mMaxConstraints = 0;
507 atomic<uint32> mNumConstraints { 0 };
508
510 PhysicsUpdateContext * mUpdateContext;
511};
512
ValidateResult
Definition: ContactListener.h:57
#define JPH_EXPORT
Definition: Core.h:227
#define JPH_SUPPRESS_WARNINGS_STD_BEGIN
Definition: Core.h:372
#define JPH_SUPPRESS_WARNINGS_STD_END
Definition: Core.h:384
std::uint64_t uint64
Definition: Core.h:443
unsigned int uint
Definition: Core.h:439
#define JPH_NAMESPACE_END
Definition: Core.h:367
std::uint32_t uint32
Definition: Core.h:442
#define JPH_NAMESPACE_BEGIN
Definition: Core.h:361
std::uint16_t uint16
Definition: Core.h:441
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:29
std::vector< T, STLAllocator< T > > Array
Definition: STLAllocator.h:81
Definition: AxisConstraintPart.h:43
Definition: Body.h:35
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:80
Definition: ContactConstraintManager.h:28
static bool sDrawContactPoint
Definition: ContactConstraintManager.h:244
CombineFunction GetCombineFriction() const
Definition: ContactConstraintManager.h:53
CombineFunction GetCombineRestitution() const
Definition: ContactConstraintManager.h:58
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:64
uint32 GetMaxConstraints() const
Get the max number of contact constraints that are allowed.
Definition: ContactConstraintManager.h:61
static bool sDrawContactPointReduction
Definition: ContactConstraintManager.h:246
void * BodyPairHandle
Handle used to keep track of the current body pair.
Definition: ContactConstraintManager.h:98
ContactListener * GetContactListener() const
Definition: ContactConstraintManager.h:43
static bool sDrawSupportingFaces
Definition: ContactConstraintManager.h:245
void GetAffectedBodies(uint32 inConstraintIdx, const Body *&outBody1, const Body *&outBody2) const
Get the affected bodies for a given constraint.
Definition: ContactConstraintManager.h:164
uint32 GetNumConstraints() const
Get the number of contact constraints that were found.
Definition: ContactConstraintManager.h:158
void SetCombineFriction(CombineFunction inCombineFriction)
Definition: ContactConstraintManager.h:52
void SetContactListener(ContactListener *inListener)
Listener that is notified whenever a contact point between two bodies is added/updated/removed.
Definition: ContactConstraintManager.h:42
float(*)(const Body &inBody1, const SubShapeID &inSubShapeID1, const Body &inBody2, const SubShapeID &inSubShapeID2) CombineFunction
Definition: ContactConstraintManager.h:48
void SetCombineRestitution(CombineFunction inCombineRestitution)
Definition: ContactConstraintManager.h:57
ContactAllocator GetContactAllocator()
Get a new allocator context for storing contacts. Note that you should call this once and then add mu...
Definition: ContactConstraintManager.h:90
static bool sDrawContactManifolds
Definition: ContactConstraintManager.h:247
Definition: ContactListener.h:68
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
Information used during the Update call.
Definition: PhysicsUpdateContext.h:24
User callbacks that allow determining which parts of the simulation should be saved by a StateRecorde...
Definition: StateRecorder.h:29
Definition: StateRecorder.h:48
Simple variable length array backed by a fixed size buffer.
Definition: StaticArray.h:12
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:16
JPH_INLINE Vec3 Cross(Vec3Arg inV2) const
Cross product.
Definition: Vec3.inl:582
JPH_INLINE Vec3 GetNormalizedPerpendicular() const
Get normalized vector that is perpendicular to this vector.
Definition: Vec3.inl:812
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:134
Templatized vector class.
Definition: Vector.h:12
Structure that holds a body pair.
Definition: BodyPair.h:14
Definition: PhysicsSettings.h:28