add shape cast function

include/bounce/collision/gjk/gjk.h

@@ -114,4 +114,18 @@ b3GJKOutput b3GJK(const b3Transform& xf1, const b3GJKProxy& proxy1,
 	const b3Transform& xf2, const b3GJKProxy& proxy2,
 	bool applyRadius, b3SimplexCache* cache);
 
+// The output of the GJK-based ray cast algorithm.
+struct b3GJKRayCastOutput
+{
+	float32 t; // time of impact
+	b3Vec3 point; // contact point at t
+	b3Vec3 normal; // contact normal at t
+	u32 iterations; // number of iterations 
+};
+
+// Find the time of impact between two proxies given the relative target translation vector.
+bool b3GJKRayCast(b3GJKRayCastOutput* output,
+	const b3Transform& xf1, const b3GJKProxy& proxy1,
+	const b3Transform& xf2, const b3GJKProxy& proxy2, const b3Vec3& translation2);
+
 #endif

src/bounce/collision/gjk/gjk.cpp

@@ -849,3 +849,98 @@ b3GJKOutput b3GJK(const b3Transform& xf1, const b3GJKProxy& proxy1,
 	cache.count = 0;
 	return b3GJK(xf1, proxy1, xf2, proxy2, false, &cache);
 }
+
+///////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Brian Mirtich  
+// "Conservative Advancement" 
+bool b3GJKRayCast(b3GJKRayCastOutput* output,
+	const b3Transform& xf1, const b3GJKProxy& proxy1,
+	const b3Transform& xf2, const b3GJKProxy& proxy2, const b3Vec3& translation2)
+{
+	float32 r1 = proxy1.radius;
+	float32 r2 = proxy2.radius;
+	float32 radius = r1 + r2;
+
+	float32 d_max = b3Length(translation2);
+	B3_ASSERT(d_max > 0.0f);
+
+	float32 t = 0.0f;
+	
+	const float32 tolerance = 0.5f * B3_LINEAR_SLOP;
+
+	b3SimplexCache cache;
+	cache.count = 0;
+
+	b3GJKOutput gjkOut = b3GJK(xf1, proxy1, xf2, proxy2, false, &cache);
+	float32 d = gjkOut.distance;
+
+	if (d == 0.0f)
+	{
+		// Overlap
+		return false;
+	}
+
+	if (d < radius + tolerance)
+	{
+		b3Vec3 n = gjkOut.point2 - gjkOut.point1;
+		n /= d;
+
+		// Touch
+		output->t = t;
+		output->point = gjkOut.point1 + r1 * n;
+		output->normal = n;
+		output->iterations = 0;
+		return true;
+	}
+
+	const u32 kMaxIters = 20;
+
+	u32 iter = 0;
+	for (;;)
+	{
+		B3_ASSERT(d < d_max);
+		B3_ASSERT(d >= radius);
+		
+		float32 dt = (d - radius) / d_max;
+		t += dt;
+
+		if (t >= 1.0f)
+		{
+			// No overlap
+			output->iterations = iter;
+			return false;
+		}
+		
+		b3Transform txf1 = xf1;
+		
+		b3Transform txf2;
+		txf2.rotation = xf2.rotation;
+		txf2.position = (1.0f - t) * xf2.position + t * (xf2.position + r);
+
+		gjkOut = b3GJK(txf1, proxy1, txf2, proxy2, false, &cache);
+		d = gjkOut.distance;
+
+		if (d < radius + tolerance)
+		{
+			break;
+		}
+
+		++iter;
+
+		if (iter == kMaxIters)
+		{
+			output->iterations = iter;
+			return false;
+		}
+	}
+	
+	b3Vec3 n = gjkOut.point2 - gjkOut.point1;
+	n /= d;
+
+	output->t = t;
+	output->point = gjkOut.point1 + r1 * n;
+	output->normal = n;
+	output->iterations = iter;
+	return true;
+}