ContactConstraintManager.h

Go to the documentation of this file.

// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
 
#pragma once
 
#include <Jolt/Core/StaticArray.h>
#include <Jolt/Core/LockFreeHashMap.h>
#include <Jolt/Physics/EPhysicsUpdateError.h>
#include <Jolt/Physics/Body/BodyPair.h>
#include <Jolt/Physics/Collision/Shape/SubShapeIDPair.h>
#include <Jolt/Physics/Collision/ManifoldBetweenTwoFaces.h>
#include <Jolt/Physics/Constraints/ConstraintPart/ContactConstraintPart.h>
#include <Jolt/Physics/Constraints/ConstraintPart/AngularFrictionConstraintPart.h>
#include <Jolt/Physics/StateRecorder.h>
#include <Jolt/Core/HashCombine.h>
#include <Jolt/Core/NonCopyable.h>
#include <Jolt/Math/Vector.h>
 
JPH_SUPPRESS_WARNINGS_STD_BEGIN
#include <atomic>
JPH_SUPPRESS_WARNINGS_STD_END
 
JPH_NAMESPACE_BEGIN
 
struct PhysicsSettings;
class PhysicsUpdateContext;

class JPH_EXPORT ContactConstraintManager : public NonCopyable
{
public:
    JPH_OVERRIDE_NEW_DELETE

    explicit                    ContactConstraintManager(const PhysicsSettings &inPhysicsSettings);
                                ~ContactConstraintManager();

    void                        Init(uint inMaxBodyPairs, uint inMaxContactConstraints);

    void                        SetContactListener(ContactListener *inListener)                     { mContactListener = inListener; }
    ContactListener *           GetContactListener() const                                          { return mContactListener; }

    using CombineFunction = float (*)(const Body &inBody1, const SubShapeID &inSubShapeID1, const Body &inBody2, const SubShapeID &inSubShapeID2);

    void                        SetCombineFriction(CombineFunction inCombineFriction)               { mCombineFriction = inCombineFriction; }
    CombineFunction             GetCombineFriction() const                                          { return mCombineFriction; }

    void                        SetCombineRestitution(CombineFunction inCombineRestitution)         { mCombineRestitution = inCombineRestitution; }
    CombineFunction             GetCombineRestitution() const                                       { return mCombineRestitution; }

    uint32                      GetMaxConstraints() const                                           { return mMaxConstraints; }

    inline ValidateResult       ValidateContactPoint(const Body &inBody1, const Body &inBody2, RVec3Arg inBaseOffset, const CollideShapeResult &inCollisionResult) const
    {
        if (mContactListener == nullptr)
            return ValidateResult::AcceptAllContactsForThisBodyPair;
 
        return mContactListener->OnContactValidate(inBody1, inBody2, inBaseOffset, inCollisionResult);
    }

    void                        PrepareConstraintBuffer(PhysicsUpdateContext *inContext);

    static const int            MaxContactPoints = 4;

    class ContactAllocator : public LFHMAllocatorContext
    {
    public:
        using LFHMAllocatorContext::LFHMAllocatorContext;
 
        uint                    mNumBodyPairs = 0;                                                  
        uint                    mNumManifolds = 0;                                                  
        EPhysicsUpdateError     mErrors = EPhysicsUpdateError::None;                                
    };

    ContactAllocator            GetContactAllocator()                                               { return mCache[mCacheWriteIdx].GetContactAllocator(); }

    void                        GetContactsFromCache(ContactAllocator &ioContactAllocator, Body &inBody1, Body &inBody2, bool &outPairHandled);

    using BodyPairHandle = void *;

    BodyPairHandle              AddBodyPair(ContactAllocator &ioContactAllocator, const Body &inBody1, const Body &inBody2);

    void                        AddContactConstraint(ContactAllocator &ioContactAllocator, bool &ioActivateAndLinkBodies, BodyPairHandle inBodyPair, Body &inBody1, Body &inBody2, const ContactManifold &inManifold);

    void                        FinalizeContactCacheAndCallContactPointRemovedCallbacks(uint inExpectedNumBodyPairs, uint inExpectedNumManifolds);

    bool                        WereBodiesInContact(const BodyID &inBody1ID, const BodyID &inBody2ID) const;

    uint32                      GetNumConstraints() const                                           { return min<uint32>(uint32(mNumConstraintsAndNextConstraintOffset.load(memory_order_relaxed)), mMaxConstraints); }

    void                        ConstraintIdxToConstraintOffset(uint32 *ioConstraintIdxBegin, const uint32 *inConstraintIdxEnd) const;

