Added TaperedCapsuleShape::CollidePoint (#1243)

Jolt/Physics/Collision/Shape/CylinderShape.cpp

@@ -32,17 +32,16 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(CylinderShapeSettings)
 }
 
 // Approximation of top face with 8 vertices
-static const float cSin45 = 0.70710678118654752440084436210485f;
-static const Vec3 cTopFace[] =
+static const Vec3 cCylinderTopFace[] =
 {
-	Vec3(0.0f,		1.0f,	1.0f),
-	Vec3(cSin45,	1.0f,	cSin45),
-	Vec3(1.0f,		1.0f,	0.0f),
-	Vec3(cSin45,	1.0f,	-cSin45),
-	Vec3(-0.0f,		1.0f,	-1.0f),
-	Vec3(-cSin45,	1.0f,	-cSin45),
-	Vec3(-1.0f,		1.0f,	0.0f),
-	Vec3(-cSin45,	1.0f,	cSin45)
+	Vec3(0.0f,			1.0f,	1.0f),
+	Vec3(0.707106769f,	1.0f,	0.707106769f),
+	Vec3(1.0f,			1.0f,	0.0f),
+	Vec3(0.707106769f,	1.0f,	-0.707106769f),
+	Vec3(-0.0f,			1.0f,	-1.0f),
+	Vec3(-0.707106769f,	1.0f,	-0.707106769f),
+	Vec3(-1.0f,			1.0f,	0.0f),
+	Vec3(-0.707106769f,	1.0f,	0.707106769f)
 };
 
 static const StaticArray<Vec3, 96> sUnitCylinderTriangles = []() {
@@ -50,13 +49,13 @@ static const StaticArray<Vec3, 96> sUnitCylinderTriangles = []() {
 
 	const Vec3 bottom_offset(0.0f, -2.0f, 0.0f);
 
-	int num_verts = sizeof(cTopFace) / sizeof(Vec3);
+	int num_verts = sizeof(cCylinderTopFace) / sizeof(Vec3);
 	for (int i = 0; i < num_verts; ++i)
 	{
-		Vec3 t1 = cTopFace[i];
-		Vec3 t2 = cTopFace[(i + 1) % num_verts];
-		Vec3 b1 = cTopFace[i] + bottom_offset;
-		Vec3 b2 = cTopFace[(i + 1) % num_verts] + bottom_offset;
+		Vec3 t1 = cCylinderTopFace[i];
+		Vec3 t2 = cCylinderTopFace[(i + 1) % num_verts];
+		Vec3 b1 = cCylinderTopFace[i] + bottom_offset;
+		Vec3 b2 = cCylinderTopFace[(i + 1) % num_verts] + bottom_offset;
 
 		// Top
 		verts.emplace_back(0.0f, 1.0f, 0.0f);
@@ -218,7 +217,7 @@ void CylinderShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg in
 		// Hitting top or bottom
 		Vec3 multiplier = y < 0.0f? Vec3(scaled_radius, scaled_half_height, scaled_radius) : Vec3(-scaled_radius, -scaled_half_height, scaled_radius);
 		Mat44 transform = inCenterOfMassTransform.PreScaled(multiplier);
-		for (const Vec3 &v : cTopFace)
+		for (const Vec3 &v : cCylinderTopFace)
 			outVertices.push_back(transform * v);
 	}
 }

Jolt/Physics/Collision/Shape/TaperedCylinderShape.cpp

@@ -7,6 +7,8 @@
 #include <Jolt/Physics/Collision/Shape/TaperedCylinderShape.h>
 #include <Jolt/Physics/Collision/Shape/CylinderShape.h>
 #include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+#include <Jolt/Physics/Collision/CollidePointResult.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
 #include <Jolt/Physics/SoftBody/SoftBodyVertex.h>
 #include <Jolt/ObjectStream/TypeDeclarations.h>
 #include <Jolt/Core/StreamIn.h>
@@ -17,17 +19,17 @@
 
 JPH_NAMESPACE_BEGIN
 
-// Approximation of a face of the tapered capsule
-static const Vec3 cTaperedCapsuleFace[] =
+// Approximation of a face of the tapered cylinder
+static const Vec3 cTaperedCylinderFace[] =
 {
 	Vec3(0.0f,			0.0f,	1.0f),
-	Vec3(0.7071067f,	0.0f,	0.7071067f),
+	Vec3(0.707106769f,	0.0f,	0.707106769f),
 	Vec3(1.0f,			0.0f,	0.0f),
-	Vec3(0.7071067f,	0.0f,	-0.7071067f),
+	Vec3(0.707106769f,	0.0f,	-0.707106769f),
 	Vec3(-0.0f,			0.0f,	-1.0f),
-	Vec3(-0.7071067f,	0.0f,	-0.7071067f),
+	Vec3(-0.707106769f,	0.0f,	-0.707106769f),
 	Vec3(-1.0f,			0.0f,	0.0f),
-	Vec3(-0.7071067f,	0.0f,	0.7071067f)
+	Vec3(-0.707106769f,	0.0f,	0.707106769f)
 };
 
 JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(TaperedCylinderShapeSettings)
@@ -269,7 +271,7 @@ void TaperedCylinderShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec
 		if (top_radius > cMinRadius)
 		{
 			Vec3 top_3d(0, top, 0);
-			for (Vec3 v : cTaperedCapsuleFace)
+			for (Vec3 v : cTaperedCylinderFace)
 				outVertices.push_back(inCenterOfMassTransform * (top_radius * v + top_3d));
 		}
 	}
