Added function to estimate the collision response from a contact added callback (#435)

Jolt/Core/StaticArray.h

@@ -87,7 +87,7 @@ public:
 	}
 
 	/// Returns maximum amount of elements the array can hold
-	size_type			capacity() const
+	static constexpr size_type capacity()
 	{
 		return N;
 	}

Jolt/Jolt.cmake

@@ -219,6 +219,8 @@ set(JOLT_PHYSICS_SRC_FILES
 	${JOLT_PHYSICS_ROOT}/Physics/Collision/CollisionGroup.cpp
 	${JOLT_PHYSICS_ROOT}/Physics/Collision/CollisionGroup.h
 	${JOLT_PHYSICS_ROOT}/Physics/Collision/ContactListener.h
+	${JOLT_PHYSICS_ROOT}/Physics/Collision/EstimateCollisionResponse.cpp
+	${JOLT_PHYSICS_ROOT}/Physics/Collision/EstimateCollisionResponse.h
 	${JOLT_PHYSICS_ROOT}/Physics/Collision/GroupFilter.cpp
 	${JOLT_PHYSICS_ROOT}/Physics/Collision/GroupFilter.h
 	${JOLT_PHYSICS_ROOT}/Physics/Collision/GroupFilterTable.cpp

Jolt/Physics/Collision/ContactListener.h

@@ -80,7 +80,7 @@ public:
 	/// bodies will receive an OnContactRemoved callback, if this is the case then Body::IsActive() will return false during the callback.
 	/// When contacts are added, the constraint solver has not run yet, so the collision impulse is unknown at that point.
 	/// The velocities of inBody1 and inBody2 are the velocities before the contact has been resolved, so you can use this to
-	/// estimate the collision impulse to e.g. determine the volume of the impact sound to play.
+	/// estimate the collision impulse to e.g. determine the volume of the impact sound to play (see: EstimateCollisionResponse).
 	virtual void			OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) { /* Do nothing */ }
 
 	/// Called whenever a contact is detected that was also detected last update.

Jolt/Physics/Collision/EstimateCollisionResponse.cpp

