Fixed bug in soft body vs tapered capsule (#876)

The calculations were slightly off causing a normal on the top or bottom sphere to be returned while the tapered part was actually closest.

Docs/ReleaseNotes.md

@@ -24,6 +24,7 @@ For breaking API changes see [this document](https://github.com/jrouwe/JoltPhysi
 * Multithreading the SetupVelocityConstraints job. This was causing a bottleneck in the case that there are a lot of constraints but very few possible collisions.
 
 ### Bug fixes
+* Fixed bug in soft body vs tapered capsule. The calculations were slightly off causing a normal on the top or bottom sphere to be returned while the tapered part was actually closest.
 * Sensors will no longer use speculative contacts, so will no longer report contacts before an actual contact is detected.
 * Hinge limit constraint forces were clamped wrongly when the hinge was exactly at the minimum limit, making it harder to push the hinge towards the maximum limit.
 * Fixed bug when a body with limited DOFs collides with static. If the resulting contact had an infinite effective mass, we would divide by zero and crash.

Jolt/Physics/Collision/Shape/TaperedCapsuleShape.cpp

@@ -311,8 +311,8 @@ void TaperedCapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransfo
 	float scale_y = abs_scale.GetY();
 	float scale_xz = abs_scale.GetX();
 	Vec3 scale_y_flip(1, Sign(inScale.GetY()), 1);
-	float scaled_top_center = scale_y * mTopCenter;
-	float scaled_bottom_center = scale_y * mBottomCenter;
+	Vec3 scaled_top_center(0, scale_y * mTopCenter, 0);
+	Vec3 scaled_bottom_center(0, scale_y * mBottomCenter, 0);
 	float scaled_top_radius = scale_xz * mTopRadius;
 	float scaled_bottom_radius = scale_xz * mBottomRadius;
 
@@ -321,29 +321,39 @@ void TaperedCapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransfo
 		{
 			Vec3 local_pos = scale_y_flip * (inverse_transform * v->mPosition);
 
-			// See comments at TaperedCapsuleShape::GetSurfaceNormal for rationale behind the math
 			Vec3 position, normal;
-			if (local_pos.GetY() > scaled_top_center + mSinAlpha * scaled_top_radius)
+
+			// If the vertex is inside the cone starting at the top center pointing along the y-axis with angle PI/2 - alpha then the closest point is on the top sphere
+			// This corresponds to: Dot(y-axis, (local_pos - top_center) / |local_pos - top_center|) >= cos(PI/2 - alpha)
+			// <=> (local_pos - top_center).y >= sin(alpha) * |local_pos - top_center|
+			Vec3 top_center_to_local_pos = local_pos - scaled_top_center;
+			float top_center_to_local_pos_len = top_center_to_local_pos.Length();
+			if (top_center_to_local_pos.GetY() >= mSinAlpha * top_center_to_local_pos_len)
 			{
 				// Top sphere
-				Vec3 top = Vec3(0, scaled_top_center, 0);
-				normal = (local_pos - top).NormalizedOr(Vec3::sAxisY());
-				position = top + scaled_top_radius * normal;
-			}
-			else if (local_pos.GetY() < scaled_bottom_center + mSinAlpha * scaled_bottom_radius)
-			{
-				// Bottom sphere
-				Vec3 bottom(0, scaled_bottom_center, 0);
-				normal = (local_pos - bottom).NormalizedOr(-Vec3::sAxisY());
-				position = bottom + scaled_bottom_radius * normal;
+				normal = top_center_to_local_pos_len != 0.0f? top_center_to_local_pos / top_center_to_local_pos_len : Vec3::sAxisY();
+				position = scaled_top_center + scaled_top_radius * normal;
 			}
 			else
 			{
-				// Tapered cylinder
-				normal = Vec3(local_pos.GetX(), 0, local_pos.GetZ()).NormalizedOr(Vec3::sAxisX());
-				normal.SetY(mTanAlpha);
-				normal = normal.NormalizedOr(Vec3::sAxisX());
-				position = Vec3(0, scaled_bottom_center, 0) + scaled_bottom_radius * normal;
+				// If the vertex is outside the cone starting at the bottom center pointing along the y-axis with angle PI/2 - alpha then the closest point is on the bottom sphere
+				// This corresponds to: Dot(y-axis, (local_pos - bottom_center) / |local_pos - bottom_center|) <= cos(PI/2 - alpha)
+				// <=> (local_pos - bottom_center).y <= sin(alpha) * |local_pos - bottom_center|
+				Vec3 bottom_center_to_local_pos = local_pos - scaled_bottom_center;
+				float bottom_center_to_local_pos_len = bottom_center_to_local_pos.Length();
+				if (bottom_center_to_local_pos.GetY() <= mSinAlpha * bottom_center_to_local_pos_len)
+				{
+					// Bottom sphere
+					normal = bottom_center_to_local_pos_len != 0.0f? bottom_center_to_local_pos / bottom_center_to_local_pos_len : -Vec3::sAxisY();
+				}
+				else
+				{
+					// Tapered cylinder
+					normal = Vec3(local_pos.GetX(), 0, local_pos.GetZ()).NormalizedOr(Vec3::sAxisX());
+					normal.SetY(mTanAlpha);
+					normal = normal.NormalizedOr(Vec3::sAxisX());
+				}
+				position = scaled_bottom_center + scaled_bottom_radius * normal;
 			}
 
 			Plane plane = Plane::sFromPointAndNormal(position, normal);

Samples/SamplesApp.cpp

@@ -1456,7 +1456,7 @@ bool SamplesApp::CastProbe(float inProbeLength, float &outFraction, RVec3 &outPo
 					SphereShape point_sphere(1.0e-6f);
 					point_sphere.SetEmbedded();
 					CollideShapeSettings settings;
-					settings.mMaxSeparationDistance = max_distance;
+					settings.mMaxSeparationDistance = sqrt(3.0f) * max_distance; // Box is extended in all directions by max_distance
 					ClosestHitCollisionCollector<CollideShapeCollector> collide_shape_collector;
 					ts.CollideShape(&point_sphere, Vec3::sReplicate(1.0f), RMat44::sTranslation(start + vertex.mPosition), settings, start, collide_shape_collector);
 					if (collide_shape_collector.HadHit())
@@ -1474,7 +1474,7 @@ bool SamplesApp::CastProbe(float inProbeLength, float &outFraction, RVec3 &outPo
 			// Draw collision plane
 			if (vertex.mCollidingShapeIndex != -1)
 			{
-				RVec3 plane_point = start + closest_point - vertex.mCollisionPlane.GetNormal() * vertex.mCollisionPlane.SignedDistance(closest_point);
+				RVec3 plane_point = start + vertex.mPosition - vertex.mCollisionPlane.GetNormal() * vertex.mCollisionPlane.SignedDistance(vertex.mPosition);
 				mDebugRenderer->DrawPlane(plane_point, vertex.mCollisionPlane.GetNormal(), Color::sGreen, 2.0f);
 
 				if (abs(closest_point_penetration - vertex.mLargestPenetration) > 0.001f)