@@ -279,7 +281,7 @@ void TaperedCylinderShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec
 		if (bottom_radius > cMinRadius)
 		{
 			Vec3 bottom_3d(0, bottom, 0);
-			for (const Vec3 *v = cTaperedCapsuleFace + std::size(cTaperedCapsuleFace) - 1; v >= cTaperedCapsuleFace; --v)
+			for (const Vec3 *v = cTaperedCylinderFace + std::size(cTaperedCylinderFace) - 1; v >= cTaperedCylinderFace; --v)
 				outVertices.push_back(inCenterOfMassTransform * (bottom_radius * *v + bottom_3d));
 		}
 	}
@@ -369,6 +371,18 @@ AABox TaperedCylinderShape::GetLocalBounds() const
 	return AABox(Vec3(-max_radius, mBottom, -max_radius), Vec3(max_radius, mTop, max_radius));
 }
 
+void TaperedCylinderShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	// Check if the point is in the tapered cylinder
+	if (inPoint.GetY() >= mBottom && inPoint.GetY() <= mTop // Within height
+		&& Square(inPoint.GetX()) + Square(inPoint.GetZ()) <= Square(mBottomRadius + (inPoint.GetY() - mBottom) * (mTopRadius - mBottomRadius) / (mTop - mBottom))) // Within the radius
+		ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
+}
+
 void TaperedCylinderShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
 {
 	JPH_ASSERT(IsValidScale(inScale));
@@ -525,7 +539,7 @@ void TaperedCylinderShape::GetTrianglesStart(GetTrianglesContext &ioContext, con
 
 int TaperedCylinderShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const
 {
-	constexpr int cNumVertices = int(std::size(cTaperedCapsuleFace));
+	constexpr int cNumVertices = int(std::size(cTaperedCylinderFace));
 
 	static_assert(cGetTrianglesMinTrianglesRequested >= 2 * cNumVertices);
 	JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested);
@@ -538,10 +552,10 @@ int TaperedCylinderShape::GetTrianglesNext(GetTrianglesContext &ioContext, int i
 	Vec3 top_3d(0, mTop, 0);
 	if ((context.mProcessed & 0b001) == 0)
 	{
-		Vec3 v0 = context.mTransform * (top_3d + mTopRadius * cTaperedCapsuleFace[0]);
-		Vec3 v1 = context.mTransform * (top_3d + mTopRadius * cTaperedCapsuleFace[1]);
+		Vec3 v0 = context.mTransform * (top_3d + mTopRadius * cTaperedCylinderFace[0]);
+		Vec3 v1 = context.mTransform * (top_3d + mTopRadius * cTaperedCylinderFace[1]);
 
-		for (const Vec3 *v = cTaperedCapsuleFace + 2, *v_end = cTaperedCapsuleFace + cNumVertices; v < v_end; ++v)
+		for (const Vec3 *v = cTaperedCylinderFace + 2, *v_end = cTaperedCylinderFace + cNumVertices; v < v_end; ++v)
 		{
 			Vec3 v2 = context.mTransform * (top_3d + mTopRadius * *v);
 
@@ -561,10 +575,10 @@ int TaperedCylinderShape::GetTrianglesNext(GetTrianglesContext &ioContext, int i
 	if ((context.mProcessed & 0b010) == 0
 		&& total_num_triangles + cNumVertices - 2 < inMaxTrianglesRequested)
 	{
-		Vec3 v0 = context.mTransform * (bottom_3d + mBottomRadius * cTaperedCapsuleFace[0]);
-		Vec3 v1 = context.mTransform * (bottom_3d + mBottomRadius * cTaperedCapsuleFace[1]);
+		Vec3 v0 = context.mTransform * (bottom_3d + mBottomRadius * cTaperedCylinderFace[0]);
+		Vec3 v1 = context.mTransform * (bottom_3d + mBottomRadius * cTaperedCylinderFace[1]);
 
-		for (const Vec3 *v = cTaperedCapsuleFace + 2, *v_end = cTaperedCapsuleFace + cNumVertices; v < v_end; ++v)
+		for (const Vec3 *v = cTaperedCylinderFace + 2, *v_end = cTaperedCylinderFace + cNumVertices; v < v_end; ++v)
 		{
 			Vec3 v2 = context.mTransform * (bottom_3d + mBottomRadius * *v);
 
@@ -583,10 +597,10 @@ int TaperedCylinderShape::GetTrianglesNext(GetTrianglesContext &ioContext, int i
 	if ((context.mProcessed & 0b100) == 0
 		&& total_num_triangles + 2 * cNumVertices < inMaxTrianglesRequested)
 	{
-		Vec3 v0t = context.mTransform * (top_3d + mTopRadius * cTaperedCapsuleFace[cNumVertices - 1]);
-		Vec3 v0b = context.mTransform * (bottom_3d + mBottomRadius * cTaperedCapsuleFace[cNumVertices - 1]);
+		Vec3 v0t = context.mTransform * (top_3d + mTopRadius * cTaperedCylinderFace[cNumVertices - 1]);
+		Vec3 v0b = context.mTransform * (bottom_3d + mBottomRadius * cTaperedCylinderFace[cNumVertices - 1]);
 
-		for (const Vec3 *v = cTaperedCapsuleFace, *v_end = cTaperedCapsuleFace + cNumVertices; v < v_end; ++v)
+		for (const Vec3 *v = cTaperedCylinderFace, *v_end = cTaperedCylinderFace + cNumVertices; v < v_end; ++v)
 		{
 			Vec3 v1t = context.mTransform * (top_3d + mTopRadius * *v);
 			v0t.StoreFloat3(outTriangleVertices++);

Jolt/Physics/Collision/Shape/TaperedCylinderShape.h

@@ -60,6 +60,9 @@ public:
 	// See ConvexShape::GetSupportFunction
 	virtual const Support *	GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override;
 
+	// See: Shape::CollidePoint
+	virtual void			CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
 	// See: Shape::CollideSoftBodyVertices
 	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
 

UnitTests/Physics/TaperedCylinderShapeTests.cpp

@@ -5,6 +5,8 @@
 #include "UnitTestFramework.h"
 #include "PhysicsTestContext.h"
 #include <Jolt/Physics/Collision/Shape/TaperedCylinderShape.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Jolt/Physics/Collision/CollidePointResult.h>
 
 TEST_SUITE("TaperedCylinderShapeTests")
 {
@@ -48,4 +50,49 @@ TEST_SUITE("TaperedCylinderShapeTests")
 		CHECK_APPROX_EQUAL(expected_inertia_yy, m1.mInertia(1, 1), 1.0e-3f);
 		CHECK_APPROX_EQUAL(expected_inertia_xx, m1.mInertia(2, 2), 1.0e-3f);
 	}
+
+	TEST_CASE("TestCollidePoint")
+	{
+		const float cTopRadius = 3.0f;
+		const float cBottomRadius = 5.0f;
+		const float cHalfHeight = 3.5f;
+
+		RefConst<Shape> shape = TaperedCylinderShapeSettings(cHalfHeight, cTopRadius, cBottomRadius).Create().Get();
+
+		auto test_inside = [shape](Vec3Arg inPoint)
+		{
+			AllHitCollisionCollector<CollidePointCollector> collector;
+			shape->CollidePoint(inPoint - shape->GetCenterOfMass(), SubShapeIDCreator(), collector);
+			CHECK(collector.mHits.size() == 1);
+		};
+
+		auto test_outside = [shape](Vec3Arg inPoint)
+		{
+			AllHitCollisionCollector<CollidePointCollector> collector;
+			shape->CollidePoint(inPoint - shape->GetCenterOfMass(), SubShapeIDCreator(), collector);
+			CHECK(collector.mHits.size() == 0);
+		};
+
+		constexpr float cEpsilon = 1.0e-3f;
+
+		test_inside(Vec3::sZero());
+
+		// Top plane
+		test_inside(Vec3(0, cHalfHeight - cEpsilon, 0));
+		test_outside(Vec3(0, cHalfHeight + cEpsilon, 0));
+
+		// Bottom plane
+		test_inside(Vec3(0, -cHalfHeight + cEpsilon, 0));
+		test_outside(Vec3(0, -cHalfHeight - cEpsilon, 0));
+
+		// COM plane
+		test_inside(Vec3(0.5f * (cTopRadius + cBottomRadius) - cEpsilon, 0, 0));
+		test_outside(Vec3(0.5f * (cTopRadius + cBottomRadius) + cEpsilon, 0, 0));
+
+		// At quarter h above COM plane
+		float h = 0.5f * cHalfHeight;
+		float r = cBottomRadius + (cTopRadius - cBottomRadius) * (h + cHalfHeight) / (2.0f * cHalfHeight);
+		test_inside(Vec3(0, h, r - cEpsilon));
+		test_outside(Vec3(0, h, r + cEpsilon));
+	}
 }