@@ -0,0 +1,142 @@
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/EstimateCollisionResponse.h>
+#include <Jolt/Physics/Body/Body.h>
+
+JPH_NAMESPACE_BEGIN
+
+void EstimateCollisionResponse(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, Vec3 &outLinearVelocity1, Vec3 &outAngularVelocity1, Vec3 &outLinearVelocity2, Vec3 &outAngularVelocity2, ContactImpulses &outContactImpulses, float inCombinedRestitution, float inMinVelocityForRestitution, uint inNumIterations)
+{
+	// Note this code is based on AxisConstraintPart, see that class for more comments on the math
+
+	// Start with zero impulses
+	outContactImpulses.resize(inManifold.mRelativeContactPointsOn1.size());
+	for (float &impulse : outContactImpulses)
+		impulse = 0.0f;
+
+	// Get body velocities
+	EMotionType motion_type1 = inBody1.GetMotionType();
+	const MotionProperties *motion_properties1 = inBody1.GetMotionPropertiesUnchecked();
+	if (motion_type1 != EMotionType::Static)
+	{
+		outLinearVelocity1 = motion_properties1->GetLinearVelocity();
+		outAngularVelocity1 = motion_properties1->GetAngularVelocity();
+	}
+	else
+		outLinearVelocity1 = outAngularVelocity1 = Vec3::sZero();
+
+	EMotionType motion_type2 = inBody2.GetMotionType();
+	const MotionProperties *motion_properties2 = inBody2.GetMotionPropertiesUnchecked();
+	if (motion_type2 != EMotionType::Static)
+	{
+		outLinearVelocity2 = motion_properties2->GetLinearVelocity();
+		outAngularVelocity2 = motion_properties2->GetAngularVelocity();
+	}
+	else
+		outLinearVelocity2 = outAngularVelocity2 = Vec3::sZero();
+
+	// Get inverse mass and inertia
+	float inv_m1, inv_m2;
+	Mat44 inv_i1, inv_i2;
+	if (motion_type1 == EMotionType::Dynamic)
+	{
+		inv_m1 = motion_properties1->GetInverseMass();
+		inv_i1 = inBody1.GetInverseInertia();
+	}
+	else
+	{
+		inv_m1 = 0.0f;
+		inv_i1 = Mat44::sZero();
+	}
+
+	if (motion_type2 == EMotionType::Dynamic)
+	{
+		inv_m2 = motion_properties2->GetInverseMass();
+		inv_i2 = inBody2.GetInverseInertia();
+	}
+	else
+	{
+		inv_m2 = 0.0f;
+		inv_i2 = Mat44::sZero();
+	}
+
+	// Get center of masses relative to the base offset
+	Vec3 com1 = Vec3(inBody1.GetCenterOfMassPosition() - inManifold.mBaseOffset);
+	Vec3 com2 = Vec3(inBody2.GetCenterOfMassPosition() - inManifold.mBaseOffset);
+
+	struct ContactConstraint
+	{
+		Vec3		mR1PlusUxAxis;
+		Vec3		mR2xAxis;
+		Vec3		mInvI1_R1PlusUxAxis;
+		Vec3		mInvI2_R2xAxis;
+		float		mEffectiveMass;
+		float		mBias;
+	};
+
+	// Initialize the constraint properties
+	ContactConstraint constraints[ContactPoints::capacity()];
+	JPH_ASSERT(inManifold.mRelativeContactPointsOn1.size() == inManifold.mRelativeContactPointsOn2.size());
+	for (uint c = 0; c < inManifold.mRelativeContactPointsOn1.size(); ++c)
+	{
+		ContactConstraint &contact = constraints[c];
+
+		// Calculate contact points relative to body 1 and 2
+		Vec3 p = 0.5f * (inManifold.mRelativeContactPointsOn1[c] + inManifold.mRelativeContactPointsOn2[c]);
+		Vec3 r1 = p - com1;
+		Vec3 r2 = p - com2;
+
+		// Calculate effective mass: K^-1 = (J M^-1 J^T)^-1
+		contact.mR1PlusUxAxis = r1.Cross(inManifold.mWorldSpaceNormal);
+		contact.mR2xAxis = r2.Cross(inManifold.mWorldSpaceNormal);
+		contact.mInvI1_R1PlusUxAxis = inv_i1.Multiply3x3(contact.mR1PlusUxAxis);
+		contact.mInvI2_R2xAxis = inv_i2.Multiply3x3(contact.mR2xAxis);
+		contact.mEffectiveMass = 1.0f / (inv_m1 + contact.mInvI1_R1PlusUxAxis.Dot(contact.mR1PlusUxAxis) + inv_m2 + contact.mInvI2_R2xAxis.Dot(contact.mR2xAxis));
+
+		// Handle elastic collisions
+		contact.mBias = 0.0f;
+		if (inCombinedRestitution > 0.0f)
+		{
+			// Calculate velocity of contact point
+			Vec3 relative_velocity = outLinearVelocity2 + outAngularVelocity2.Cross(r2) - outLinearVelocity1 - outAngularVelocity1.Cross(r1);
+			float normal_velocity = relative_velocity.Dot(inManifold.mWorldSpaceNormal);
+
+			// If it is big enough, apply restitution
+			if (normal_velocity < -inMinVelocityForRestitution)
+				contact.mBias = inCombinedRestitution * normal_velocity;
+		}
+	}
+
+	// If there's only 1 contact point, we only need 1 iteration
+	int num_iterations = inManifold.mRelativeContactPointsOn1.size() == 1? 1 : inNumIterations;
+
+	// Calculate the impulse needed to resolve the contacts
+	for (int iteration = 0; iteration < num_iterations; ++iteration)
+		for (uint c = 0; c < inManifold.mRelativeContactPointsOn1.size(); ++c)
+		{
+			const ContactConstraint &contact = constraints[c];
+			float &total_lambda = outContactImpulses[c];
+
+			// Calculate jacobian multiplied by linear/angular velocity
+			float jv = inManifold.mWorldSpaceNormal.Dot(outLinearVelocity1 - outLinearVelocity2) + contact.mR1PlusUxAxis.Dot(outAngularVelocity1) - contact.mR2xAxis.Dot(outAngularVelocity2);
+
+			// Lagrange multiplier is:
+			//
+			// lambda = -K^-1 (J v + b)
+			float lambda = contact.mEffectiveMass * (jv - contact.mBias);
+			float new_lambda = max(total_lambda + lambda, 0.0f); // Clamp impulse
+			lambda = new_lambda - total_lambda; // Lambda potentially got clamped, calculate the new impulse to apply
+			total_lambda = new_lambda; // Store accumulated impulse
+
+			// Apply impulse to body velocities
+			outLinearVelocity1 -= (lambda * inv_m1) * inManifold.mWorldSpaceNormal;
+			outAngularVelocity1 -= lambda * contact.mInvI1_R1PlusUxAxis;
+			outLinearVelocity2 += (lambda * inv_m2) * inManifold.mWorldSpaceNormal;
+			outAngularVelocity2 += lambda * contact.mInvI2_R2xAxis;
+		}
+}
+
+JPH_NAMESPACE_END

