add collision detection routines for shape against sphere

include/bounce/dynamics/shapes/capsule_shape.h

@@ -33,7 +33,9 @@ public:
 
 	void ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const;
 
-	bool TestPoint(const b3Vec3& point, const b3Transform& xf) const;
+	bool TestSphere(const b3Sphere& sphere, const b3Transform& xf) const;
+	
+	bool TestSphere(b3TestSphereOutput* output, const b3Sphere& sphere, const b3Transform& xf) const;
 
 	bool RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf) const;
 

include/bounce/dynamics/shapes/hull_shape.h

@@ -35,7 +35,9 @@ public :
 
 	void ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const;	
 
-	bool TestPoint(const b3Vec3& point, const b3Transform& xf) const;
+	bool TestSphere(const b3Sphere& sphere, const b3Transform& xf) const;
+	
+	bool TestSphere(b3TestSphereOutput* output, const b3Sphere& sphere, const b3Transform& xf) const;
 
 	bool RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf) const;
 

include/bounce/dynamics/shapes/mesh_shape.h

@@ -37,7 +37,9 @@ public:
 
 	void ComputeAABB(b3AABB3* output, const b3Transform& xf, u32 childIndex) const;
 
-	bool TestPoint(const b3Vec3& point, const b3Transform& xf) const;
+	bool TestSphere(const b3Sphere& sphere, const b3Transform& xf) const;
+
+	bool TestSphere(b3TestSphereOutput* output, const b3Sphere& sphere, const b3Transform& xf) const;
 	
 	bool RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf) const;
 

include/bounce/dynamics/shapes/shape.h

@@ -22,6 +22,7 @@
 #include <bounce/common/math/transform.h>
 #include <bounce/common/template/list.h>
 #include <bounce/collision/collision.h>
+#include <bounce/collision/shapes/sphere.h>
 
 struct b3ContactEdge;
 
@@ -37,6 +38,12 @@ enum b3ShapeType
 	e_maxShapes
 };
 
+struct b3TestSphereOutput
+{
+	float32 separation;
+	b3Vec3 normal;
+};
+
 struct b3ShapeDef 
 {
 	b3ShapeDef() 
@@ -72,7 +79,7 @@ struct b3MassData
 class b3Shape
 {
 public:
-	b3Shape() { }
+	b3Shape();
 	virtual ~b3Shape() { }
 
 	// Get the shape type.
@@ -88,8 +95,13 @@ public:
 	// Compute the shape world AABB.
 	virtual void ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const = 0;
 
-	// Test if a point is contained inside this shape.
+	// Test if a sphere is contained inside this shape.
-	virtual bool TestPoint(const b3Vec3& point, const b3Transform& xf) const = 0;
+	virtual bool TestSphere(const b3Sphere& sphere, const b3Transform& xf) const = 0;
+
+	// Test if a sphere is contained inside this shape.
+	// If the sphere is inside this shape then return the minimum separation distance and normal.
+	// The direction of the normal points from this shape to the sphere.
+	virtual bool TestSphere(b3TestSphereOutput* output, const b3Sphere& sphere, const b3Transform& xf) const = 0;
 
 	// Compute the ray intersection point, normal of surface, and fraction.
 	virtual bool RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf) const = 0;

include/bounce/dynamics/shapes/sphere_shape.h

@@ -33,7 +33,9 @@ public :
 
 	void ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const;
 
-	bool TestPoint(const b3Vec3& point, const b3Transform& xf) const;
+	bool TestSphere(const b3Sphere& sphere, const b3Transform& xf) const;
+	
+	bool TestSphere(b3TestSphereOutput* output, const b3Sphere& sphere, const b3Transform& xf) const;
 
 	bool RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf) const;
 

src/bounce/dynamics/shapes/capsule_shape.cpp

@@ -140,10 +140,10 @@ void b3CapsuleShape::ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const
 	aabb->m_upper = b3Max(c1, c2) + r;
 }
 
