From a37d60e13cc5be5bc3f37285f117c10b7b8a83b3 Mon Sep 17 00:00:00 2001 From: Jorrit Rouwe Date: Fri, 14 Apr 2023 15:32:06 +0200 Subject: [PATCH] Implemented NotifyShapeChange to notify constraint of COM changes (#506) Added example test to show how it is used. --- .../Collision/Shape/MutableCompoundShape.h | 1 + Jolt/Physics/Constraints/ConeConstraint.cpp | 8 ++ Jolt/Physics/Constraints/ConeConstraint.h | 1 + Jolt/Physics/Constraints/Constraint.h | 7 + .../Constraints/DistanceConstraint.cpp | 8 ++ Jolt/Physics/Constraints/DistanceConstraint.h | 1 + Jolt/Physics/Constraints/FixedConstraint.cpp | 8 ++ Jolt/Physics/Constraints/FixedConstraint.h | 1 + Jolt/Physics/Constraints/GearConstraint.h | 1 + Jolt/Physics/Constraints/HingeConstraint.cpp | 8 ++ Jolt/Physics/Constraints/HingeConstraint.h | 1 + Jolt/Physics/Constraints/PathConstraint.cpp | 8 ++ Jolt/Physics/Constraints/PathConstraint.h | 1 + Jolt/Physics/Constraints/PointConstraint.cpp | 8 ++ Jolt/Physics/Constraints/PointConstraint.h | 1 + Jolt/Physics/Constraints/PulleyConstraint.cpp | 8 ++ Jolt/Physics/Constraints/PulleyConstraint.h | 1 + .../Constraints/RackAndPinionConstraint.h | 1 + Jolt/Physics/Constraints/SixDOFConstraint.cpp | 8 ++ Jolt/Physics/Constraints/SixDOFConstraint.h | 1 + Jolt/Physics/Constraints/SliderConstraint.cpp | 8 ++ Jolt/Physics/Constraints/SliderConstraint.h | 1 + .../Constraints/SwingTwistConstraint.cpp | 8 ++ .../Constraints/SwingTwistConstraint.h | 1 + Jolt/Physics/Vehicle/VehicleConstraint.h | 1 + Samples/Samples.cmake | 2 + Samples/SamplesApp.cpp | 2 + .../Constraints/ConstraintVsCOMChangeTest.cpp | 130 ++++++++++++++++++ .../Constraints/ConstraintVsCOMChangeTest.h | 32 +++++ 29 files changed, 267 insertions(+) create mode 100644 Samples/Tests/Constraints/ConstraintVsCOMChangeTest.cpp create mode 100644 Samples/Tests/Constraints/ConstraintVsCOMChangeTest.h diff --git a/Jolt/Physics/Collision/Shape/MutableCompoundShape.h b/Jolt/Physics/Collision/Shape/MutableCompoundShape.h index 8d1c58f83..e7908b4b3 100644 --- a/Jolt/Physics/Collision/Shape/MutableCompoundShape.h +++ b/Jolt/Physics/Collision/Shape/MutableCompoundShape.h @@ -85,6 +85,7 @@ class MutableCompoundShape final : public CompoundShape /// Recalculate the center of mass and shift all objects so they're centered around it /// (this needs to be done of dynamic bodies and if the center of mass changes significantly due to adding / removing / repositioning sub shapes or else the simulation will look unnatural) + /// Note that after adjusting the center of mass of an object you need to call BodyInterface::NotifyShapeChanged and Constraint::NotifyShapeChanged on the relevant bodies / constraints. void AdjustCenterOfMass(); ///@} diff --git a/Jolt/Physics/Constraints/ConeConstraint.cpp b/Jolt/Physics/Constraints/ConeConstraint.cpp index c50347d21..93b28741d 100644 --- a/Jolt/Physics/Constraints/ConeConstraint.cpp +++ b/Jolt/Physics/Constraints/ConeConstraint.cpp @@ -89,6 +89,14 @@ ConeConstraint::ConeConstraint(Body &inBody1, Body &inBody2, const ConeConstrain } } +void ConeConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + void ConeConstraint::CalculateRotationConstraintProperties(float inDeltaTime, Mat44Arg inRotation1, Mat44Arg inRotation2) { // Rotation is along the cross product of both twist axis diff --git a/Jolt/Physics/Constraints/ConeConstraint.h b/Jolt/Physics/Constraints/ConeConstraint.h index 43723e20b..08ab190e2 100644 --- a/Jolt/Physics/Constraints/ConeConstraint.h +++ b/Jolt/Physics/Constraints/ConeConstraint.h @@ -76,6 +76,7 @@ class ConeConstraint final : public TwoBodyConstraint // Generic interface of a constraint virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Cone; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; virtual void SetupVelocityConstraint(float inDeltaTime) override; virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; virtual bool SolveVelocityConstraint(float inDeltaTime) override; diff --git a/Jolt/Physics/Constraints/Constraint.h b/Jolt/Physics/Constraints/Constraint.h index 67f93fee8..1516621ec 100644 --- a/Jolt/Physics/Constraints/Constraint.h +++ b/Jolt/Physics/Constraints/Constraint.h @@ -11,6 +11,7 @@ JPH_NAMESPACE_BEGIN +class BodyID; class IslandBuilder; class LargeIslandSplitter; class BodyManager; @@ -141,6 +142,12 @@ class Constraint : public RefTarget, public NonCopyable uint64 GetUserData() const { return mUserData; } void SetUserData(uint64 inUserData) { mUserData = inUserData; } + /// Notify the constraint that the shape of a body has changed and that its center of mass has moved by inDeltaCOM. + /// Bodies don't know which constraints are connected to them so the user is responsible for notifying the relevant constraints when a body changes. + /// @param inBodyID ID of the body that has changed + /// @param inDeltaCOM The delta of the center of mass of the body (shape->GetCenterOfMass() - shape_before_change->GetCenterOfMass()) + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) = 0; + ///@name Solver interface ///@{ virtual bool IsActive() const { return mEnabled; } diff --git a/Jolt/Physics/Constraints/DistanceConstraint.cpp b/Jolt/Physics/Constraints/DistanceConstraint.cpp index 29faf370d..6cfcbf1cb 100644 --- a/Jolt/Physics/Constraints/DistanceConstraint.cpp +++ b/Jolt/Physics/Constraints/DistanceConstraint.cpp @@ -95,6 +95,14 @@ DistanceConstraint::DistanceConstraint(Body &inBody1, Body &inBody2, const Dista SetDamping(inSettings.mDamping); } +void DistanceConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + void DistanceConstraint::CalculateConstraintProperties(float inDeltaTime) { // Update world space positions (the bodies may have moved) diff --git a/Jolt/Physics/Constraints/DistanceConstraint.h b/Jolt/Physics/Constraints/DistanceConstraint.h index cdba99dd5..1c34eac52 100644 --- a/Jolt/Physics/Constraints/DistanceConstraint.h +++ b/Jolt/Physics/Constraints/DistanceConstraint.h @@ -59,6 +59,7 @@ class DistanceConstraint final : public TwoBodyConstraint // Generic interface of a constraint virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Distance; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; virtual void SetupVelocityConstraint(float inDeltaTime) override; virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; virtual bool SolveVelocityConstraint(float inDeltaTime) override; diff --git a/Jolt/Physics/Constraints/FixedConstraint.cpp b/Jolt/Physics/Constraints/FixedConstraint.cpp index 74c05ad51..c47cd3d38 100644 --- a/Jolt/Physics/Constraints/FixedConstraint.cpp +++ b/Jolt/Physics/Constraints/FixedConstraint.cpp @@ -107,6 +107,14 @@ FixedConstraint::FixedConstraint(Body &inBody1, Body &inBody2, const FixedConstr } } +void FixedConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + void FixedConstraint::SetupVelocityConstraint(float inDeltaTime) { // Calculate constraint values that don't change when the bodies don't change position diff --git a/Jolt/Physics/Constraints/FixedConstraint.h b/Jolt/Physics/Constraints/FixedConstraint.h index b6bca8d5b..b6b8f878c 100644 --- a/Jolt/Physics/Constraints/FixedConstraint.h +++ b/Jolt/Physics/Constraints/FixedConstraint.h @@ -55,6 +55,7 @@ class FixedConstraint final : public TwoBodyConstraint // Generic interface of a constraint virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Fixed; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; virtual void SetupVelocityConstraint(float inDeltaTime) override; virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; virtual bool SolveVelocityConstraint(float inDeltaTime) override; diff --git a/Jolt/Physics/Constraints/GearConstraint.h b/Jolt/Physics/Constraints/GearConstraint.h index fbcef5db1..722986b62 100644 --- a/Jolt/Physics/Constraints/GearConstraint.h +++ b/Jolt/Physics/Constraints/GearConstraint.h @@ -59,6 +59,7 @@ class GearConstraint final : public TwoBodyConstraint // Generic interface of a constraint virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Gear; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override { /* Do nothing */ } virtual void SetupVelocityConstraint(float inDeltaTime) override; virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; virtual bool SolveVelocityConstraint(float inDeltaTime) override; diff --git a/Jolt/Physics/Constraints/HingeConstraint.cpp b/Jolt/Physics/Constraints/HingeConstraint.cpp index 58155955c..dfa2b6b54 100644 --- a/Jolt/Physics/Constraints/HingeConstraint.cpp +++ b/Jolt/Physics/Constraints/HingeConstraint.cpp @@ -112,6 +112,14 @@ HingeConstraint::HingeConstraint(Body &inBody1, Body &inBody2, const HingeConstr } } +void HingeConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + float HingeConstraint::GetCurrentAngle() const { // See: CalculateA1AndTheta diff --git a/Jolt/Physics/Constraints/HingeConstraint.h b/Jolt/Physics/Constraints/HingeConstraint.h index 4b9f367cd..6208fe52a 100644 --- a/Jolt/Physics/Constraints/HingeConstraint.h +++ b/Jolt/Physics/Constraints/HingeConstraint.h @@ -65,6 +65,7 @@ class HingeConstraint final : public TwoBodyConstraint // Generic interface of a constraint virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Hinge; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; virtual void SetupVelocityConstraint(float inDeltaTime) override; virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; virtual bool SolveVelocityConstraint(float inDeltaTime) override; diff --git a/Jolt/Physics/Constraints/PathConstraint.cpp b/Jolt/Physics/Constraints/PathConstraint.cpp index 3d8044aff..e51524380 100644 --- a/Jolt/Physics/Constraints/PathConstraint.cpp +++ b/Jolt/Physics/Constraints/PathConstraint.cpp @@ -74,6 +74,14 @@ PathConstraint::PathConstraint(Body &inBody1, Body &inBody2, const PathConstrain SetPath(inSettings.mPath, inSettings.mPathFraction); } +void PathConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mPathToBody1.SetTranslation(mPathToBody1.GetTranslation() - inDeltaCOM); + else if (mBody2->GetID() == inBodyID) + mPathToBody2.SetTranslation(mPathToBody2.GetTranslation() - inDeltaCOM); +} + void PathConstraint::SetPath(const PathConstraintPath *inPath, float inPathFraction) { mPath = inPath; diff --git a/Jolt/Physics/Constraints/PathConstraint.h b/Jolt/Physics/Constraints/PathConstraint.h index ca693cd50..35082550c 100644 --- a/Jolt/Physics/Constraints/PathConstraint.h +++ b/Jolt/Physics/Constraints/PathConstraint.h @@ -74,6 +74,7 @@ class PathConstraint final : public TwoBodyConstraint // Generic interface of a constraint virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Path; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; virtual void SetupVelocityConstraint(float inDeltaTime) override; virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; virtual bool SolveVelocityConstraint(float inDeltaTime) override; diff --git a/Jolt/Physics/Constraints/PointConstraint.cpp b/Jolt/Physics/Constraints/PointConstraint.cpp index 8cc274cbb..7a99eae4f 100644 --- a/Jolt/Physics/Constraints/PointConstraint.cpp +++ b/Jolt/Physics/Constraints/PointConstraint.cpp @@ -63,6 +63,14 @@ PointConstraint::PointConstraint(Body &inBody1, Body &inBody2, const PointConstr } } +void PointConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + void PointConstraint::SetPoint1(EConstraintSpace inSpace, RVec3Arg inPoint1) { if (inSpace == EConstraintSpace::WorldSpace) diff --git a/Jolt/Physics/Constraints/PointConstraint.h b/Jolt/Physics/Constraints/PointConstraint.h index a6f06e49c..8b70c375e 100644 --- a/Jolt/Physics/Constraints/PointConstraint.h +++ b/Jolt/Physics/Constraints/PointConstraint.h @@ -47,6 +47,7 @@ class PointConstraint final : public TwoBodyConstraint // Generic interface of a constraint virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Point; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; virtual void SetupVelocityConstraint(float inDeltaTime) override; virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; virtual bool SolveVelocityConstraint(float inDeltaTime) override; diff --git a/Jolt/Physics/Constraints/PulleyConstraint.cpp b/Jolt/Physics/Constraints/PulleyConstraint.cpp index 806770fae..25ddde923 100644 --- a/Jolt/Physics/Constraints/PulleyConstraint.cpp +++ b/Jolt/Physics/Constraints/PulleyConstraint.cpp @@ -100,6 +100,14 @@ PulleyConstraint::PulleyConstraint(Body &inBody1, Body &inBody2, const PulleyCon mWorldSpaceNormal1 = mWorldSpaceNormal2 = -Vec3::sAxisY(); } +void PulleyConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + float PulleyConstraint::CalculatePositionsNormalsAndLength() { // Update world space positions (the bodies may have moved) diff --git a/Jolt/Physics/Constraints/PulleyConstraint.h b/Jolt/Physics/Constraints/PulleyConstraint.h index 74e281f44..3f2f1c0c7 100644 --- a/Jolt/Physics/Constraints/PulleyConstraint.h +++ b/Jolt/Physics/Constraints/PulleyConstraint.h @@ -66,6 +66,7 @@ class PulleyConstraint final : public TwoBodyConstraint // Generic interface of a constraint virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Pulley; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; virtual void SetupVelocityConstraint(float inDeltaTime) override; virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; virtual bool SolveVelocityConstraint(float inDeltaTime) override; diff --git a/Jolt/Physics/Constraints/RackAndPinionConstraint.h b/Jolt/Physics/Constraints/RackAndPinionConstraint.h index 0afcb1900..ab731e836 100644 --- a/Jolt/Physics/Constraints/RackAndPinionConstraint.h +++ b/Jolt/Physics/Constraints/RackAndPinionConstraint.h @@ -61,6 +61,7 @@ class RackAndPinionConstraint final : public TwoBodyConstraint // Generic interface of a constraint virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::RackAndPinion; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override { /* Nothing */ } virtual void SetupVelocityConstraint(float inDeltaTime) override; virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; virtual bool SolveVelocityConstraint(float inDeltaTime) override; diff --git a/Jolt/Physics/Constraints/SixDOFConstraint.cpp b/Jolt/Physics/Constraints/SixDOFConstraint.cpp index 9ce98d3cd..0fa62b38c 100644 --- a/Jolt/Physics/Constraints/SixDOFConstraint.cpp +++ b/Jolt/Physics/Constraints/SixDOFConstraint.cpp @@ -155,6 +155,14 @@ SixDOFConstraint::SixDOFConstraint(Body &inBody1, Body &inBody2, const SixDOFCon CacheRotationMotorActive(); } +void SixDOFConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + void SixDOFConstraint::SetTranslationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax) { mLimitMin[EAxis::TranslationX] = inLimitMin.GetX(); diff --git a/Jolt/Physics/Constraints/SixDOFConstraint.h b/Jolt/Physics/Constraints/SixDOFConstraint.h index bd4c5e530..81bce6e5e 100644 --- a/Jolt/Physics/Constraints/SixDOFConstraint.h +++ b/Jolt/Physics/Constraints/SixDOFConstraint.h @@ -101,6 +101,7 @@ class SixDOFConstraint final : public TwoBodyConstraint /// Generic interface of a constraint virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::SixDOF; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; virtual void SetupVelocityConstraint(float inDeltaTime) override; virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; virtual bool SolveVelocityConstraint(float inDeltaTime) override; diff --git a/Jolt/Physics/Constraints/SliderConstraint.cpp b/Jolt/Physics/Constraints/SliderConstraint.cpp index 5ce768820..5c6d68e5e 100644 --- a/Jolt/Physics/Constraints/SliderConstraint.cpp +++ b/Jolt/Physics/Constraints/SliderConstraint.cpp @@ -159,6 +159,14 @@ SliderConstraint::SliderConstraint(Body &inBody1, Body &inBody2, const SliderCon SetDamping(inSettings.mDamping); } +void SliderConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + float SliderConstraint::GetCurrentPosition() const { // See: CalculateR1R2U and CalculateSlidingAxisAndPosition diff --git a/Jolt/Physics/Constraints/SliderConstraint.h b/Jolt/Physics/Constraints/SliderConstraint.h index a1a20f403..ac8554e70 100644 --- a/Jolt/Physics/Constraints/SliderConstraint.h +++ b/Jolt/Physics/Constraints/SliderConstraint.h @@ -76,6 +76,7 @@ class SliderConstraint final : public TwoBodyConstraint // Generic interface of a constraint virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Slider; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; virtual void SetupVelocityConstraint(float inDeltaTime) override; virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; virtual bool SolveVelocityConstraint(float inDeltaTime) override; diff --git a/Jolt/Physics/Constraints/SwingTwistConstraint.cpp b/Jolt/Physics/Constraints/SwingTwistConstraint.cpp index 1874c42bd..04d34b8f0 100644 --- a/Jolt/Physics/Constraints/SwingTwistConstraint.cpp +++ b/Jolt/Physics/Constraints/SwingTwistConstraint.cpp @@ -124,6 +124,14 @@ SwingTwistConstraint::SwingTwistConstraint(Body &inBody1, Body &inBody2, const S UpdateLimits(); } +void SwingTwistConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + Quat SwingTwistConstraint::GetRotationInConstraintSpace() const { // Let b1, b2 be the center of mass transform of body1 and body2 (For body1 this is mBody1->GetCenterOfMassTransform()) diff --git a/Jolt/Physics/Constraints/SwingTwistConstraint.h b/Jolt/Physics/Constraints/SwingTwistConstraint.h index 61178f9a8..3787e9b61 100644 --- a/Jolt/Physics/Constraints/SwingTwistConstraint.h +++ b/Jolt/Physics/Constraints/SwingTwistConstraint.h @@ -74,6 +74,7 @@ class SwingTwistConstraint final : public TwoBodyConstraint ///@name Generic interface of a constraint virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::SwingTwist; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; virtual void SetupVelocityConstraint(float inDeltaTime) override; virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; virtual bool SolveVelocityConstraint(float inDeltaTime) override; diff --git a/Jolt/Physics/Vehicle/VehicleConstraint.h b/Jolt/Physics/Vehicle/VehicleConstraint.h index 8f9955f12..d1068f6df 100644 --- a/Jolt/Physics/Vehicle/VehicleConstraint.h +++ b/Jolt/Physics/Vehicle/VehicleConstraint.h @@ -125,6 +125,7 @@ class VehicleConstraint : public Constraint, public PhysicsStepListener // Generic interface of a constraint virtual bool IsActive() const override { return mIsActive && Constraint::IsActive(); } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override { /* Do nothing */ } virtual void SetupVelocityConstraint(float inDeltaTime) override; virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; virtual bool SolveVelocityConstraint(float inDeltaTime) override; diff --git a/Samples/Samples.cmake b/Samples/Samples.cmake index 630020a18..56ea7e2de 100644 --- a/Samples/Samples.cmake +++ b/Samples/Samples.cmake @@ -25,6 +25,8 @@ set(SAMPLES_SRC_FILES ${SAMPLES_ROOT}/Tests/Constraints/ConeConstraintTest.h ${SAMPLES_ROOT}/Tests/Constraints/ConstraintSingularityTest.cpp ${SAMPLES_ROOT}/Tests/Constraints/ConstraintSingularityTest.h + ${SAMPLES_ROOT}/Tests/Constraints/ConstraintVsCOMChangeTest.cpp + ${SAMPLES_ROOT}/Tests/Constraints/ConstraintVsCOMChangeTest.h ${SAMPLES_ROOT}/Tests/Constraints/DistanceConstraintTest.cpp ${SAMPLES_ROOT}/Tests/Constraints/DistanceConstraintTest.h ${SAMPLES_ROOT}/Tests/Constraints/FixedConstraintTest.cpp diff --git a/Samples/SamplesApp.cpp b/Samples/SamplesApp.cpp index 7ac1b37ee..990185ddf 100644 --- a/Samples/SamplesApp.cpp +++ b/Samples/SamplesApp.cpp @@ -149,6 +149,7 @@ JPH_DECLARE_RTTI_FOR_FACTORY(PathConstraintTest) JPH_DECLARE_RTTI_FOR_FACTORY(RackAndPinionConstraintTest) JPH_DECLARE_RTTI_FOR_FACTORY(GearConstraintTest) JPH_DECLARE_RTTI_FOR_FACTORY(PulleyConstraintTest) +JPH_DECLARE_RTTI_FOR_FACTORY(ConstraintVsCOMChangeTest) static TestNameAndRTTI sConstraintTests[] = { @@ -170,6 +171,7 @@ static TestNameAndRTTI sConstraintTests[] = { "Pulley Constraint", JPH_RTTI(PulleyConstraintTest) }, { "Spring", JPH_RTTI(SpringTest) }, { "Constraint Singularity", JPH_RTTI(ConstraintSingularityTest) }, + { "Constraint vs Center Of Mass Change",JPH_RTTI(ConstraintVsCOMChangeTest) }, }; JPH_DECLARE_RTTI_FOR_FACTORY(BoxShapeTest) diff --git a/Samples/Tests/Constraints/ConstraintVsCOMChangeTest.cpp b/Samples/Tests/Constraints/ConstraintVsCOMChangeTest.cpp new file mode 100644 index 000000000..5acd5137c --- /dev/null +++ b/Samples/Tests/Constraints/ConstraintVsCOMChangeTest.cpp @@ -0,0 +1,130 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include + +JPH_IMPLEMENT_RTTI_VIRTUAL(ConstraintVsCOMChangeTest) +{ + JPH_ADD_BASE_CLASS(ConstraintVsCOMChangeTest, Test) +} + +void ConstraintVsCOMChangeTest::Initialize() +{ + constexpr int cChainLength = 15; + constexpr float cMinAngle = DegreesToRadians(-10.0f); + constexpr float cMaxAngle = DegreesToRadians(20.0f); + + // Floor + CreateFloor(); + + // Create box shape + mBox = new BoxShape(Vec3::sReplicate(0.5f * cBoxSize)); + + // Build a collision group filter that disables collision between adjacent bodies + Ref group_filter = new GroupFilterTable(cChainLength); + for (CollisionGroup::SubGroupID i = 0; i < cChainLength - 1; ++i) + group_filter->DisableCollision(i, i + 1); + + // Create chain of bodies + RVec3 position(0, 25, 0); + for (int i = 0; i < cChainLength; ++i) + { + position += Vec3(cBoxSize, 0, 0); + Quat rotation = Quat::sIdentity(); + + // Create compound shape specific for this body + MutableCompoundShapeSettings compound_shape; + compound_shape.SetEmbedded(); + compound_shape.AddShape(Vec3::sZero(), Quat::sIdentity(), mBox); + + // Create body + Body& segment = *mBodyInterface->CreateBody(BodyCreationSettings(&compound_shape, position, rotation, i == 0 ? EMotionType::Static : EMotionType::Dynamic, i == 0 ? Layers::NON_MOVING : Layers::MOVING)); + segment.SetCollisionGroup(CollisionGroup(group_filter, 0, CollisionGroup::SubGroupID(i))); + mBodyInterface->AddBody(segment.GetID(), EActivation::Activate); + + if (i > 0) + { + // Create hinge + HingeConstraintSettings settings; + settings.mPoint1 = settings.mPoint2 = position + Vec3(-0.5f * cBoxSize, -0.5f * cBoxSize, 0); + settings.mHingeAxis1 = settings.mHingeAxis2 = Vec3::sAxisZ(); + settings.mNormalAxis1 = settings.mNormalAxis2 = Vec3::sAxisX(); + settings.mLimitsMin = cMinAngle; + settings.mLimitsMax = cMaxAngle; + Constraint* constraint = settings.Create(*mBodies.back(), segment); + mPhysicsSystem->AddConstraint(constraint); + + mConstraints.push_back(constraint); + } + + mBodies.push_back(&segment); + } +} + +void ConstraintVsCOMChangeTest::PrePhysicsUpdate(const PreUpdateParams& inParams) +{ + // Increment time + mTime += inParams.mDeltaTime; + + UpdateShapes(); +} + +void ConstraintVsCOMChangeTest::SaveState(StateRecorder& inStream) const +{ + inStream.Write(mTime); +} + +void ConstraintVsCOMChangeTest::RestoreState(StateRecorder& inStream) +{ + inStream.Read(mTime); + + UpdateShapes(); +} + +void ConstraintVsCOMChangeTest::UpdateShapes() +{ + // Check if we need to change the configuration + int num_shapes = int(mTime) & 1? 2 : 1; + if (mNumShapes != num_shapes) + { + mNumShapes = num_shapes; + + // Change the COM of the bodies + for (int i = 1; i < (int)mBodies.size(); i += 2) + { + Body *b = mBodies[i]; + MutableCompoundShape *s = static_cast(const_cast(b->GetShape())); + + // Remember the center of mass before the change + Vec3 prev_com = s->GetCenterOfMass(); + + // First remove all existing shapes + for (int j = s->GetNumSubShapes() - 1; j >= 0; --j) + s->RemoveShape(j); + + // Then create the desired number of shapes + for (int j = 0; j < num_shapes; ++j) + s->AddShape(Vec3(0, 0, (1.0f + cBoxSize) * j), Quat::sIdentity(), mBox); + + // Update the center of mass to account for the new box configuration + s->AdjustCenterOfMass(); + + // Notify the physics system that the shape has changed + mBodyInterface->NotifyShapeChanged(b->GetID(), prev_com, true, EActivation::Activate); + + // Notify the constraints that the shape has changed (this could be done more efficient as we know which constraints are affected) + Vec3 delta_com = s->GetCenterOfMass() - prev_com; + for (Constraint *c : mConstraints) + c->NotifyShapeChanged(b->GetID(), delta_com); + } + } +} diff --git a/Samples/Tests/Constraints/ConstraintVsCOMChangeTest.h b/Samples/Tests/Constraints/ConstraintVsCOMChangeTest.h new file mode 100644 index 000000000..2cd7dafbf --- /dev/null +++ b/Samples/Tests/Constraints/ConstraintVsCOMChangeTest.h @@ -0,0 +1,32 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +// This test demonstrates how to notify a constraint that the center of mass of a body changed (constraints store their attachment points in center of mass space). +class ConstraintVsCOMChangeTest : public Test +{ +public: + JPH_DECLARE_RTTI_VIRTUAL(ConstraintVsCOMChangeTest) + + // See: Test + virtual void Initialize() override; + virtual void PrePhysicsUpdate(const PreUpdateParams& inParams) override; + virtual void SaveState(StateRecorder& inStream) const override; + virtual void RestoreState(StateRecorder& inStream) override; + +private: + void UpdateShapes(); + + RefConst mBox; + Array mBodies; + Array> mConstraints; + + static constexpr float cBoxSize = 2.0f; + + float mTime = 0.0f; + int mNumShapes = -1; +};