Jolt/Physics/Collision/EstimateCollisionResponse.h

@@ -0,0 +1,30 @@
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/ContactListener.h>
+
+JPH_NAMESPACE_BEGIN
+
+using ContactImpulses = StaticArray<float, ContactPoints::capacity()>;
+
+/// This function estimates the contact impulses and body velocity changes as a result of a collision.
+/// It can be used in the ContactListener::OnContactAdded to determine the strength of the collision to e.g. play a sound or trigger a particle system.
+/// It only uses the contact points and restitution to estimate the velocity changes and will ignore friction.
+/// This function is accurate when two bodies collide but will not be accurate when more than 2 bodies collide at the same time as it does not know about these other collisions.
+/// 
+/// @param inBody1 Colliding body 1
+/// @param inBody2 Colliding body 2
+/// @param inManifold The collision manifold
+/// @param outLinearVelocity1 Outputs the estimated linear velocity of body 1 after collision
+/// @param outAngularVelocity1 Outputs the estimated angular velocity of body 1 after collision
+/// @param outLinearVelocity2 Outputs the estimated linear velocity of body 2 after collision
+/// @param outAngularVelocity2 Outputs the estimated angular velocity of body 2 after collision
+/// @param outContactImpulses An array that will contain the estimated contact impulses when the function returns
+/// @param inCombinedRestitution The combined restitution of body 1 and body 2 (see ContactSettings::mCombinedRestitution)
+/// @param inMinVelocityForRestitution Minimal velocity required for restitution to be applied (see PhysicsSettings::mMinVelocityForRestitution)
+/// @param inNumIterations Number of iterations to use for the impulse estimation (default = 4)
+void EstimateCollisionResponse(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, Vec3 &outLinearVelocity1, Vec3 &outAngularVelocity1, Vec3 &outLinearVelocity2, Vec3 &outAngularVelocity2, ContactImpulses &outContactImpulses, float inCombinedRestitution, float inMinVelocityForRestitution = 1.0f, uint inNumIterations = 10);
+
+JPH_NAMESPACE_END

Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h

@@ -98,7 +98,7 @@ public:
 
 		if constexpr (Type1 == EMotionType::Dynamic)
 		{
-			Vec3 invi1_r1_plus_u_x_axis = inInvI1 * r1_plus_u_x_axis;
+			Vec3 invi1_r1_plus_u_x_axis = inInvI1.Multiply3x3(r1_plus_u_x_axis);
 			invi1_r1_plus_u_x_axis.StoreFloat3(&mInvI1_R1PlusUxAxis);
 			inv_effective_mass = inMotionProperties1->GetInverseMass() + invi1_r1_plus_u_x_axis.Dot(r1_plus_u_x_axis);
 		}
