Added friction estimation to EstimateCollisionResponse (#441)

Jolt/Physics/Collision/EstimateCollisionResponse.cpp

@@ -8,35 +8,41 @@
 
 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)
+void EstimateCollisionResponse(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, CollisionEstimationResult &outResult, float inCombinedFriction, float inCombinedRestitution, float inMinVelocityForRestitution, uint inNumIterations)
 {
 	// Note this code is based on AxisConstraintPart, see that class for more comments on the math
 
+	ContactPoints::size_type num_points = inManifold.mRelativeContactPointsOn1.size();
+	JPH_ASSERT(num_points == inManifold.mRelativeContactPointsOn2.size());
+
 	// Start with zero impulses
-	outContactImpulses.resize(inManifold.mRelativeContactPointsOn1.size());
-	for (float &impulse : outContactImpulses)
-		impulse = 0.0f;
+	outResult.mImpulses.resize(num_points);
+	memset(outResult.mImpulses.data(), 0, num_points * sizeof(CollisionEstimationResult::Impulse));
+
+	// Calculate friction directions
+	outResult.mTangent1 = inManifold.mWorldSpaceNormal.GetNormalizedPerpendicular();
+	outResult.mTangent2 = inManifold.mWorldSpaceNormal.Cross(outResult.mTangent1);
 
 	// 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();
+		outResult.mLinearVelocity1 = motion_properties1->GetLinearVelocity();
+		outResult.mAngularVelocity1 = motion_properties1->GetAngularVelocity();
 	}
 	else
-		outLinearVelocity1 = outAngularVelocity1 = Vec3::sZero();
+		outResult.mLinearVelocity1 = outResult.mAngularVelocity1 = 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();
+		outResult.mLinearVelocity2 = motion_properties2->GetLinearVelocity();
+		outResult.mAngularVelocity2 = motion_properties2->GetAngularVelocity();
 	}
 	else
-		outLinearVelocity2 = outAngularVelocity2 = Vec3::sZero();
+		outResult.mLinearVelocity2 = outResult.mAngularVelocity2 = Vec3::sZero();
 
 	// Get inverse mass and inertia
 	float inv_m1, inv_m2;
@@ -67,76 +73,110 @@ void EstimateCollisionResponse(const Body &inBody1, const Body &inBody2, const C
 	Vec3 com1 = Vec3(inBody1.GetCenterOfMassPosition() - inManifold.mBaseOffset);
 	Vec3 com2 = Vec3(inBody2.GetCenterOfMassPosition() - inManifold.mBaseOffset);
 
-	struct ContactConstraint
+	struct AxisConstraint
 	{
-		Vec3		mR1PlusUxAxis;
-		Vec3		mR2xAxis;
-		Vec3		mInvI1_R1PlusUxAxis;
-		Vec3		mInvI2_R2xAxis;
-		float		mEffectiveMass;
-		float		mBias;
+		inline void		Initialize(Vec3Arg inR1, Vec3Arg inR2, Vec3Arg inWorldSpaceNormal, float inInvM1, float inInvM2, Mat44Arg inInvI1, Mat44Arg inInvI2)
+		{
+			// Calculate effective mass: K^-1 = (J M^-1 J^T)^-1
+			mR1PlusUxAxis = inR1.Cross(inWorldSpaceNormal);
+			mR2xAxis = inR2.Cross(inWorldSpaceNormal);
+			mInvI1_R1PlusUxAxis = inInvI1.Multiply3x3(mR1PlusUxAxis);
+			mInvI2_R2xAxis = inInvI2.Multiply3x3(mR2xAxis);
+			mEffectiveMass = 1.0f / (inInvM1 + mInvI1_R1PlusUxAxis.Dot(mR1PlusUxAxis) + inInvM2 + mInvI2_R2xAxis.Dot(mR2xAxis));
+			mBias = 0.0f;
+		}
+
+		inline void		Solve(Vec3Arg inWorldSpaceNormal, float inInvM1, float inInvM2, float inMinLambda, float inMaxLambda, float &ioTotalLambda, CollisionEstimationResult &ioResult) const
+		{
+			// Calculate jacobian multiplied by linear/angular velocity
+			float jv = inWorldSpaceNormal.Dot(ioResult.mLinearVelocity1 - ioResult.mLinearVelocity2) + mR1PlusUxAxis.Dot(ioResult.mAngularVelocity1) - mR2xAxis.Dot(ioResult.mAngularVelocity2);
+
+			// Lagrange multiplier is:
+			//
+			// lambda = -K^-1 (J v + b)
+			float lambda = mEffectiveMass * (jv - mBias);
+			float new_lambda = Clamp(ioTotalLambda + lambda, inMinLambda, inMaxLambda); // Clamp impulse
+			lambda = new_lambda - ioTotalLambda; // Lambda potentially got clamped, calculate the new impulse to apply
+			ioTotalLambda = new_lambda; // Store accumulated impulse
+
+			// Apply impulse to body velocities
+			ioResult.mLinearVelocity1 -= (lambda * inInvM1) * inWorldSpaceNormal;
+			ioResult.mAngularVelocity1 -= lambda * mInvI1_R1PlusUxAxis;
+			ioResult.mLinearVelocity2 += (lambda * inInvM2) * inWorldSpaceNormal;
+			ioResult.mAngularVelocity2 += lambda * mInvI2_R2xAxis;
+		}
+
+		Vec3			mR1PlusUxAxis;
+		Vec3			mR2xAxis;
+		Vec3			mInvI1_R1PlusUxAxis;
+		Vec3			mInvI2_R2xAxis;
+		float			mEffectiveMass;
+		float			mBias;
+	};
+
+	struct Constraint
+	{
+		AxisConstraint	mContact;
+		AxisConstraint	mFriction1;
+		AxisConstraint	mFriction2;
 	};
 
 	// 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)
+	Constraint constraints[ContactPoints::Capacity];
+	for (uint c = 0; c < num_points; ++c)
 	{
-		ContactConstraint &contact = constraints[c];
+		Constraint &constraint = 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));
+		// Initialize contact constraint
+		constraint.mContact.Initialize(r1, r2, inManifold.mWorldSpaceNormal, inv_m1, inv_m2, inv_i1, inv_i2);
 
 		// 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);