-bool b3CapsuleShape::TestPoint(const b3Vec3& point, const b3Transform& xf) const
+bool b3CapsuleShape::TestSphere(const b3Sphere& sphere, const b3Transform& xf) const
 {
 	// The point in the frame of the capsule
-	b3Vec3 Q = b3MulT(xf, point);
+	b3Vec3 Q = b3MulT(xf, sphere.vertex);
 
 	b3Vec3 A = m_centers[0];
 	b3Vec3 B = m_centers[1];
@@ -153,10 +153,12 @@ bool b3CapsuleShape::TestPoint(const b3Vec3& point, const b3Transform& xf) const
 	float32 u = b3Dot(B - Q, AB);
 	float32 v = b3Dot(Q - A, AB);
 
+	float32 radius = m_radius + sphere.radius;
+
 	if (v <= 0.0f)
 	{
 		// A
-		if (b3DistanceSquared(A, Q) > m_radius * m_radius)
+		if (b3DistanceSquared(A, Q) > radius * radius)
 		{
 			return false;
 		}
@@ -166,7 +168,7 @@ bool b3CapsuleShape::TestPoint(const b3Vec3& point, const b3Transform& xf) const
 	if (u <= 0.0f)
 	{
 		// B
-		if (b3DistanceSquared(B, Q) > m_radius * m_radius)
+		if (b3DistanceSquared(B, Q) > radius * radius)
 		{
 			return false;
 		}
@@ -177,7 +179,7 @@ bool b3CapsuleShape::TestPoint(const b3Vec3& point, const b3Transform& xf) const
 	float32 s = b3Dot(AB, AB);
 	B3_ASSERT(s > 0.0f);
 	b3Vec3 P = (1.0f / s) * (u * A + v * B);
-	if (b3DistanceSquared(P, Q) > m_radius * m_radius)
+	if (b3DistanceSquared(P, Q) > radius * radius)
 	{
 		return false;
 	}
@@ -185,6 +187,102 @@ bool b3CapsuleShape::TestPoint(const b3Vec3& point, const b3Transform& xf) const
 	return true;
 }
 
+bool b3CapsuleShape::TestSphere(b3TestSphereOutput* output, const b3Sphere& sphere, const b3Transform& xf) const
+{
+	b3Vec3 Q = sphere.vertex;
+
+	b3Vec3 A = b3Mul(xf, m_centers[0]);
+	b3Vec3 B = b3Mul(xf, m_centers[1]);
+	b3Vec3 AB = B - A;
+
+	// Barycentric coordinates for Q
+	float32 u = b3Dot(B - Q, AB);
+	float32 v = b3Dot(Q - A, AB);
+
+	float32 radius = m_radius + sphere.radius;
+
+	if (v <= 0.0f)
+	{
+		// A
+		b3Vec3 P = A;
+		b3Vec3 d = Q - P;
+		float32 dd = b3Dot(d, d);
+		if (dd > radius * radius)
+		{
+			return false;
+		}
+
+		b3Vec3 n(0.0f, 1.0f, 0.0f);
+		float32 len = b3Sqrt(dd);
+		if (len > B3_EPSILON)
+		{
+			n = d / len;
+		}
+
+		output->separation = len - radius;
+		output->normal = n;
+
+		return true;
+	}
+
+	if (u <= 0.0f)
+	{
+		// B
+		b3Vec3 P = B;
+		b3Vec3 d = Q - P;
+		float32 dd = b3Dot(d, d);
+		if (dd > radius * radius)
+		{
+			return false;
+		}
+
+		b3Vec3 n(0.0f, 1.0f, 0.0f);
+		float32 len = b3Sqrt(dd);
+		if (len > B3_EPSILON)
+		{
+			n = d / len;
+		}
+
+		output->separation = len - radius;
+		output->normal = n;
+
+		return true;
+	}
+
+	// AB
+	float32 s = b3Dot(AB, AB);
+	//B3_ASSERT(s > 0.0f);
+	b3Vec3 P;
+	if (s < B3_LINEAR_SLOP * B3_LINEAR_SLOP)
+	{
+		P = A;
+	}
+	else
+	{
+		P = (u * A + v * B) / s;
+	}
+
+	b3Vec3 d = Q - P;
+	float32 dd = b3Dot(d, d);
+	if (dd > radius * radius)
+	{
+		return false;
+	}
+
+	b3Vec3 QA = A - Q;
+	b3Vec3 e = b3Cross(AB, QA);
+	b3Vec3 n = b3Cross(AB, e);
+	if (b3Dot(n, QA) < 0.0f)
+	{
+		n = -n;
+	}
+	n.Normalize();
+
+	output->separation = b3Sqrt(dd) - radius;
+	output->normal = -n;
+	return true;
+}
+
 bool b3CapsuleShape::RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf) const 
 {
 	b3Vec3 A = input.p1;

src/bounce/dynamics/shapes/hull_shape.cpp

@@ -142,21 +142,62 @@ void b3HullShape::ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const
 	aabb->Extend(m_radius);
 }
 
-bool b3HullShape::TestPoint(const b3Vec3& point, const b3Transform& xf) const
+bool b3HullShape::TestSphere(const b3Sphere& sphere, const b3Transform& xf) const
 {
-	// Put the point into the hull's frame of reference.
+	b3Vec3 support = b3MulT(xf, sphere.vertex);
-	b3Vec3 p = b3MulT(xf, point);
+	float32 radius = m_radius + sphere.radius;
+
 	for (u32 i = 0; i < m_hull->faceCount; ++i)
 	{
-		float32 d = b3Distance(p, m_hull->planes[i]);
+		b3Plane plane = m_hull->GetPlane(i);
-		if (d > m_radius)
+		float32 separation = b3Distance(support, plane);
+
+		if (separation > radius)
 		{
 			return false;
 		}
 	}
+
 	return true;
 }
 
+bool b3HullShape::TestSphere(b3TestSphereOutput* output, const b3Sphere& sphere, const b3Transform& xf) const
+{
+	// Perform computations in the local space of the first hull.
+	b3Vec3 support = b3MulT(xf, sphere.vertex);
+	float32 radius = m_radius + sphere.radius;
+	
+	u32 maxIndex = ~0;
+	float32 maxSeparation = -B3_MAX_FLOAT;
+
+	for (u32 i = 0; i < m_hull->faceCount; ++i)
+	{
+		b3Plane plane = m_hull->GetPlane(i);
+		float32 separation = b3Distance(support, plane);
+
+		if (separation > radius)
+		{
+			return false;
+		}
+
+		if (separation > maxSeparation)
+		{
+			maxIndex = i;
+			maxSeparation = separation;
+		}
+	}
+
+	if (maxIndex != ~0)
+	{
+		output->separation = maxSeparation;
+		output->normal = b3Mul(xf.rotation, m_hull->GetPlane(maxIndex).normal);
+		return true;
+	}
+	
+	B3_ASSERT(false);
+	return false;
+}
+
 bool b3HullShape::RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf) const
 {
 	u32 planeCount = m_hull->faceCount;

src/bounce/dynamics/shapes/mesh_shape.cpp

@@ -73,9 +73,17 @@ void b3MeshShape::ComputeAABB(b3AABB3* output, const b3Transform& xf, u32 index)
 	output->Extend(m_radius);
 }
 
