Improved stability of cylinder contacts (#1540)

* When the inDirection is more than 5 degrees from vertical, align the vertices so that 1 of the vertices points towards inDirection in the XZ plane. This ensures that we always have a vertex towards max penetration depth.
* Removed duplicate 'if' in ManifoldBetweenTwoFaces

Jolt/Physics/Collision/ManifoldBetweenTwoFaces.cpp

@@ -136,6 +136,8 @@ void PruneContactPoints(Vec3Arg inPenetrationAxis, ContactPoints &ioContactPoint
 
 void ManifoldBetweenTwoFaces(Vec3Arg inContactPoint1, Vec3Arg inContactPoint2, Vec3Arg inPenetrationAxis, float inMaxContactDistance, const ConvexShape::SupportingFace &inShape1Face, const ConvexShape::SupportingFace &inShape2Face, ContactPoints &outContactPoints1, ContactPoints &outContactPoints2 JPH_IF_DEBUG_RENDERER(, RVec3Arg inCenterOfMass))
 {
+	JPH_ASSERT(inMaxContactDistance > 0.0f);
+
 #ifdef JPH_DEBUG_RENDERER
 	if (ContactConstraintManager::sDrawContactPoint)
 	{
@@ -195,8 +197,8 @@ void ManifoldBetweenTwoFaces(Vec3Arg inContactPoint1, Vec3Arg inContactPoint2, V
 				// distance = -|penetration_axis| * (p2 - plane_origin) . plane_normal / penetration_axis . plane_normal
 				float distance = (p2 - plane_origin).Dot(plane_normal) / penetration_axis_dot_plane_normal; // note left out -|penetration_axis| term
 
-				// If the point is behind or less then inMaxContactDistance in front of the plane of face 2, add it as a contact point
-				if (distance <= 0.0f || distance * penetration_axis_len < inMaxContactDistance)
+				// If the point is less than inMaxContactDistance in front of the plane of face 2, add it as a contact point
+				if (distance * penetration_axis_len < inMaxContactDistance)
 				{
 					Vec3 p1 = p2 - distance * inPenetrationAxis;
 					outContactPoints1.push_back(p1);

Jolt/Physics/Collision/Shape/CylinderShape.cpp

@@ -201,9 +201,10 @@ void CylinderShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg in
 
 	float x = inDirection.GetX(), y = inDirection.GetY(), z = inDirection.GetZ();
 	float xz_sq = Square(x) + Square(z);
+	float y_sq = Square(y);
 
 	// Check which component is bigger
-	if (xz_sq > Square(y))
+	if (xz_sq > y_sq)
 	{
 		// Hitting side
 		float f = -scaled_radius / sqrt(xz_sq);
@@ -215,8 +216,21 @@ void CylinderShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg in
 	else
 	{
 		// Hitting top or bottom
+
+		// When the inDirection is more than 5 degrees from vertical, align the vertices so that 1 of the vertices
+		// points towards inDirection in the XZ plane. This ensures that we always have a vertex towards max penetration depth.
+		Mat44 transform = inCenterOfMassTransform;
+		if (xz_sq > 0.00765427f * y_sq)
+		{
+			Vec4 base_x = Vec4(x, 0, z, 0) / sqrt(xz_sq);
+			Vec4 base_z = base_x.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W>() * Vec4(-1, 0, 1, 0);
+			transform = transform * Mat44(base_x, Vec4(0, 1, 0, 0), base_z, Vec4(0, 0, 0, 1));
+		}
+
+		// Adjust for scale and height
 		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);
+		transform = transform.PreScaled(multiplier);
+
 		for (const Vec3 &v : cCylinderTopFace)
 			outVertices.push_back(transform * v);
 	}

Jolt/Physics/Collision/Shape/TaperedCylinderShape.cpp

@@ -265,24 +265,40 @@ void TaperedCylinderShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec
 		outVertices.push_back(inCenterOfMassTransform * (normal_xz * top_radius + Vec3(0, top, 0)));
 		outVertices.push_back(inCenterOfMassTransform * (normal_xz * bottom_radius + Vec3(0, bottom, 0)));
 	}
-	else if (inDirection.GetY() < 0.0f)
+	else
 	{
-		// Top of the cylinder
-		if (top_radius > cMinRadius)
+		// When the inDirection is more than 5 degrees from vertical, align the vertices so that 1 of the vertices
+		// points towards inDirection in the XZ plane. This ensures that we always have a vertex towards max penetration depth.
+		Mat44 transform = inCenterOfMassTransform;
+		Vec4 base_x = Vec4(inDirection.GetX(), 0, inDirection.GetZ(), 0);
+		float xz_sq = base_x.LengthSq();
+		float y_sq = Square(inDirection.GetY());
+		if (xz_sq > 0.00765427f * y_sq)
 		{
-			Vec3 top_3d(0, top, 0);
-			for (Vec3 v : cTaperedCylinderFace)
-				outVertices.push_back(inCenterOfMassTransform * (top_radius * v + top_3d));
+			base_x /= sqrt(xz_sq);
+			Vec4 base_z = base_x.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W>() * Vec4(-1, 0, 1, 0);
+			transform = transform * Mat44(base_x, Vec4(0, 1, 0, 0), base_z, Vec4(0, 0, 0, 1));
 		}
-	}
-	else
-	{
-		// Bottom of the cylinder
-		if (bottom_radius > cMinRadius)
+
+		if (inDirection.GetY() < 0.0f)
+		{
+			// Top of the cylinder
+			if (top_radius > cMinRadius)
+			{
+				Vec3 top_3d(0, top, 0);
+				for (Vec3 v : cTaperedCylinderFace)
+					outVertices.push_back(transform * (top_radius * v + top_3d));
+			}
+		}
+		else
 		{
-			Vec3 bottom_3d(0, bottom, 0);
-			for (const Vec3 *v = cTaperedCylinderFace + std::size(cTaperedCylinderFace) - 1; v >= cTaperedCylinderFace; --v)
-				outVertices.push_back(inCenterOfMassTransform * (bottom_radius * *v + bottom_3d));
+			// Bottom of the cylinder
+			if (bottom_radius > cMinRadius)
+			{
+				Vec3 bottom_3d(0, bottom, 0);
+				for (const Vec3 *v = cTaperedCylinderFace + std::size(cTaperedCylinderFace) - 1; v >= cTaperedCylinderFace; --v)
+					outVertices.push_back(transform * (bottom_radius * *v + bottom_3d));
+			}
 		}
 	}
 }