+			Vec3 relative_velocity = outResult.mLinearVelocity2 + outResult.mAngularVelocity2.Cross(r2) - outResult.mLinearVelocity1 - outResult.mAngularVelocity1.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;
+				constraint.mContact.mBias = inCombinedRestitution * normal_velocity;
+		}
+
+		if (inCombinedFriction > 0.0f)
+		{
+			// Initialize friction constraints
+			constraint.mFriction1.Initialize(r1, r2, outResult.mTangent1, inv_m1, inv_m2, inv_i1, inv_i2);
+			constraint.mFriction2.Initialize(r1, r2, outResult.mTangent2, inv_m1, inv_m2, inv_i1, inv_i2);
 		}
 	}
 
 	// If there's only 1 contact point, we only need 1 iteration
-	int num_iterations = inManifold.mRelativeContactPointsOn1.size() == 1? 1 : inNumIterations;
+	int num_iterations = inCombinedFriction == 0.0f && num_points == 1? 1 : inNumIterations;
 
-	// Calculate the impulse needed to resolve the contacts
+	// Solve iteratively
 	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;
-		}
+	{
+		// Solve friction constraints first
+		if (inCombinedFriction > 0.0f)
+			for (uint c = 0; c < num_points; ++c)
+			{
+				const Constraint &constraint = constraints[c];
+				CollisionEstimationResult::Impulse &impulse = outResult.mImpulses[c];
+
+				float max_impulse = inCombinedFriction * impulse.mContactImpulse;
+				constraint.mFriction1.Solve(outResult.mTangent1, inv_m1, inv_m2, -max_impulse, max_impulse, impulse.mFrictionImpulse1, outResult);
+				constraint.mFriction2.Solve(outResult.mTangent2, inv_m1, inv_m2, -max_impulse, max_impulse, impulse.mFrictionImpulse2, outResult);
+			}
+
+		// Solve contact constraints last
+		for (uint c = 0; c < num_points; ++c)
+			constraints[c].mContact.Solve(inManifold.mWorldSpaceNormal, inv_m1, inv_m2, 0.0f, FLT_MAX, outResult.mImpulses[c].mContactImpulse, outResult);
+	}
 }
 
 JPH_NAMESPACE_END

Jolt/Physics/Collision/EstimateCollisionResponse.h

@@ -7,7 +7,28 @@
 
 JPH_NAMESPACE_BEGIN
 
-using ContactImpulses = StaticArray<float, ContactPoints::Capacity>;
+/// A structure that contains the estimated contact and friction impulses and the resulting body velocities
+struct CollisionEstimationResult
+{
+	Vec3			mLinearVelocity1;				///< The estimated linear velocity of body 1 after collision
+	Vec3			mAngularVelocity1;				///< The estimated angular velocity of body 1 after collision
+	Vec3			mLinearVelocity2;				///< The estimated linear velocity of body 2 after collision
+	Vec3			mAngularVelocity2;				///< The estimated angular velocity of body 2 after collision
+
+	Vec3			mTangent1;						///< Normalized tangent of contact normal
+	Vec3			mTangent2;						///< Second normalized tangent of contact normal (forms a basis with mTangent1 and mWorldSpaceNormal)
+
+	struct Impulse
+	{
+		float		mContactImpulse;				///< Estimated contact impulses (kg m / s)
+		float		mFrictionImpulse1;				///< Estimated friction impulses in the direction of tangent 1 (kg m / s)
+		float		mFrictionImpulse2;				///< Estimated friction impulses in the direction of tangent 2 (kg m / s)
+	};
+
+	using Impulses = StaticArray<Impulse, ContactPoints::Capacity>;
+
+	Impulses		mImpulses;
+};
 
 /// 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.
