Fixed deterministic simulation of motor cycle (#505)

The previous contact was not stored properly leading to an error in the lean angle calculation
jrouwe · Apr 14, 2023 · 0d39abb · 0d39abb
1 parent a468ac0
commit 0d39abb
Show file tree

Hide file tree

Showing 11 changed files with 87 additions and 8 deletions.
diff --git a/Jolt/Physics/PhysicsSystem.cpp b/Jolt/Physics/PhysicsSystem.cpp
@@ -119,6 +119,8 @@ EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionStep
 {	
 	JPH_PROFILE_FUNCTION();
 
+	JPH_DET_LOG("PhysicsSystem::Update: dt: " << inDeltaTime << " steps: " << inCollisionSteps << " substeps: " << inIntegrationSubSteps);
+
 	JPH_ASSERT(inDeltaTime >= 0.0f);
 	JPH_ASSERT(inIntegrationSubSteps <= PhysicsUpdateContext::cMaxSubSteps);
 

diff --git a/Jolt/Physics/Vehicle/MotorcycleController.cpp b/Jolt/Physics/Vehicle/MotorcycleController.cpp
@@ -108,6 +108,8 @@ void MotorcycleController::PreCollide(float inDeltaTime, PhysicsSystem &inPhysic
 	mTargetLean -= mTargetLean * mTargetLean.Dot(forward);
 	mTargetLean = mTargetLean.NormalizedOr(world_up);
 
+	JPH_DET_LOG("WheeledVehicleController::PreCollide: target_lean: " << target_lean << " mTargetLean: " << mTargetLean);
+
 	// Calculate max steering angle based on the max lean angle we're willing to take
 	// See: https://en.wikipedia.org/wiki/Bicycle_and_motorcycle_dynamics#Leaning
 	// LeanAngle = Atan(Velocity^2 / (Gravity * TurnRadius))
@@ -216,6 +218,20 @@ bool MotorcycleController::SolveLongitudinalAndLateralConstraints(float inDeltaT
 	return impulse;
 }
 
+void MotorcycleController::SaveState(StateRecorder& inStream) const
+{
+	WheeledVehicleController::SaveState(inStream);
+
+	inStream.Write(mTargetLean);
+}
+
+void MotorcycleController::RestoreState(StateRecorder& inStream)
+{
+	WheeledVehicleController::RestoreState(inStream);
+
+	inStream.Read(mTargetLean);
+}
+
 #ifdef JPH_DEBUG_RENDERER
 
 void MotorcycleController::Draw(DebugRenderer *inRenderer) const 

diff --git a/Jolt/Physics/Vehicle/MotorcycleController.h b/Jolt/Physics/Vehicle/MotorcycleController.h
@@ -50,6 +50,8 @@ class MotorcycleController : public WheeledVehicleController
 	// See: VehicleController
 	virtual void				PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) override;
 	virtual bool				SolveLongitudinalAndLateralConstraints(float inDeltaTime) override;
+	virtual void				SaveState(StateRecorder& inStream) const override;
+	virtual void				RestoreState(StateRecorder& inStream) override;
 #ifdef JPH_DEBUG_RENDERER
 	virtual void				Draw(DebugRenderer *inRenderer) const override;
 #endif // JPH_DEBUG_RENDERER

diff --git a/Jolt/Physics/Vehicle/VehicleConstraint.cpp b/Jolt/Physics/Vehicle/VehicleConstraint.cpp
@@ -543,6 +543,9 @@ void VehicleConstraint::SaveState(StateRecorder &inStream) const
 	{
 		inStream.Write(w->mAngularVelocity);
 		inStream.Write(w->mAngle);
+		inStream.Write(w->mContactBodyID); // Used by MotorcycleController::PreCollide
+		inStream.Write(w->mContactNormal); // Used by MotorcycleController::PreCollide
+		inStream.Write(w->mContactLateral); // Used by MotorcycleController::PreCollide
 
 		w->mSuspensionPart.SaveState(inStream);
 		w->mSuspensionMaxUpPart.SaveState(inStream);
@@ -564,6 +567,10 @@ void VehicleConstraint::RestoreState(StateRecorder &inStream)
 	{
 		inStream.Read(w->mAngularVelocity);
 		inStream.Read(w->mAngle);
+		inStream.Read(w->mContactBodyID);
+		inStream.Read(w->mContactNormal);
+		inStream.Read(w->mContactLateral);
+		w->mContactBody = nullptr; // No longer valid
 
 		w->mSuspensionPart.RestoreState(inStream);
 		w->mSuspensionMaxUpPart.RestoreState(inStream);

diff --git a/Jolt/Physics/Vehicle/Wheel.h b/Jolt/Physics/Vehicle/Wheel.h
@@ -72,7 +72,7 @@ class Wheel
 	void					SetSteerAngle(float inAngle)				{ mSteerAngle = inAngle; }
 
 	/// Returns true if the wheel is touching an object
-	bool					HasContact() const							{ return mContactBody != nullptr; }
+	inline bool				HasContact() const							{ return !mContactBodyID.IsInvalid(); }
 
 	/// Returns the body ID of the body that this wheel is touching
 	BodyID					GetContactBodyID() const					{ return mContactBodyID; }
@@ -81,19 +81,19 @@ class Wheel
 	SubShapeID				GetContactSubShapeID() const				{ return mContactSubShapeID; }
 
 	/// Returns the current contact position in world space (note by the time you call this the vehicle has moved)
-	RVec3					GetContactPosition() const					{ JPH_ASSERT(mContactBody != nullptr); return mContactPosition; }
+	RVec3					GetContactPosition() const					{ JPH_ASSERT(HasContact()); return mContactPosition; }
 
 	/// Velocity of the contact point (m / s, not relative to the wheel but in world space)
-	Vec3					GetContactPointVelocity() const				{ JPH_ASSERT(mContactBody != nullptr); return mContactPointVelocity; }
+	Vec3					GetContactPointVelocity() const				{ JPH_ASSERT(HasContact()); return mContactPointVelocity; }
 
 	/// Returns the current contact mormal in world space (note by the time you call this the vehicle has moved)
-	Vec3					GetContactNormal() const					{ JPH_ASSERT(mContactBody != nullptr); return mContactNormal; }
+	Vec3					GetContactNormal() const					{ JPH_ASSERT(HasContact()); return mContactNormal; }
 
 	/// Returns longitudinal direction (direction along the wheel relative to floor) in world space (note by the time you call this the vehicle has moved)
-	Vec3					GetContactLongitudinal() const				{ JPH_ASSERT(mContactBody != nullptr); return mContactLongitudinal; }
+	Vec3					GetContactLongitudinal() const				{ JPH_ASSERT(HasContact()); return mContactLongitudinal; }
 
 	/// Returns lateral direction (sideways direction) in world space (note by the time you call this the vehicle has moved)
-	Vec3					GetContactLateral() const					{ JPH_ASSERT(mContactBody != nullptr); return mContactLateral; }
+	Vec3					GetContactLateral() const					{ JPH_ASSERT(HasContact()); return mContactLateral; }
 
 	/// Get the length of the suspension for a wheel (m) relative to the suspension attachment point (hard point)
 	float					GetSuspensionLength() const					{ return mSuspensionLength; }

diff --git a/Samples/Tests/Vehicle/MotorcycleTest.cpp b/Samples/Tests/Vehicle/MotorcycleTest.cpp
@@ -183,6 +183,22 @@ void MotorcycleTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
 	}
 }
 
+void MotorcycleTest::SaveState(StateRecorder& inStream) const
+{
+	VehicleTest::SaveState(inStream);
+
+	inStream.Write(mPreviousForward);
+	inStream.Write(mCurrentRight);
+}
+
+void MotorcycleTest::RestoreState(StateRecorder& inStream)
+{
+	VehicleTest::RestoreState(inStream);
+
+	inStream.Read(mPreviousForward);
+	inStream.Read(mCurrentRight);
+}
+
 void MotorcycleTest::GetInitialCamera(CameraState &ioState) const 
 {
 	// Position camera behind motorcycle

diff --git a/Samples/Tests/Vehicle/MotorcycleTest.h b/Samples/Tests/Vehicle/MotorcycleTest.h
@@ -20,6 +20,8 @@ class MotorcycleTest : public VehicleTest
 	// 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;
 
 	virtual void				GetInitialCamera(CameraState &ioState) const override;
 	virtual RMat44				GetCameraPivot(float inCameraHeading, float inCameraPitch) const override;

diff --git a/Samples/Tests/Vehicle/TankTest.cpp b/Samples/Tests/Vehicle/TankTest.cpp
@@ -289,7 +289,23 @@ void TankTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
 	}
 }
 
-void TankTest::GetInitialCamera(CameraState &ioState) const 
+void TankTest::SaveState(StateRecorder& inStream) const
+{
+	VehicleTest::SaveState(inStream);
+
+	inStream.Write(mPreviousForward);
+	inStream.Write(mReloadTime);
+}
+
+void TankTest::RestoreState(StateRecorder& inStream)
+{
+	VehicleTest::RestoreState(inStream);
+
+	inStream.Read(mPreviousForward);
+	inStream.Read(mReloadTime);
+}
+
+void TankTest::GetInitialCamera(CameraState &ioState) const
 {
 	// Position camera behind tank
 	ioState.mPos = RVec3(0, 4.0f, 0);

diff --git a/Samples/Tests/Vehicle/TankTest.h b/Samples/Tests/Vehicle/TankTest.h
@@ -20,6 +20,8 @@ class TankTest : public VehicleTest
 	// 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;
 
 	virtual void				GetInitialCamera(CameraState &ioState) const override;
 	virtual RMat44				GetCameraPivot(float inCameraHeading, float inCameraPitch) const override;

diff --git a/Samples/Tests/Vehicle/VehicleConstraintTest.cpp b/Samples/Tests/Vehicle/VehicleConstraintTest.cpp
@@ -229,7 +229,21 @@ void VehicleConstraintTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
 	}
 }
 
-void VehicleConstraintTest::GetInitialCamera(CameraState &ioState) const 
+void VehicleConstraintTest::SaveState(StateRecorder& inStream) const
+{
+	VehicleTest::SaveState(inStream);
+
+	inStream.Write(mPreviousForward);
+}
+
+void VehicleConstraintTest::RestoreState(StateRecorder& inStream)
+{
+	VehicleTest::RestoreState(inStream);
+
+	inStream.Read(mPreviousForward);
+}
+
+void VehicleConstraintTest::GetInitialCamera(CameraState &ioState) const
 {
 	// Position camera behind car
 	RVec3 cam_tgt = RVec3(0, 0, 5);

diff --git a/Samples/Tests/Vehicle/VehicleConstraintTest.h b/Samples/Tests/Vehicle/VehicleConstraintTest.h
@@ -19,6 +19,8 @@ class VehicleConstraintTest : public VehicleTest
 	// 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;
 
 	virtual void				GetInitialCamera(CameraState &ioState) const override;
 	virtual RMat44				GetCameraPivot(float inCameraHeading, float inCameraPitch) const override;