    void                        SortContacts(uint32 *ioConstraintOffsetBegin, uint32 *inConstraintOffsetEnd) const;

    inline void                 GetAffectedBodies(uint32 inConstraintOffset, const Body *&outBody1, const Body *&outBody2) const
    {
        const ContactConstraintBase &constraint = *reinterpret_cast<const ContactConstraintBase *>(mConstraints + inConstraintOffset);
        outBody1 = constraint.mBody1;
        outBody2 = constraint.mBody2;
    }

    template <class MotionPropertiesCallback>
    void                        WarmStartVelocityConstraints(const uint32 *inConstraintOffsetBegin, const uint32 *inConstraintOffsetEnd, float inWarmStartImpulseRatio, MotionPropertiesCallback &ioCallback);

    bool                        SolveVelocityConstraints(const uint32 *inConstraintOffsetBegin, const uint32 *inConstraintOffsetEnd);

    void                        StoreAppliedImpulses(const uint32 *inConstraintOffsetBegin, const uint32 *inConstraintOffsetEnd) const;

    bool                        SolvePositionConstraints(const uint32 *inConstraintOffsetBegin, const uint32 *inConstraintOffsetEnd);

    void                        RecycleConstraintBuffer();

    void                        FinishConstraintBuffer();

    void                        OnCCDContactAdded(ContactAllocator &ioContactAllocator, const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &outSettings);
 
#ifdef JPH_DEBUG_RENDERER
    // Drawing properties
    static bool                 sDrawContactPoint;
    static bool                 sDrawSupportingFaces;
    static bool                 sDrawContactPointReduction;
    static bool                 sDrawContactManifolds;
#endif // JPH_DEBUG_RENDERER

    void                        SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const;

    bool                        RestoreState(StateRecorder &inStream, const StateRecorderFilter *inFilter);
 
private:
    class CachedContactPoint
    {
    public:
        void                    SaveState(StateRecorder &inStream) const;
        void                    RestoreState(StateRecorder &inStream);

        Float3                  mPosition1;
        Float3                  mPosition2;

        float                   mNonPenetrationLambda;
    };
 
    static_assert(sizeof(CachedContactPoint) == 28, "Unexpected size");
    static_assert(alignof(CachedContactPoint) == 4, "Assuming 4 byte aligned");

    class CachedManifold
    {
    public:
        static constexpr int    sGetRequiredExtraSize(int inNumContactPoints)                       { return max(0, inNumContactPoints - 1) * sizeof(CachedContactPoint); }

        static constexpr int    sGetRequiredTotalSize(int inNumContactPoints)                       { return sizeof(CachedManifold) + sGetRequiredExtraSize(inNumContactPoints); }

        void                    SaveState(StateRecorder &inStream) const;
        void                    RestoreState(StateRecorder &inStream);

        uint32                  mNextWithSameBodyPair;

        Float3                  mContactNormal;

        Vector<2>               mFrictionLambda;
        float                   mAngularFrictionLambda;

        enum class EFlags : uint16
        {
            ContactPersisted    = 1,                                                                
            CCDContact          = 2                                                                 
        };

        mutable atomic<uint16>  mFlags { 0 };

        uint16                  mNumContactPoints;

        CachedContactPoint      mContactPoints[1];
    };
 
    static_assert(sizeof(CachedManifold) == 60, "This structure is expect to not contain any waste due to alignment");
    static_assert(alignof(CachedManifold) == 4, "Assuming 4 byte aligned");

    using ManifoldMap = LockFreeHashMap<SubShapeIDPair, CachedManifold>;
    using MKeyValue = ManifoldMap::KeyValue;
    using MKVAndCreated = std::pair<MKeyValue *, bool>;

    class CachedBodyPair
    {
    public:
        void                    SaveState(StateRecorder &inStream) const;
        void                    RestoreState(StateRecorder &inStream);

        Float3                  mDeltaPosition;

        Float3                  mDeltaRotation;

        uint32                  mFirstCachedManifold;
    };
 
    static_assert(sizeof(CachedBodyPair) == 28, "Unexpected size");
    static_assert(alignof(CachedBodyPair) == 4, "Assuming 4 byte aligned");

    using BodyPairMap = LockFreeHashMap<BodyPair, CachedBodyPair>;
    using BPKeyValue = BodyPairMap::KeyValue;

    class ManifoldCache
    {
    public:
        void                    Init(uint inMaxBodyPairs, uint inMaxContactConstraints, uint inCachedManifoldsSize);

        void                    Clear();

        void                    Prepare(uint inExpectedNumBodyPairs, uint inExpectedNumManifolds);

        ContactAllocator        GetContactAllocator()                       { return ContactAllocator(mAllocator, cAllocatorBlockSize); }

