bugfix, cso gjk cast

src/bounce/collision/gjk/gjk.cpp

@@ -881,11 +881,11 @@ bool b3GJKRayCast(b3GJKRayCastOutput* output,
 		return false;
 	}
 
+	b3Vec3 n = gjkOut.point2 - gjkOut.point1;
+	n /= d;
+
 	if (d < radius + tolerance)
 	{
-		b3Vec3 n = gjkOut.point2 - gjkOut.point1;
-		n /= d;
-
 		// Touch
 		output->t = t;
 		output->point = gjkOut.point1 + r1 * n;
@@ -920,6 +920,14 @@ bool b3GJKRayCast(b3GJKRayCastOutput* output,
 		gjkOut = b3GJK(txf1, proxy1, txf2, proxy2, false, &cache);
 		d = gjkOut.distance;
 
+		if (d == 0.0f)
+		{
+			break;
+		}
+
+		n = gjkOut.point2 - gjkOut.point1;
+		n /= d;
+
 		if (d < radius + tolerance)
 		{
 			break;
@@ -934,12 +942,186 @@ bool b3GJKRayCast(b3GJKRayCastOutput* output,
 		}
 	}
 	
-	b3Vec3 n = gjkOut.point2 - gjkOut.point1;
-	n /= d;
+	if (d > 0.0f)
+	{
+		n = gjkOut.point2 - gjkOut.point1;
+		n /= d;
+	}
 
 	output->t = t;
 	output->point = gjkOut.point1 + r1 * n;
 	output->normal = n;
 	output->iterations = iter;
 	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Gino van der Bergen
+// "Smooth Mesh Contacts with GJK"
+// Game Physics Pearls 2010, page 99
+bool b3GJKShapeCast(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;
+
+	b3Vec3 r = translation2;
+
+	float32 t = 0.0f;
+	b3Vec3 n = b3Vec3_zero;
+
+	u32 index1 = proxy1.GetSupportIndex(b3MulT(xf1.rotation, -r));
+	u32 index2 = proxy2.GetSupportIndex(b3MulT(xf2.rotation, r));
+	b3Vec3 w1 = xf1 * proxy1.GetVertex(index1);
+	b3Vec3 w2 = xf2 * proxy2.GetVertex(index2);
+	b3Vec3 v = w1 - w2;
+
+	b3Simplex simplex;
+	simplex.m_count = 0;
+
+	b3SimplexVertex* vertices = simplex.m_vertices;
+
+	u32 save1[4], save2[4];
+	u32 saveCount = 0;
+
+	const u32 kMaxIters = 20;
+	const float32 kTolerance = 10.0f * B3_EPSILON;
+
+	float32 maxTolerance = 1.0f;
+
+	u32 iter = 0;
+	while (iter < kMaxIters && b3Abs(b3LengthSquared(v) - radius * radius) > kTolerance * maxTolerance)
+	{
+		// Support in direction -v
+		index1 = proxy1.GetSupportIndex(b3MulT(xf1.rotation, -v));
+		index2 = proxy2.GetSupportIndex(b3MulT(xf2.rotation, v));
+		w1 = xf1 * proxy1.GetVertex(index1);
+		w2 = xf2 * proxy2.GetVertex(index2);
+		b3Vec3 p = w1 - w2;
+
+		// Support plane on boundary of CSO is (-v, p)
+		// -v is normal at p
+		float32 vp = b3Dot(v, p);
+		float32 vr = b3Dot(v, r);
+
+		if (vp - radius > t * vr)
+		{
+			if (vr > 0.0f)
+			{
+				t = (vp - radius) / vr;
+
+				if (t > 1.0f)
+				{
+					output->iterations = iter;
+					return false;
+				}
+
+				n = -v;
+
+				// Flush the simplex
+				simplex.m_count = 0;
+				saveCount = 0;
+			}
+			else
+			{
+				output->iterations = iter;
+				return false;
+			}
+		}
+
+		// Copy simplex so we can identify duplicates.
+		saveCount = simplex.m_count;
+		for (u32 i = 0; i < saveCount; ++i)
+		{
+			save1[i] = vertices[i].index1;
+			save2[i] = vertices[i].index2;
+		}
+
+		// Unite p - s to simplex
+		b3Vec3 s = t * r;
+
+		b3SimplexVertex* vertex = vertices + simplex.m_count;
+		vertex->index1 = index1;
+		vertex->point1 = w1;
+		vertex->index2 = index2;
+		vertex->point2 = w2;
+		vertex->point = p - s;
+
+		// If we found a duplicate support point we must exit to avoid cycling.
+		bool duplicate = false;
+		for (u32 i = 0; i < saveCount; ++i)
+		{
+			if (vertex->index1 == save1[i] && vertex->index2 == save2[i])
+			{
+				duplicate = true;
+				break;
+			}
+		}
+
+		if (duplicate)
+		{
+			break;
+		}
+
+		++simplex.m_count;
+
+		// Compute tolerance
+		maxTolerance = -B3_EPSILON;
+		for (u32 i = 0; i < simplex.m_count; ++i)
+		{
+			maxTolerance = b3Max(maxTolerance, b3LengthSquared(vertices[i].point));
+		}
+
+		// Sub-solve
+		const b3Vec3 origin = b3Vec3_zero;
+
+		switch (simplex.m_count)
+		{
+		case 1:
+			break;
+		case 2:
+			simplex.Solve2(origin);
+			break;
+		case 3:
+			simplex.Solve3(origin);
+			break;
+		case 4:
+			simplex.Solve4(origin);
+			break;
+		default:
+			B3_ASSERT(false);
+			break;
+		}
+
+		if (simplex.m_count == 4)
+		{
+			// Overlap
+			output->iterations = iter;
+			return false;
+		}
+
+		v = simplex.GetClosestPoint();
+
+		++iter;
+	}
+
+	// Prepare output.
+	b3Vec3 point1, point2;
+	simplex.GetClosestPoints(&point1, &point2);
+
+	if (b3LengthSquared(v) > B3_EPSILON * B3_EPSILON)
+	{
+		n = -v;
+	}
+
+	n.Normalize();
+
+	output->t = t;
+	output->point = point1 + r1 * n;
+	output->normal = n;
+	output->iterations = iter;
+	return true;
 }