@@ -111,7 +111,7 @@ public:
 
 		if constexpr (Type2 == EMotionType::Dynamic)
 		{
-			Vec3 invi2_r2_x_axis = inInvI2 * r2_x_axis;
+			Vec3 invi2_r2_x_axis = inInvI2.Multiply3x3(r2_x_axis);
 			invi2_r2_x_axis.StoreFloat3(&mInvI2_R2xAxis);
 			inv_effective_mass += inMotionProperties2->GetInverseMass() + invi2_r2_x_axis.Dot(r2_x_axis);
 		}

Samples/Tests/General/ContactListenerTest.cpp

@@ -8,6 +8,7 @@
 #include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
 #include <Jolt/Physics/Collision/Shape/SphereShape.h>
 #include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
+#include <Jolt/Physics/Collision/EstimateCollisionResponse.h>
 #include <Jolt/Physics/Body/BodyCreationSettings.h>
 #include <Layers.h>
 #include <Renderer/DebugRendererImp.h>
@@ -55,6 +56,12 @@ void ContactListenerTest::Initialize()
 	mBody[3] = &body4;
 }
 
+void ContactListenerTest::PostPhysicsUpdate(float inDeltaTime)
+{
+	for (Body *body : mBody)
+		Trace("State, body: %08x, v=%s, w=%s", body->GetID().GetIndex(), ConvertToString(body->GetLinearVelocity()).c_str(), ConvertToString(body->GetAngularVelocity()).c_str());
+}
+
 ValidateResult ContactListenerTest::OnContactValidate(const Body &inBody1, const Body &inBody2, RVec3Arg inBaseOffset, const CollideShapeResult &inCollisionResult)
 {
 	// Body 1 and 2 should never collide
@@ -69,4 +76,19 @@ void ContactListenerTest::OnContactAdded(const Body &inBody1, const Body &inBody
 		JPH_ASSERT(ioSettings.mCombinedRestitution == 0.0f);
 		ioSettings.mCombinedRestitution = 1.0f;
 	}
+
+	// Estimate the contact impulses. Note that these won't be 100% accurate unless you set the friction of the bodies to 0 (EstimateCollisionResponse ignores friction)
+	ContactImpulses impulses;
+	Vec3 v1, w1, v2, w2;
+	EstimateCollisionResponse(inBody1, inBody2, inManifold, v1, w1, v2, w2, impulses, ioSettings.mCombinedRestitution);
+
+	// Trace the result
+	String impulses_str;
+	for (float impulse : impulses)
+		impulses_str += StringFormat("%f ", (double)impulse);
+
+	Trace("Estimated velocity after collision, body1: %08x, v=%s, w=%s, body2: %08x, v=%s, w=%s, impulses: %s",
+		inBody1.GetID().GetIndex(), ConvertToString(v1).c_str(), ConvertToString(w1).c_str(),
+		inBody2.GetID().GetIndex(), ConvertToString(v2).c_str(), ConvertToString(w2).c_str(),
+		impulses_str.c_str());
 }

Samples/Tests/General/ContactListenerTest.h

@@ -14,6 +14,7 @@ public:
 
 	// See: Test
 	virtual void			Initialize() override;
+	virtual void			PostPhysicsUpdate(float inDeltaTime) override;
 
 	// If this test implements a contact listener, it should be returned here
 	virtual ContactListener *GetContactListener() override		{ return this; }

UnitTests/Physics/EstimateCollisionResponseTest.cpp