        const MKeyValue *       Find(const SubShapeIDPair &inKey, uint64 inKeyHash) const;
        MKeyValue *             Create(ContactAllocator &ioContactAllocator, const SubShapeIDPair &inKey, uint64 inKeyHash, int inNumContactPoints);
        MKVAndCreated           FindOrCreate(ContactAllocator &ioContactAllocator, const SubShapeIDPair &inKey, uint64 inKeyHash, int inNumContactPoints);
        uint32                  ToHandle(const MKeyValue *inKeyValue) const;
        const MKeyValue *       FromHandle(uint32 inHandle) const;
        MKeyValue *             FromHandle(uint32 inHandle);

        const BPKeyValue *      Find(const BodyPair &inKey, uint64 inKeyHash) const;
        BPKeyValue *            Create(ContactAllocator &ioContactAllocator, const BodyPair &inKey, uint64 inKeyHash);
        void                    GetAllBodyPairsSorted(Array<const BPKeyValue *> &outAll) const;
        void                    GetAllManifoldsSorted(const CachedBodyPair &inBodyPair, Array<const MKeyValue *> &outAll) const;
        void                    GetAllCCDManifoldsSorted(Array<const MKeyValue *> &outAll) const;
        void                    ContactPointRemovedCallbacks(ContactListener *inListener);
 
#ifdef JPH_ENABLE_ASSERTS
        uint                    GetNumManifolds() const                     { return mCachedManifolds.GetNumKeyValues(); }

        uint                    GetNumBodyPairs() const                     { return mCachedBodyPairs.GetNumKeyValues(); }

        void                    Finalize();
#endif

        void                    SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const;
        bool                    RestoreState(const ManifoldCache &inReadCache, StateRecorder &inStream, const StateRecorderFilter *inFilter);
 
    private:
        static constexpr uint32 cAllocatorBlockSize = 4096;

        LFHMAllocator           mAllocator;

        ManifoldMap             mCachedManifolds { mAllocator };

        BodyPairMap             mCachedBodyPairs { mAllocator };
 
#ifdef JPH_ENABLE_ASSERTS
        bool                    mIsFinalized = false;                       
#endif
    };
 
    ManifoldCache               mCache[2];                                  
    int                         mCacheWriteIdx = 0;                         
    ManifoldCache *             mReadCache = nullptr;                       
    ManifoldCache *             mWriteCache = nullptr;                      

    template <EMotionType Type1, EMotionType Type2>
    class WorldContactPoint
    {
    public:
        using ConstraintPart = ContactConstraintPart<Type1, Type2>;

        JPH_INLINE void         CalculateNonPenetrationConstraintProperties(float inDeltaTime, Vec3Arg inGravity, const Body &inBody1, const Body &inBody2, float inInvM1, float inInvM2, Mat44Arg inInvI1, Mat44Arg inInvI2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal, const ContactSettings &inSettings, float inMinVelocityForRestitution);

        ConstraintPart          mNonPenetrationConstraint;
        // Note that this needs to be followed by data of size float, see comment at ContactConstraintPart

        float                   mDistanceToFrictionCenter;
    };
 
    class ContactConstraintBase
    {
    public:
        JPH_INLINE Vec3         GetWorldSpaceNormal() const
        {
            return Vec3::sLoadFloat3Unsafe(mWorldSpaceNormal);
        }

        JPH_INLINE void         GetTangents(Vec3 &outTangent1, Vec3 &outTangent2) const
        {
            Vec3 ws_normal = GetWorldSpaceNormal();
            outTangent1 = ws_normal.GetNormalizedPerpendicular();
            outTangent2 = ws_normal.Cross(outTangent1);
        }
 
        Body *                  mBody1;
        Body *                  mBody2;
        uint64                  mSortKey;
        Float3                  mWorldSpaceNormal;
        float                   mCombinedFriction;
        float                   mInvMass1;
        float                   mInvInertiaScale1;
        float                   mInvMass2;
        float                   mInvInertiaScale2;
        uint32                  mCachedManifoldHandle;
        uint32                  mNumContactPoints;
    };

    template <EMotionType Type1, EMotionType Type2>
    class ContactConstraint : public ContactConstraintBase
    {
    public:
        JPH_INLINE void         CalculateFrictionConstraintProperties(const Body &inBody1, const Body &inBody2, float inInvM1, float inInvM2, Mat44Arg inInvI1, Mat44Arg inInvI2, const RVec3 *inWorldSpaceContacts, Vec3Arg inWorldSpaceNormal, Vec3Arg inWorldSpaceTangent1, Vec3Arg inWorldSpaceTangent2, const ContactSettings &inSettings);
 
