Implemented NotifyShapeChange to notify constraint of COM changes (#506)

Added example test to show how it is used.
jrouwe · Apr 14, 2023 · a37d60e · a37d60e
1 parent 0d39abb
commit a37d60e
Show file tree

Hide file tree

Showing 29 changed files with 267 additions and 0 deletions.
diff --git 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
@@ -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
@@ -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
@@ -11,6 +11,7 @@
 
 JPH_NAMESPACE_BEGIN
 
+class BodyID;
 class IslandBuilder;
 class LargeIslandSplitter;
 class BodyManager;
@@ -141,6 +142,12 @@ class Constraint : public RefTarget<Constraint>, 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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)