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/AxisConstraintPart.h>
#include <Jolt/Physics/Constraints/ConstraintPart/DualAxisConstraintPart.h>
#include <Jolt/Core/HashCombine.h>
#include <Jolt/Core/NonCopyable.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, bool &outConstraintCreated);

    using BodyPairHandle = void *;

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

    bool                        AddContactConstraint(ContactAllocator &ioContactAllocator, 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>(mNumConstraints, mMaxConstraints); }

    void                        SortContacts(uint32 *inConstraintIdxBegin, uint32 *inConstraintIdxEnd) const;

    inline void                 GetAffectedBodies(uint32 inConstraintIdx, const Body *&outBody1, const Body *&outBody2) const
    {
        const ContactConstraint &constraint = mConstraints[inConstraintIdx];
        outBody1 = constraint.mBody1;
        outBody2 = constraint.mBody2;
    }

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

    bool                        SolveVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd);

    void                        StoreAppliedImpulses(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd) const;

    bool                        SolvePositionConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd);

    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;
        Vector<2>               mFrictionLambda;
    };
 
    static_assert(sizeof(CachedContactPoint) == 36, "Unexpected size");
    static_assert(alignof(CachedContactPoint) == 4, "Assuming 4 byte aligned");

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

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

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

        uint32                  mNextWithSameBodyPair;

        Float3                  mContactNormal;

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

        mutable atomic<uint16>  mFlags { 0 };

        uint16                  mNumContactPoints;

        CachedContactPoint      mContactPoints[1];
    };
 
    static_assert(sizeof(CachedManifold) == 56, "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;

        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;                         

    class WorldContactPoint
    {
    public:
        void                    CalculateNonPenetrationConstraintProperties(const Body &inBody1, float inInvMass1, float inInvInertiaScale1, const Body &inBody2, float inInvMass2, float inInvInertiaScale2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal);
 
        template <EMotionType Type1, EMotionType Type2>
        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);

        AxisConstraintPart      mNonPenetrationConstraint;
        AxisConstraintPart      mFrictionConstraint1;
        AxisConstraintPart      mFrictionConstraint2;

        CachedContactPoint *    mContactPoint;
    };
 
    using WorldContactPoints = StaticArray<WorldContactPoint, MaxContactPoints>;

    class ContactConstraint
    {
    public:
    #ifdef JPH_DEBUG_RENDERER
        void                    Draw(DebugRenderer *inRenderer, ColorArg inManifoldColor) const;
    #endif // JPH_DEBUG_RENDERER

        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;
        WorldContactPoints      mContactPoints;
    };
 
public:
    static constexpr uint       cMaxContactConstraintsLimit = ~uint(0) / sizeof(ContactConstraint);

    static constexpr uint       cMaxBodyPairsLimit = ~uint(0) / sizeof(BodyPairMap::KeyValue);
 
private:
    template <EMotionType Type1, EMotionType Type2>
    JPH_INLINE void             TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravityDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2);

    inline void                 CalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravityDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2);

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

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

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

    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()); };

    ContactConstraint *         mConstraints = nullptr;
    uint32                      mMaxConstraints = 0;
    atomic<uint32>              mNumConstraints { 0 };

    PhysicsUpdateContext *      mUpdateContext;
};
 
JPH_NAMESPACE_END