@@ -0,0 +1,71 @@
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Physics/Collision/EstimateCollisionResponse.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include "PhysicsTestContext.h"
+#include "Layers.h"
+
+TEST_SUITE("EstimateCollisionResponseTests")
+{
+	// Test CastShape ordering according to penetration depth
+	TEST_CASE("TestEstimateCollisionResponse")
+	{
+		const Vec3 cBox1HalfExtents(0.1f, 1, 2);
+		const Vec3 cBox2HalfExtents(0.2f, 3, 4);
+
+		// Test different motion types, restitution, positions and angular velocities
+		for (EMotionType mt : { EMotionType::Static, EMotionType::Kinematic, EMotionType::Dynamic })
+			for (float restitution : { 0.0f, 0.3f, 1.0f })
+				for (float y : { 0.0f, 0.5f, cBox2HalfExtents.GetY() })
+					for (float z : { 0.0f, 0.5f, cBox2HalfExtents.GetZ() })
+						for (float w : { 0.0f, -1.0f, 1.0f })
+						{
+							PhysicsTestContext c;
+							c.ZeroGravity();
+
+							// Install a listener that predicts the collision response
+							class MyListener : public ContactListener
+							{
+							public:
+								virtual void	OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+								{
+									EstimateCollisionResponse(inBody1, inBody2, inManifold, mPredictedV1, mPredictedW1, mPredictedV2, mPredictedW2, mImpulses, ioSettings.mCombinedRestitution);
+								}
+
+								ContactImpulses	mImpulses;
+								Vec3			mPredictedV1 = Vec3::sNaN();
+								Vec3			mPredictedW1 = Vec3::sNaN();
+								Vec3			mPredictedV2 = Vec3::sNaN();
+								Vec3			mPredictedW2 = Vec3::sNaN();
+							};
+
+							MyListener listener;
+							c.GetSystem()->SetContactListener(&listener);
+
+							const RVec3 cBaseOffset(1, 2, 3);
+							const Real cEpsilon = 0.0001_r;
+
+							Body &box1 = c.CreateBox(cBaseOffset, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, cBox1HalfExtents);
+							box1.SetRestitution(restitution);
+							box1.SetFriction(0.0f); // No friction, the estimation doesn't handle that
+							box1.SetLinearVelocity(Vec3(1, 1, 0));
+							box1.SetAngularVelocity(Vec3(0, w, 0));
+
+							Body &box2 = c.CreateBox(cBaseOffset + RVec3(cBox1HalfExtents.GetX() + cBox2HalfExtents.GetX() - cEpsilon, y, z), Quat::sIdentity(), mt, EMotionQuality::Discrete, mt == EMotionType::Static? Layers::NON_MOVING : Layers::MOVING, cBox2HalfExtents);
+							box2.SetRestitution(restitution);
+							box2.SetFriction(0.0f);
+							if (mt != EMotionType::Static)
+								box2.SetLinearVelocity(Vec3(-1, 0, 0));
+
+							c.SimulateSingleStep();
+
+							// Check that the predicted velocities are correct
+							CHECK_APPROX_EQUAL(listener.mPredictedV1, box1.GetLinearVelocity());
+							CHECK_APPROX_EQUAL(listener.mPredictedW1, box1.GetAngularVelocity());
+							CHECK_APPROX_EQUAL(listener.mPredictedV2, box2.GetLinearVelocity());
+							CHECK_APPROX_EQUAL(listener.mPredictedW2, box2.GetAngularVelocity());
+						}
+	}
+}

UnitTests/UnitTests.cmake

@@ -41,6 +41,7 @@ set(UNIT_TESTS_SRC_FILES
 	${UNIT_TESTS_ROOT}/Physics/CollisionGroupTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/ContactListenerTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/ConvexVsTrianglesTest.cpp
+	${UNIT_TESTS_ROOT}/Physics/EstimateCollisionResponseTest.cpp
 	${UNIT_TESTS_ROOT}/Physics/HeightFieldShapeTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/MotionQualityLinearCastTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/OffsetCenterOfMassShapeTests.cpp