@@ -17,14 +38,11 @@ using ContactImpulses = StaticArray<float, ContactPoints::Capacity>;
 /// @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 outResult A structure that contains the estimated contact and friction impulses and the resulting body velocities
+/// @param inCombinedFriction The combined friction of body 1 and body 2 (see ContactSettings::mCombinedFriction)
 /// @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);
+/// @param inNumIterations Number of iterations to use for the impulse estimation (see PhysicsSettings::mNumVelocitySteps)
+void EstimateCollisionResponse(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, CollisionEstimationResult &outResult, float inCombinedFriction, float inCombinedRestitution, float inMinVelocityForRestitution = 1.0f, uint inNumIterations = 10);
 
 JPH_NAMESPACE_END

Samples/Tests/General/ContactListenerTest.cpp

@@ -77,18 +77,17 @@ void ContactListenerTest::OnContactAdded(const Body &inBody1, const Body &inBody
 		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);
+	// Estimate the contact impulses.
+	CollisionEstimationResult result;
+	EstimateCollisionResponse(inBody1, inBody2, inManifold, result, ioSettings.mCombinedFriction, ioSettings.mCombinedRestitution);
 
 	// Trace the result
 	String impulses_str;
-	for (float impulse : impulses)
-		impulses_str += StringFormat("%f ", (double)impulse);
+	for (const CollisionEstimationResult::Impulse &impulse : result.mImpulses)
+		impulses_str += StringFormat("(%f, %f, %f) ", (double)impulse.mContactImpulse, (double)impulse.mFrictionImpulse1, (double)impulse.mFrictionImpulse2);
 
 	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(),
+		inBody1.GetID().GetIndex(), ConvertToString(result.mLinearVelocity1).c_str(), ConvertToString(result.mAngularVelocity1).c_str(),
+		inBody2.GetID().GetIndex(), ConvertToString(result.mLinearVelocity2).c_str(), ConvertToString(result.mAngularVelocity2).c_str(),
 		impulses_str.c_str());
 }

UnitTests/Physics/EstimateCollisionResponseTest.cpp

@@ -12,60 +12,65 @@ TEST_SUITE("EstimateCollisionResponseTests")
 	// Test CastShape ordering according to penetration depth
 	TEST_CASE("TestEstimateCollisionResponse")
 	{
+		PhysicsTestContext c;
+		c.ZeroGravity();
+
 		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
+				for (float friction : { 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 })
 							{
-							public:
-								virtual void	OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+								// Install a listener that predicts the collision response
+								class MyListener : public ContactListener
 								{
-									EstimateCollisionResponse(inBody1, inBody2, inManifold, mPredictedV1, mPredictedW1, mPredictedV2, mPredictedW2, mImpulses, ioSettings.mCombinedRestitution);
-								}
+								public:
+									virtual void	OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+									{
+										EstimateCollisionResponse(inBody1, inBody2, inManifold, mResult, ioSettings.mCombinedFriction, ioSettings.mCombinedRestitution);
+									}
+
+									CollisionEstimationResult mResult;
+								};
 
-								ContactImpulses	mImpulses;
-								Vec3			mPredictedV1 = Vec3::sNaN();
-								Vec3			mPredictedW1 = Vec3::sNaN();
-								Vec3			mPredictedV2 = Vec3::sNaN();
-								Vec3			mPredictedW2 = Vec3::sNaN();
-							};
+								MyListener listener;
+								c.GetSystem()->SetContactListener(&listener);
 
-							MyListener listener;
-							c.GetSystem()->SetContactListener(&listener);
+								const RVec3 cBaseOffset(1, 2, 3);
+								const Real cEpsilon = 0.0001_r;
 
-							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.SetFriction(friction);
+								box1.SetRestitution(restitution);
+								box1.SetLinearVelocity(Vec3(1, 1, 0));
+								box1.SetAngularVelocity(Vec3(0, w, 0));
 
-							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.SetFriction(friction);
+								box2.SetRestitution(restitution);
+								if (mt != EMotionType::Static)
+									box2.SetLinearVelocity(Vec3(-1, 0, 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));
+								// Step the simulation
+								c.SimulateSingleStep();
 
-							c.SimulateSingleStep();
+								// Check that the predicted velocities are correct
+								CHECK_APPROX_EQUAL(listener.mResult.mLinearVelocity1, box1.GetLinearVelocity());
+								CHECK_APPROX_EQUAL(listener.mResult.mAngularVelocity1, box1.GetAngularVelocity());
+								CHECK_APPROX_EQUAL(listener.mResult.mLinearVelocity2, box2.GetLinearVelocity());
+								CHECK_APPROX_EQUAL(listener.mResult.mAngularVelocity2, box2.GetAngularVelocity());
 
-							// 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());
-						}
+								// Remove the bodies in reverse order
+								BodyInterface &bi = c.GetBodyInterface();
+								bi.RemoveBody(box2.GetID());
+								bi.RemoveBody(box1.GetID());
+								bi.DestroyBody(box2.GetID());
+								bi.DestroyBody(box1.GetID());
+							}
 	}
 }