-bool b3MeshShape::TestPoint(const b3Vec3& point, const b3Transform& xf) const
+bool b3MeshShape::TestSphere(const b3Sphere& sphere, const b3Transform& xf) const
 {
-	B3_NOT_USED(point);
+	B3_NOT_USED(sphere);
+	B3_NOT_USED(xf);
+	return false;
+}
+
+bool b3MeshShape::TestSphere(b3TestSphereOutput* output, const b3Sphere& sphere, const b3Transform& xf) const
+{
+	B3_NOT_USED(output);
+	B3_NOT_USED(sphere);
 	B3_NOT_USED(xf);
 	return false;
 }

src/bounce/dynamics/shapes/shape.cpp

@@ -29,6 +29,18 @@
 #include <bounce/collision/shapes/hull.h>
 #include <bounce/collision/shapes/mesh.h>
 
+b3Shape::b3Shape() 
+{
+	m_density = 0.0f;
+	m_friction = 0.0f;
+	m_restitution = 0.0f;
+	
+	m_isSensor = false;
+	m_userData = nullptr;
+
+	m_body = nullptr;
+}
+
 void b3Shape::SetSensor(bool flag)
 {
 	if (flag != m_isSensor)

src/bounce/dynamics/shapes/sphere_shape.cpp

@@ -58,15 +58,41 @@ void b3SphereShape::ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const
 	aabb->m_upper = center + r;
 }
 
-bool b3SphereShape::TestPoint(const b3Vec3& point, const b3Transform& xf) const
+bool b3SphereShape::TestSphere(const b3Sphere& sphere, const b3Transform& xf) const
 {
 	b3Vec3 center = b3Mul(xf, m_center);
-	float32 rr = m_radius * m_radius;
+	float32 radius = m_radius + sphere.radius;
-	b3Vec3 d = point - center;
+	float32 rr = radius * radius;
+	b3Vec3 d = sphere.vertex - center;
 	float32 dd = b3Dot(d, d);
 	return dd <= rr;
 }
 
+bool b3SphereShape::TestSphere(b3TestSphereOutput* output, const b3Sphere& sphere, const b3Transform& xf) const
+{
+	b3Vec3 center = b3Mul(xf, m_center);
+	float32 radius = m_radius + sphere.radius;
+	float32 rr = radius * radius;
+	b3Vec3 d = sphere.vertex - center;
+	float32 dd = b3Dot(d, d);
+	
+	if (dd <= rr)
+	{
+		float32 d_len = b3Sqrt(dd);
+
+		output->separation = d_len - radius;
+		output->normal.Set(0.0f, 1.0, 0.0f);
+		if (d_len > B3_EPSILON)
+		{
+			output->normal = d / d_len;
+		}
+
+		return true;
+	}
+
+	return false;
+}
+
 bool b3SphereShape::RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf) const
 {
 	// dot(x - c, x - c) - r^2 = 0