    #ifdef JPH_DEBUG_RENDERER
        void                    Draw(DebugRenderer *inRenderer, const ManifoldCache &inManifoldCache, ColorArg inManifoldColor) const;
    #endif // JPH_DEBUG_RENDERER
 
        ContactConstraintPart<Type1, Type2> mFrictionConstraint1;
        ContactConstraintPart<Type1, Type2> mFrictionConstraint2;
        AngularFrictionConstraintPart<Type1, Type2> mAngularFrictionConstraint;
 
        WorldContactPoint<Type1, Type2> mContactPoints[1];
    };
 
public:
    static constexpr uint32     cMaxConstraintSize = sizeof(ContactConstraint<EMotionType::Dynamic, EMotionType::Dynamic>) + (MaxContactPoints - 1) * sizeof(WorldContactPoint<EMotionType::Dynamic, EMotionType::Dynamic>);

    static constexpr uint       cMaxContactConstraintsLimit = ~uint(0) / cMaxConstraintSize;

    static constexpr uint       cMaxBodyPairsLimit = ~uint(0) / sizeof(BodyPairMap::KeyValue);
 
private:
    template <EMotionType Type1, EMotionType Type2>
    JPH_INLINE ContactConstraint<Type1, Type2> *CreateConstraint(bool &ioActivateAndLinkBodies, Body &inBody1, Body &inBody2, uint64 inSortKey, uint32 inCachedManifoldHandle, Vec3Arg inWorldSpaceNormal, const ContactSettings &inSettings, uint32 inNumContactPoints);

    template <EMotionType Type1, EMotionType Type2>
    void                        TemplatedGetContactsFromCache(ContactAllocator &ioContactAllocator, Body &inBody1, Body &inBody2, const CachedBodyPair &inCachedBodyPair, CachedBodyPair &outCachedBodyPair);

    template <EMotionType Type1, EMotionType Type2>
    void                        TemplatedAddContactConstraint(ContactAllocator &ioContactAllocator, bool &ioActivateAndLinkBodies, BodyPairHandle inBodyPairHandle, Body &inBody1, Body &inBody2, const ContactManifold &inManifold);

    template <EMotionType Type1, EMotionType Type2>
    static JPH_INLINE void      sGetVelocities(const MotionProperties *inMotionProperties1, const MotionProperties *inMotionProperties2, Vec3 &outLinearVelocity1, Vec3 &outAngularVelocity1, Vec3 &outLinearVelocity2, Vec3 &outAngularVelocity2);

    template <EMotionType Type1, EMotionType Type2>
    static JPH_INLINE void      sSetVelocities(MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2, Vec3Arg inLinearVelocity1, Vec3Arg inAngularVelocity1, Vec3Arg inLinearVelocity2, Vec3Arg inAngularVelocity2);

    template <EMotionType Type1, EMotionType Type2>
    static void                 sWarmStartConstraint(ContactConstraintBase &ioConstraint, MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2, float inWarmStartImpulseRatio);

    template <EMotionType Type1, EMotionType Type2>
    static bool                 sSolveVelocityConstraint(ContactConstraintBase &ioConstraint, MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2);

    template <EMotionType Type1, EMotionType Type2>
    static void                 sStoreAppliedImpulses(ContactConstraintBase &ioConstraint, ManifoldCache &inManifoldCache);

    template <EMotionType Type1, EMotionType Type2>
    static bool                 sSolvePositionConstraint(ContactConstraintBase &ioConstraint, Body &ioBody1, Body &ioBody2, const PhysicsSettings &inSettings, const ManifoldCache &inManifoldCache);

    const PhysicsSettings &     mPhysicsSettings;

    ContactListener *           mContactListener = nullptr;

    CombineFunction             mCombineFriction = [](const Body &inBody1, const SubShapeID &, const Body &inBody2, const SubShapeID &) { return Sqrt(inBody1.GetFriction() * inBody2.GetFriction()); };
    CombineFunction             mCombineRestitution = [](const Body &inBody1, const SubShapeID &, const Body &inBody2, const SubShapeID &) { return max(inBody1.GetRestitution(), inBody2.GetRestitution()); };

    uint8 *                     mConstraints = nullptr; 
    uint32 *                    mConstraintIdxToOffset = nullptr; 
    uint32                      mMaxConstraints = 0;
    atomic<uint64>              mNumConstraintsAndNextConstraintOffset { 0 }; // Lower 32 bits: number of constraints, upper 32 bits next constraint offset

    PhysicsUpdateContext *      mUpdateContext;
};
 
JPH_NAMESPACE_END