Fixed incorrect initial value of v in GJKClosestPoint::CastShape (#149)

Was using the untransformed object A instead of the Minkowski difference B - transformed(A). Results in approx 15% less iterations before returning a result.

Jolt/Geometry/GJKClosestPoint.h

@@ -330,7 +330,7 @@ public:
 
 #ifdef JPH_GJK_DEBUG
 		// Generate the hull of the Minkowski difference for visualization
-		MinkowskiDifference<A, B> diff(inA, inB);
+		MinkowskiDifference diff(inA, inB);
 		mGeometry = DebugRenderer::sInstance->CreateTriangleGeometryForConvex([&diff](Vec3Arg inDirection) { return diff.GetSupport(inDirection); });
 
 		for (int i = 0; i < 4; ++i)
@@ -666,12 +666,11 @@ public:
 	bool		CastShape(Mat44Arg inStart, Vec3Arg inDirection, float inTolerance, const A &inA, const B &inB, float &ioLambda)
 	{
 		// Transform the shape to be cast to the starting position
-		using TransformedA = TransformedConvexObject<A>;
-		TransformedA transformed_a(inStart, inA);
+		TransformedConvexObject transformed_a(inStart, inA);
 
 		// Calculate the minkowski difference inB - inA
 		// inA is moving, so we need to add the back side of inB to the front side of inA
-		MinkowskiDifference<B, TransformedA> difference(inB, transformed_a);
+		MinkowskiDifference difference(inB, transformed_a);
 
 		// Do a raycast against the Minkowski difference
 		return CastRay(Vec3::sZero(), inDirection, inTolerance, difference, ioLambda);
@@ -703,15 +702,14 @@ public:
 		float sum_convex_radius = inConvexRadiusA + inConvexRadiusB;
 
 		// Transform the shape to be cast to the starting position
-		using TransformedA = TransformedConvexObject<A>;
-		TransformedA transformed_a(inStart, inA);
+		TransformedConvexObject transformed_a(inStart, inA);
 
 		// Reset state
 		mNumPoints = 0;
 
 		float lambda = 0.0f;
 		Vec3 x = Vec3::sZero(); // Since A is already transformed we can start the cast from zero
-		Vec3 v = -inA.GetSupport(Vec3::sZero());
+		Vec3 v = -inB.GetSupport(Vec3::sZero()) + transformed_a.GetSupport(Vec3::sZero()); // See CastRay: v = x - inA.GetSupport(Vec3::sZero()) where inA is the Minkowski difference inB - transformed_a (see CastShape above) and x is zero
 		float v_len_sq = FLT_MAX;
 		bool allow_restart = false;
 

UnitTests/Physics/CastShapeTests.cpp

@@ -223,10 +223,10 @@ TEST_SUITE("CastShapeTests")
 			CHECK(collector.HadHit());
 			CHECK(collector.mHit.mBodyID2 == bodies.front()->GetID());
 			CHECK_APPROX_EQUAL(collector.mHit.mFraction, 4.0f / 10.0f);
-			CHECK_APPROX_EQUAL(collector.mHit.mPenetrationAxis.Normalized(), Vec3(1, 0, 0), 2.0e-2f);
+			CHECK_APPROX_EQUAL(collector.mHit.mPenetrationAxis.Normalized(), Vec3(1, 0, 0), 2.0e-3f);
 			CHECK_APPROX_EQUAL(collector.mHit.mPenetrationDepth, 0.0f);
-			CHECK_APPROX_EQUAL(collector.mHit.mContactPointOn1, Vec3(0, 0, 0));
-			CHECK_APPROX_EQUAL(collector.mHit.mContactPointOn2, Vec3(0, 0, 0));
+			CHECK_APPROX_EQUAL(collector.mHit.mContactPointOn1, Vec3(0, 0, 0), 1.0e-4f);
+			CHECK_APPROX_EQUAL(collector.mHit.mContactPointOn2, Vec3(0, 0, 0), 1.0e-4f);
 			CHECK(!collector.mHit.mIsBackFaceHit);
 		}
 
@@ -240,11 +240,11 @@ TEST_SUITE("CastShapeTests")
 			c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, settings, collector);
 			CHECK(collector.HadHit());
 			CHECK(collector.mHit.mBodyID2 == bodies.back()->GetID());
-			CHECK_APPROX_EQUAL(collector.mHit.mFraction, 2.0f / 10.0f);
-			CHECK_APPROX_EQUAL(collector.mHit.mPenetrationAxis.Normalized(), Vec3(-1, 0, 0), 1.0e-3f);
+			CHECK_APPROX_EQUAL(collector.mHit.mFraction, 2.0f / 10.0f, 1.0e-4f);
+			CHECK_APPROX_EQUAL(collector.mHit.mPenetrationAxis.Normalized(), Vec3(-1, 0, 0), 2.0e-3f);
 			CHECK_APPROX_EQUAL(collector.mHit.mPenetrationDepth, 0.0f);
-			CHECK_APPROX_EQUAL(collector.mHit.mContactPointOn1, Vec3(2, 0, 0), 1.0e-4f);
-			CHECK_APPROX_EQUAL(collector.mHit.mContactPointOn2, Vec3(2, 0, 0), 1.0e-4f);
+			CHECK_APPROX_EQUAL(collector.mHit.mContactPointOn1, Vec3(2, 0, 0), 4.0e-4f);
+			CHECK_APPROX_EQUAL(collector.mHit.mContactPointOn2, Vec3(2, 0, 0), 4.0e-4f);
 			CHECK(!collector.mHit.mIsBackFaceHit);
 		}
 

UnitTests/Physics/MotionQualityLinearCastTests.cpp

@@ -302,11 +302,11 @@ TEST_SUITE("MotionQualityLinearCastTests")
 		Vec3 new_velocity1 = Vec3(0, 0, cAngledVelocity.GetZ());
 		Vec3 new_velocity2 = Vec3(cAngledVelocity.GetX(), 0, 0);
 		CHECK_APPROX_EQUAL(box1.GetPosition(), Vec3(2.0f * cBoxExtent, 0, 0), 2.3f * cPenetrationSlop); // We're moving 2x as fast in the z direction and the slop is allowed in x direction: sqrt(1^2 + 2^2) ~ 2.3
-		CHECK_APPROX_EQUAL(box1.GetLinearVelocity(), new_velocity1);
-		CHECK_APPROX_EQUAL(box1.GetAngularVelocity(), Vec3::sZero(), 1.0e-4f);
+		CHECK_APPROX_EQUAL(box1.GetLinearVelocity(), new_velocity1, 1.0e-4f);
+		CHECK_APPROX_EQUAL(box1.GetAngularVelocity(), Vec3::sZero(), 2.0e-4f);
 		CHECK_APPROX_EQUAL(box2.GetPosition(), Vec3::sZero());
-		CHECK_APPROX_EQUAL(box2.GetLinearVelocity(), new_velocity2);
-		CHECK_APPROX_EQUAL(box2.GetAngularVelocity(), Vec3::sZero(), 1.0e-4f);
+		CHECK_APPROX_EQUAL(box2.GetLinearVelocity(), new_velocity2, 1.0e-4f);
+		CHECK_APPROX_EQUAL(box2.GetAngularVelocity(), Vec3::sZero(), 2.0e-4f);
 		CHECK(box2.IsActive());
 		CHECK(activation.Contains(LoggingBodyActivationListener::EType::Activated, box2.GetID()));
 	}