Fixed soft body collision vs scaled shapes (#655)

* None of the shapes dealt properly with scaled shapes. Now the scale is passed separately just like the other collision functions and each shape handles the scale explicitly.
* Added option to shoot a soft body cube
* Fixed bugs in the handling of cylinder and convex hull shapes didn't return the proper normal when an edge was closest to the soft body vertex

Jolt/Physics/Collision/Shape/BoxShape.cpp

@@ -224,10 +224,10 @@ void BoxShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShape
 		ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
 }
 
-void BoxShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+void BoxShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, Array<SoftBodyVertex> &ioVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
 {
 	Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation();
-	Vec3 half_extent = mHalfExtent;
+	Vec3 half_extent = inScale.Abs() * mHalfExtent;
 
 	for (SoftBodyVertex &v : ioVertices)
 		if (v.mInvMass > 0.0f)

Jolt/Physics/Collision/Shape/BoxShape.h

@@ -77,7 +77,7 @@ public:
 	virtual void			CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
 	// See: Shape::ColideSoftBodyVertices
-	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
 
 	// See Shape::GetTrianglesStart
 	virtual void			GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;

Jolt/Physics/Collision/Shape/CapsuleShape.cpp

@@ -322,29 +322,36 @@ void CapsuleShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubS
 		ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
 }
 
-void CapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+void CapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, Array<SoftBodyVertex> &ioVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
 {
+	JPH_ASSERT(IsValidScale(inScale));
+
 	Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation();
 
+	// Get scaled capsule
+	float scale = abs(inScale.GetX());
+	float half_height_of_cylinder = scale * mHalfHeightOfCylinder;
+	float radius = scale * mRadius;
+
 	for (SoftBodyVertex &v : ioVertices)
 		if (v.mInvMass > 0.0f)
 		{
 			// Calculate penetration
 			Vec3 local_pos = inverse_transform * v.mPosition;
-			if (abs(local_pos.GetY()) <= mHalfHeightOfCylinder)
+			if (abs(local_pos.GetY()) <= half_height_of_cylinder)
 			{
 				// Near cylinder
 				Vec3 normal = local_pos;
 				normal.SetY(0.0f);
 				float normal_length = normal.Length();
-				float penetration = mRadius - normal_length;
+				float penetration = radius - normal_length;
 				if (penetration > v.mLargestPenetration)
 				{
 					v.mLargestPenetration = penetration;
 
 					// Calculate contact point and normal
 					normal = normal_length > 0.0f? normal / normal_length : Vec3::sAxisX();
-					Vec3 point = mRadius * normal;
+					Vec3 point = radius * normal;
 
 					// Store collision
 					v.mCollisionPlane = Plane::sFromPointAndNormal(point, normal).GetTransformed(inCenterOfMassTransform);
@@ -354,17 +361,17 @@ void CapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Arr
 			else
 			{
 				// Near cap
-				Vec3 center = Vec3(0, Sign(local_pos.GetY()) * mHalfHeightOfCylinder, 0);
+				Vec3 center = Vec3(0, Sign(local_pos.GetY()) * half_height_of_cylinder, 0);
 				Vec3 delta = local_pos - center;
 				float distance = delta.Length();
-				float penetration = mRadius - distance;
+				float penetration = radius - distance;
 				if (penetration > v.mLargestPenetration)
 				{
 					v.mLargestPenetration = penetration;
 
 					// Calculate contact point and normal
 					Vec3 normal = delta / distance;
-					Vec3 point = center + mRadius * normal;
+					Vec3 point = center + radius * normal;
 
 					// Store collision
 					v.mCollisionPlane = Plane::sFromPointAndNormal(point, normal).GetTransformed(inCenterOfMassTransform);

Jolt/Physics/Collision/Shape/CapsuleShape.h

@@ -86,7 +86,7 @@ public:
 	virtual void			CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
 	// See: Shape::ColideSoftBodyVertices
-	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
 
 	// See Shape::TransformShape
 	virtual void			TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override;

Jolt/Physics/Collision/Shape/CompoundShape.cpp

@@ -246,10 +246,10 @@ void CompoundShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg i
 }
 #endif // JPH_DEBUG_RENDERER
 
-void CompoundShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+void CompoundShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
 {
 	for (const SubShape &shape : mSubShapes)
-		shape.mShape->CollideSoftBodyVertices(inCenterOfMassTransform * Mat44::sRotationTranslation(shape.GetRotation(), shape.GetPositionCOM()), ioVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex);
+		shape.mShape->CollideSoftBodyVertices(inCenterOfMassTransform * Mat44::sRotationTranslation(shape.GetRotation(), shape.GetPositionCOM()), shape.TransformScale(inScale), ioVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex);
 }
 
 void CompoundShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const

Jolt/Physics/Collision/Shape/CompoundShape.h

@@ -106,7 +106,7 @@ public:
 #endif // JPH_DEBUG_RENDERER
 
 	// See: Shape::ColideSoftBodyVertices
-	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
 
 	// See Shape::TransformShape
 	virtual void					TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override;

Jolt/Physics/Collision/Shape/ConvexHullShape.cpp

@@ -1058,10 +1058,14 @@ void ConvexHullShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inS
 	ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
 }
 
-void ConvexHullShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+void ConvexHullShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, Array<SoftBodyVertex> &ioVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
 {
 	Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation();
 
+	Vec3 inv_scale = inScale.Reciprocal();
+	bool is_not_scaled = ScaleHelpers::IsNotScaled(inScale);
+	float scale_flip = ScaleHelpers::IsInsideOut(inScale)? -1.0f : 1.0f;
+
 	for (SoftBodyVertex &v : ioVertices)
 		if (v.mInvMass > 0.0f)
 		{
@@ -1069,24 +1073,51 @@ void ConvexHullShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform,
 
 			// Find most facing plane
 			float max_distance = -FLT_MAX;
-			const Plane *max_plane = nullptr;
-			for (const Plane &p : mPlanes)
+			Vec3 max_plane_normal = Vec3::sZero();
+			uint max_plane_idx = 0;
+			if (is_not_scaled)
 			{
-				float distance = p.SignedDistance(local_pos);
-				if (distance > max_distance)
+				// Without scale, it is trivial to calculate the distance to the hull
+				for (const Plane &p : mPlanes)
 				{
-					max_distance = distance;
-					max_plane = &p;
+					float distance = p.SignedDistance(local_pos);
+					if (distance > max_distance)
+					{
+						max_distance = distance;
+						max_plane_normal = p.GetNormal();
+						max_plane_idx = uint(&p - mPlanes.data());
+					}
 				}
 			}
+			else
+			{
+				// When there's scale we need to calculate the planes first
+				for (uint i = 0; i < (uint)mPlanes.size(); ++i)
+				{
+					// Calculate plane normal and point by scaling the original plane
+					Vec3 plane_normal = (inv_scale * mPlanes[i].GetNormal()).Normalized();
+					Vec3 plane_point = inScale * mPoints[mVertexIdx[mFaces[i].mFirstVertex]].mPosition;
+
+					float distance = plane_normal.Dot(local_pos - plane_point);
+					if (distance > max_distance)
+					{
+						max_distance = distance;
+						max_plane_normal = plane_normal;
+						max_plane_idx = i;
+					}
+				}
+			}
+			bool is_outside = max_distance > 0.0f;
+
+			// Project point onto that plane
+			Vec3 closest_point = local_pos - max_distance * max_plane_normal;
 
 			// Check edges if we're outside the hull (when inside we know the closest face is also the closest point to the surface)
-			float closest_edge_distance = FLT_MAX;
-			Plane closest_plane = *max_plane;
-			if (max_distance <= 0.0f)
+			if (is_outside)
 			{
 				// Loop over edges
-				const Face &face = mFaces[int(max_plane - mPlanes.data())];
+				float closest_point_dist_sq = FLT_MAX;
+				const Face &face = mFaces[max_plane_idx];
 				for (const uint8 *v_start = &mVertexIdx[face.mFirstVertex], *v1 = v_start, *v_end = v_start + face.mNumVertices; v1 < v_end; ++v1)
 				{
 					// Find second point
@@ -1095,36 +1126,41 @@ void ConvexHullShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform,
 						v2 = v_start;
 
 					// Get edge points
-					Vec3 p1 = mPoints[*v1].mPosition;
-					Vec3 p2 = mPoints[*v2].mPosition;
+					Vec3 p1 = inScale * mPoints[*v1].mPosition;
+					Vec3 p2 = inScale * mPoints[*v2].mPosition;
 
 					// Check if the position is outside the edge (if not, the face will be closer)
-					Vec3 edge_normal = (p2 - p1).Cross(max_plane->GetNormal());
-					if (edge_normal.Dot(local_pos - p1) > 0.0f)
+					Vec3 edge_normal = (p2 - p1).Cross(max_plane_normal);
+					if (scale_flip * edge_normal.Dot(local_pos - p1) > 0.0f)
 					{
 						// Get closest point on edge
 						uint32 set;
 						Vec3 closest = ClosestPoint::GetClosestPointOnLine(p1 - local_pos, p2 - local_pos, set);
-						float distance = closest.Length();
-						if (distance < closest_edge_distance)
-						{
-							closest_edge_distance = distance;
-							Vec3 point = local_pos + closest;
-							Vec3 normal = (local_pos - point).NormalizedOr(max_plane->GetNormal());
-							closest_plane = Plane::sFromPointAndNormal(point, normal);
-						}
+						float distance_sq = closest.LengthSq();
+						if (distance_sq < closest_point_dist_sq)
+							closest_point = local_pos + closest;
 					}
 				}
 			}
 
-			// Closest point on edge
-			float penetration = -(closest_edge_distance != FLT_MAX? closest_edge_distance : max_distance);
+			// Check if this is the largest penetration
+			Vec3 normal = local_pos - closest_point;
+			float normal_length = normal.Length();
+			float penetration = normal_length;
+			if (is_outside)
+				penetration = -penetration;
+			else
+				normal = -normal;
 			if (penetration > v.mLargestPenetration)
 			{
 				v.mLargestPenetration = penetration;
 
+				// Calculate contact plane
+				normal = normal_length > 0.0f? normal / normal_length : max_plane_normal;
+				Plane plane = Plane::sFromPointAndNormal(closest_point, normal);
+
 				// Store collision
-				v.mCollisionPlane = closest_plane.GetTransformed(inCenterOfMassTransform);
+				v.mCollisionPlane = plane.GetTransformed(inCenterOfMassTransform);
 				v.mCollidingShapeIndex = inCollidingShapeIndex;
 			}
 		}

Jolt/Physics/Collision/Shape/ConvexHullShape.h

@@ -90,7 +90,7 @@ public:
 	virtual void			CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
 	// See: Shape::ColideSoftBodyVertices
-	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
 
 	// See Shape::GetTrianglesStart
 	virtual void			GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;

Jolt/Physics/Collision/Shape/CylinderShape.cpp

@@ -300,10 +300,17 @@ void CylinderShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSub
 		ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
 }
 
-void CylinderShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+void CylinderShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, Array<SoftBodyVertex> &ioVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
 {
+	JPH_ASSERT(IsValidScale(inScale));
+
 	Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation();
 
+	// Get scaled cylinder
+	Vec3 abs_scale = inScale.Abs();
+	float half_height = abs_scale.GetY() * mHalfHeight;
+	float radius = abs_scale.GetX() * mRadius;
+
 	for (SoftBodyVertex &v : ioVertices)
 		if (v.mInvMass > 0.0f)
 		{
@@ -313,29 +320,29 @@ void CylinderShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Ar
 			Vec3 side_normal = local_pos;
 			side_normal.SetY(0.0f);
 			float side_normal_length = side_normal.Length();
-			float side_penetration = mRadius - side_normal_length;
+			float side_penetration = radius - side_normal_length;
 
 			// Calculate penetration into top or bottom plane
-			float top_penetration = mHalfHeight - abs(local_pos.GetY());
+			float top_penetration = half_height - abs(local_pos.GetY());
 
 			Vec3 point, normal;
 			if (side_penetration < 0.0f && top_penetration < 0.0f)
 			{
 				// We're outside the cylinder height and radius
-				point = side_normal * (mRadius / side_normal_length) + Vec3(0, mHalfHeight * Sign(local_pos.GetY()), 0);
-				normal = point.Normalized();
+				point = side_normal * (radius / side_normal_length) + Vec3(0, half_height * Sign(local_pos.GetY()), 0);
+				normal = (local_pos - point).NormalizedOr(Vec3::sAxisY());
 			}
 			else if (side_penetration < top_penetration)
 			{
 				// Side surface is closest
 				normal = side_normal_length > 0.0f? side_normal / side_normal_length : Vec3::sAxisX();
-				point = mRadius * normal;
+				point = radius * normal;
 			}
 			else
 			{
 				// Top or bottom plane is closest
 				normal = Vec3(0, Sign(local_pos.GetY()), 0);
-				point = mHalfHeight * normal;
+				point = half_height * normal;
 			}
 
 			// Calculate penetration

Jolt/Physics/Collision/Shape/CylinderShape.h

@@ -81,7 +81,7 @@ public:
 	virtual void			CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
 	// See: Shape::ColideSoftBodyVertices
-	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
 
 	// See Shape::TransformShape
 	virtual void			TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override;

Jolt/Physics/Collision/Shape/HeightFieldShape.cpp

@@ -1544,9 +1544,9 @@ void HeightFieldShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &in
 	// A height field doesn't have volume, so we can't test insideness
 }
 
-void HeightFieldShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+void HeightFieldShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
 {
-	sCollideSoftBodyVerticesUsingRayCast(*this, inCenterOfMassTransform, ioVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex);
+	sCollideSoftBodyVerticesUsingRayCast(*this, inCenterOfMassTransform, inScale, ioVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex);
 }
 
 void HeightFieldShape::sCastConvexVsHeightField(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)

Jolt/Physics/Collision/Shape/HeightFieldShape.h

@@ -141,7 +141,7 @@ public:
 	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
 	// See: Shape::ColideSoftBodyVertices
-	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
 
 	// See Shape::GetTrianglesStart
 	virtual void					GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;

Jolt/Physics/Collision/Shape/MeshShape.cpp

@@ -778,9 +778,9 @@ void MeshShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShap
 	sCollidePointUsingRayCast(*this, inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter);
 }
 
-void MeshShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+void MeshShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
 {
-	sCollideSoftBodyVerticesUsingRayCast(*this, inCenterOfMassTransform, ioVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex);
+	sCollideSoftBodyVerticesUsingRayCast(*this, inCenterOfMassTransform, inScale, ioVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex);
 }
 
 void MeshShape::sCastConvexVsMesh(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)

Jolt/Physics/Collision/Shape/MeshShape.h

@@ -107,7 +107,7 @@ public:
 	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
 	// See: Shape::ColideSoftBodyVertices
-	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
 
 	// See Shape::GetTrianglesStart
 	virtual void					GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;

Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.cpp

@@ -127,9 +127,9 @@ void OffsetCenterOfMassShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCrea
 	mInnerShape->CollidePoint(inPoint + mOffset, inSubShapeIDCreator, ioCollector, inShapeFilter);
 }
 
-void OffsetCenterOfMassShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+void OffsetCenterOfMassShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
 {
-	mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform.PreTranslated(-mOffset), ioVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex);
+	mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale, ioVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex);
 }
 
 void OffsetCenterOfMassShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const

Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h

@@ -96,7 +96,7 @@ public:
 	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
 	// See: Shape::ColideSoftBodyVertices
-	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
 
 	// See Shape::CollectTransformedShapes
 	virtual void					CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override;

Jolt/Physics/Collision/Shape/RotatedTranslatedShape.cpp

@@ -161,9 +161,9 @@ void RotatedTranslatedShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreat
 	mInnerShape->CollidePoint(transform * inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter);
 }
 
-void RotatedTranslatedShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+void RotatedTranslatedShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
 {
-	mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform * Mat44::sRotation(mRotation), ioVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex);
+	mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform * Mat44::sRotation(mRotation), inScale, ioVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex);
 }
 
 void RotatedTranslatedShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const

Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h

@@ -98,7 +98,7 @@ public:
 	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
 	// See: Shape::ColideSoftBodyVertices
-	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
 
 	// See Shape::CollectTransformedShapes
 	virtual void					CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override;

Jolt/Physics/Collision/Shape/ScaledShape.cpp

@@ -132,9 +132,9 @@ void ScaledShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubSh
 	mInnerShape->CollidePoint(inv_scale * inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter);
 }
 
-void ScaledShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+void ScaledShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
 {
-	mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform * Mat44::sScale(mScale), ioVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex);
+	mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform, inScale * mScale, ioVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex);
 }
 
 void ScaledShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const

Jolt/Physics/Collision/Shape/ScaledShape.h

@@ -94,7 +94,7 @@ public:
 	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
 	// See: Shape::ColideSoftBodyVertices
-	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
 
 	// See Shape::CollectTransformedShapes
 	virtual void					CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override;

Jolt/Physics/Collision/Shape/Shape.cpp

@@ -331,9 +331,10 @@ Shape::ShapeResult Shape::ScaleShape(Vec3Arg inScale) const
 	return compound.Create();
 }
 
-void Shape::sCollideSoftBodyVerticesUsingRayCast(const Shape &inShape, Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex)
+void Shape::sCollideSoftBodyVerticesUsingRayCast(const Shape &inShape, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex)
 {
-	Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation();
+	Mat44 inverse_transform = Mat44::sScale(inScale.Reciprocal()) * inCenterOfMassTransform.InversedRotationTranslation();
+	Mat44 direction_preserving_transform = inverse_transform.Transposed3x3(); // To transform normals: transpose of the inverse
 
 	for (SoftBodyVertex &v : ioVertices)
 		if (v.mInvMass > 0.0f)
@@ -356,7 +357,7 @@ void Shape::sCollideSoftBodyVerticesUsingRayCast(const Shape &inShape, Mat44Arg
 
 					// Calculate contact point and normal
 					Vec3 point = ray.GetPointOnRay(hit.mFraction);
-					Vec3 normal = inCenterOfMassTransform.Multiply3x3(inShape.GetSurfaceNormal(hit.mSubShapeID2, inverse_transform * point));
+					Vec3 normal = direction_preserving_transform.Multiply3x3(inShape.GetSurfaceNormal(hit.mSubShapeID2, inverse_transform * point)).Normalized();
 
 					// Store collision
 					v.mCollisionPlane = Plane::sFromPointAndNormal(point, normal);

Jolt/Physics/Collision/Shape/Shape.h

@@ -303,11 +303,12 @@ public:
 
 	/// Collides all vertices of a soft body with this shape and updates SoftBodyVertex::mCollisionPlane, SoftBodyVertex::mCollidingShapeIndex and SoftBodyVertex::mLargestPenetration if a collision with more penetration was found.
 	/// @param inCenterOfMassTransform Center of mass transform for this shape relative to the vertices.
+	/// @param inScale The scale to use for this shape
 	/// @param ioVertices The vertices of the soft body
 	/// @param inDeltaTime Delta time of this time step (can be used to extrapolate the position using the velocity of the particle)
 	/// @param inDisplacementDueToGravity Displacement due to gravity during this time step
 	/// @param inCollidingShapeIndex Value to store in SoftBodyVertex::mCollidingShapeIndex when a collision was found
-	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const = 0;
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const = 0;
 
 	/// Collect the leaf transformed shapes of all leaf shapes of this shape.
 	/// inBox is the world space axis aligned box which leaf shapes should collide with.
@@ -421,7 +422,7 @@ protected:
 	static void						sCollidePointUsingRayCast(const Shape &inShape, Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter);
 
 	/// A fallback version of CollideSoftBodyVertices that uses a raycast to collide the vertices with the shape.
-	static void						sCollideSoftBodyVerticesUsingRayCast(const Shape &inShape, Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex);
+	static void						sCollideSoftBodyVerticesUsingRayCast(const Shape &inShape, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex);
 
 private:
 	uint64							mUserData = 0;

Jolt/Physics/Collision/Shape/SphereShape.cpp

@@ -275,9 +275,10 @@ void SphereShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubSh
 		ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
 }
 
-void SphereShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+void SphereShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, Array<SoftBodyVertex> &ioVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
 {
 	Vec3 center = inCenterOfMassTransform.GetTranslation();
+	float radius = GetScaledRadius(inScale);
 
 	for (SoftBodyVertex &v : ioVertices)
 		if (v.mInvMass > 0.0f)
@@ -285,14 +286,14 @@ void SphereShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Arra
 			// Calculate penetration
 			Vec3 delta = v.mPosition - center;
 			float distance = delta.Length();
-			float penetration = mRadius - distance;
+			float penetration = radius - distance;
 			if (penetration > v.mLargestPenetration)
 			{
 				v.mLargestPenetration = penetration;
 
 				// Calculate contact point and normal
 				Vec3 normal = distance > 0.0f? delta / distance : Vec3::sAxisY();
-				Vec3 point = center + mRadius * normal;
+				Vec3 point = center + radius * normal;
 
 				// Store collision
 				v.mCollisionPlane = Plane::sFromPointAndNormal(point, normal);

Jolt/Physics/Collision/Shape/SphereShape.h

@@ -81,7 +81,7 @@ public:
 	virtual void			CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
 	// See: Shape::ColideSoftBodyVertices
-	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
 
 	// See Shape::TransformShape
 	virtual void			TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override;

Jolt/Physics/Collision/Shape/TaperedCapsuleShape.cpp

@@ -300,30 +300,42 @@ AABox TaperedCapsuleShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform,
 	return AABox(p1, p2);
 }
 
-void TaperedCapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+void TaperedCapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, Array<SoftBodyVertex> &ioVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
 {
+	JPH_ASSERT(IsValidScale(inScale));
+
 	Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation();
 
+	// Get scaled tapered capsule
+	Vec3 abs_scale = inScale.Abs();
+	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;
+	float scaled_top_radius = scale_xz * mTopRadius;
+	float scaled_bottom_radius = scale_xz * mBottomRadius;
+	
 	for (SoftBodyVertex &v : ioVertices)
 		if (v.mInvMass > 0.0f)
 		{
-			Vec3 local_pos = inverse_transform * v.mPosition;
+			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() > mTopCenter + mSinAlpha * mTopRadius)
+			if (local_pos.GetY() > scaled_top_center + mSinAlpha * scaled_top_radius)
 			{
 				// Top sphere
-				Vec3 top = Vec3(0, mTopCenter, 0);
+				Vec3 top = Vec3(0, scaled_top_center, 0);
 				normal = (local_pos - top).NormalizedOr(Vec3::sAxisY());
-				position = top + mTopRadius * normal;
+				position = top + scaled_top_radius * normal;
 			}
-			else if (local_pos.GetY() < mBottomCenter + mSinAlpha * mBottomRadius)
+			else if (local_pos.GetY() < scaled_bottom_center + mSinAlpha * scaled_bottom_radius)
 			{
 				// Bottom sphere
-				Vec3 bottom(0, mBottomCenter, 0);
+				Vec3 bottom(0, scaled_bottom_center, 0);
 				normal = (local_pos - bottom).NormalizedOr(-Vec3::sAxisY());
-				position = bottom + mBottomRadius * normal;
+				position = bottom + scaled_bottom_radius * normal;
 			}
 			else
 			{
@@ -331,7 +343,7 @@ void TaperedCapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransfo
 				normal = Vec3(local_pos.GetX(), 0, local_pos.GetZ()).NormalizedOr(Vec3::sAxisX());
 				normal.SetY(mTanAlpha);
 				normal = normal.NormalizedOr(Vec3::sAxisX());
-				position = Vec3(0, mBottomCenter, 0) + mBottomRadius * normal;
+				position = Vec3(0, scaled_bottom_center, 0) + scaled_bottom_radius * normal;
 			}
 
 			Plane plane = Plane::sFromPointAndNormal(position, normal);
@@ -340,6 +352,9 @@ void TaperedCapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransfo
 			{
 				v.mLargestPenetration = penetration;
 
+				// Need to flip the normal's y if capsule is flipped (this corresponds to flipping both the point and the normal around y)
+				plane.SetNormal(scale_y_flip * plane.GetNormal());
+
 				// Store collision
 				v.mCollisionPlane = plane.GetTransformed(inCenterOfMassTransform);
 				v.mCollidingShapeIndex = inCollidingShapeIndex;

Jolt/Physics/Collision/Shape/TaperedCapsuleShape.h

@@ -72,7 +72,7 @@ public:
 	virtual const Support *	GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override;
 
 	// See: Shape::ColideSoftBodyVertices
-	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
 
 #ifdef JPH_DEBUG_RENDERER
 	// See Shape::Draw

Jolt/Physics/Collision/Shape/TriangleShape.cpp

@@ -17,8 +17,10 @@
 #include <Jolt/Physics/Collision/CollideConvexVsTriangles.h>
 #include <Jolt/Physics/Collision/CollideSphereVsTriangles.h>
 #include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Physics/SoftBody/SoftBodyVertex.h>
 #include <Jolt/Geometry/ConvexSupport.h>
 #include <Jolt/Geometry/RayTriangle.h>
+#include <Jolt/Geometry/ClosestPoint.h>
 #include <Jolt/ObjectStream/TypeDeclarations.h>
 #include <Jolt/Core/StreamIn.h>
 #include <Jolt/Core/StreamOut.h>
@@ -257,9 +259,58 @@ void TriangleShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSub
 	// Can't be inside a triangle
 }
 
-void TriangleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+void TriangleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
 {
-	sCollideSoftBodyVerticesUsingRayCast(*this, inCenterOfMassTransform, ioVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex);
+	Vec3 v1 = inCenterOfMassTransform * (inScale * mV1);
+	Vec3 v2 = inCenterOfMassTransform * (inScale * mV2);
+	Vec3 v3 = inCenterOfMassTransform * (inScale * mV3);
+
+	if (ScaleHelpers::IsInsideOut(inScale))
+		swap(v1, v2);
+
+	Vec3 triangle_normal = (v2 - v1).Cross(v3 - v1).NormalizedOr(Vec3::sAxisY());
+
+	for (SoftBodyVertex &v : ioVertices)
+		if (v.mInvMass > 0.0f)
+		{
+			// Get the closest point from the vertex to the triangle
+			uint32 set;
+			Vec3 v1_minus_position = v1 - v.mPosition;
+			Vec3 closest_point = ClosestPoint::GetClosestPointOnTriangle(v1_minus_position, v2 - v.mPosition, v3 - v.mPosition, set);
+
+			if (set == 0b111)
+			{
+				// Closest is interior to the triangle, use plane as collision plane but don't allow more than 10cm penetration
+				// because otherwise a triangle half a level a way will have a huge penetration if it is back facing
+				float penetration = min(triangle_normal.Dot(v1_minus_position), 0.1f);
+				if (penetration > v.mLargestPenetration)
+				{
+					v.mLargestPenetration = penetration;
+
+					// Store collision
+					v.mCollisionPlane = Plane::sFromPointAndNormal(v1, triangle_normal);
+					v.mCollidingShapeIndex = inCollidingShapeIndex;
+				}
+			}
+			else if (closest_point.Dot(triangle_normal) < 0.0f) // Ignore back facing edges
+			{
+				// Closest point is on an edge or vertex, use closest point as collision plane
+				float closest_point_length = closest_point.Length();
+				float penetration = -closest_point_length;
+				if (penetration > v.mLargestPenetration)
+				{
+					v.mLargestPenetration = penetration;
+
+					// Calculate contact point and normal
+					Vec3 point = v.mPosition + closest_point;
+					Vec3 normal = -closest_point / closest_point_length;
+
+					// Store collision
+					v.mCollisionPlane = Plane::sFromPointAndNormal(point, normal);
+					v.mCollidingShapeIndex = inCollidingShapeIndex;
+				}
+			}
+		}
 }
 
 void TriangleShape::sCollideConvexVsTriangle(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter)

Jolt/Physics/Collision/Shape/TriangleShape.h

@@ -85,7 +85,7 @@ public:
 	virtual void			CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
 	// See: Shape::ColideSoftBodyVertices
-	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
 
 	// See Shape::TransformShape
 	virtual void			TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override;

Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp

@@ -189,7 +189,7 @@ void SoftBodyMotionProperties::Update(float inDeltaTime, Body &inSoftBody, Vec3
 
 	// Generate collision planes
 	for (const CollidingShape &cs : collector.mHits)
-		cs.mShape->CollideSoftBodyVertices(cs.mCenterOfMassTransform, mVertices, inDeltaTime, displacement_due_to_gravity, int(&cs - collector.mHits.data()));
+		cs.mShape->CollideSoftBodyVertices(cs.mCenterOfMassTransform, Vec3::sReplicate(1.0f), mVertices, inDeltaTime, displacement_due_to_gravity, int(&cs - collector.mHits.data()));
 
 	float inv_dt_sq = 1.0f / dt_sq;
 	float linear_damping = max(0.0f, 1.0f - GetLinearDamping() * dt); // See: MotionProperties::ApplyForceTorqueAndDragInternal

Jolt/Physics/SoftBody/SoftBodyShape.cpp

@@ -106,9 +106,9 @@ void SoftBodyShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSub
 	sCollidePointUsingRayCast(*this, inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter);
 }
 
-void SoftBodyShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+void SoftBodyShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
 {
-	JPH_ASSERT(false, "Should not be called");
+	/* Not implemented */
 }
 
 const PhysicsMaterial *SoftBodyShape::GetMaterial(const SubShapeID &inSubShapeID) const

Jolt/Physics/SoftBody/SoftBodyShape.h

@@ -44,7 +44,7 @@ public:
 	virtual bool					CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override;
 	virtual void					CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
-	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
 	virtual void					GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;
 	virtual int						GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override;
 	virtual Stats					GetStats() const override;

Samples/SamplesApp.cpp

@@ -16,6 +16,7 @@
 #include <Jolt/Physics/StateRecorderImpl.h>
 #include <Jolt/Physics/Body/BodyCreationSettings.h>
 #include <Jolt/Physics/SoftBody/SoftBodyMotionProperties.h>
+#include <Jolt/Physics/SoftBody/SoftBodyCreationSettings.h>
 #include <Jolt/Physics/PhysicsScene.h>
 #include <Jolt/Physics/Collision/RayCast.h>
 #include <Jolt/Physics/Collision/ShapeCast.h>
@@ -42,6 +43,7 @@
 #include <Utils/Log.h>
 #include <Utils/ShapeCreator.h>
 #include <Utils/CustomMemoryHook.h>
+#include <Utils/SoftBodyCreator.h>
 #include <Renderer/DebugRendererImp.h>
 
 JPH_SUPPRESS_WARNINGS_STD_BEGIN
@@ -515,7 +517,7 @@ SamplesApp::SamplesApp()
 			UIElement *shoot_options = mDebugUI->CreateMenu();
 			mDebugUI->CreateTextButton(shoot_options, "Shoot Object (B)", [=]() { ShootObject(); });
 			mDebugUI->CreateSlider(shoot_options, "Initial Velocity", mShootObjectVelocity, 0.0f, 500.0f, 10.0f, [this](float inValue) { mShootObjectVelocity = inValue; });
-			mDebugUI->CreateComboBox(shoot_options, "Shape", { "Sphere", "ConvexHull", "Thin Bar" }, (int)mShootObjectShape, [=](int inItem) { mShootObjectShape = (EShootObjectShape)inItem; });
+			mDebugUI->CreateComboBox(shoot_options, "Shape", { "Sphere", "ConvexHull", "Thin Bar", "Soft Body Cube" }, (int)mShootObjectShape, [=](int inItem) { mShootObjectShape = (EShootObjectShape)inItem; });
 			mDebugUI->CreateComboBox(shoot_options, "Motion Quality", { "Discrete", "LinearCast" }, (int)mShootObjectMotionQuality, [=](int inItem) { mShootObjectMotionQuality = (EMotionQuality)inItem; });
 			mDebugUI->CreateSlider(shoot_options, "Friction", mShootObjectFriction, 0.0f, 1.0f, 0.05f, [this](float inValue) { mShootObjectFriction = inValue; });
 			mDebugUI->CreateSlider(shoot_options, "Restitution", mShootObjectRestitution, 0.0f, 1.0f, 0.05f, [this](float inValue) { mShootObjectRestitution = inValue; });
@@ -920,6 +922,10 @@ RefConst<Shape> SamplesApp::CreateShootObjectShape()
 	case EShootObjectShape::ThinBar:
 		shape = BoxShapeSettings(Vec3(0.05f, 0.8f, 0.03f), 0.015f).Create().Get();
 		break;
+
+	case EShootObjectShape::SoftBodyCube:
+		JPH_ASSERT(false);
+		break;
 	}
 
 	// Scale shape if needed
@@ -931,15 +937,36 @@ RefConst<Shape> SamplesApp::CreateShootObjectShape()
 
 void SamplesApp::ShootObject()
 {
-	// Configure body
-	BodyCreationSettings creation_settings(CreateShootObjectShape(), GetCamera().mPos, Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
-	creation_settings.mMotionQuality = mShootObjectMotionQuality;
-	creation_settings.mFriction = mShootObjectFriction;
-	creation_settings.mRestitution = mShootObjectRestitution;
-	creation_settings.mLinearVelocity = mShootObjectVelocity * GetCamera().mForward;
-
-	// Create body
-	mPhysicsSystem->GetBodyInterface().CreateAndAddBody(creation_settings, EActivation::Activate);
+	if (mShootObjectShape != EShootObjectShape::SoftBodyCube)
+	{
+		// Configure body
+		BodyCreationSettings creation_settings(CreateShootObjectShape(), GetCamera().mPos, Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+		creation_settings.mMotionQuality = mShootObjectMotionQuality;
+		creation_settings.mFriction = mShootObjectFriction;
+		creation_settings.mRestitution = mShootObjectRestitution;
+		creation_settings.mLinearVelocity = mShootObjectVelocity * GetCamera().mForward;
+
+		// Create body
+		mPhysicsSystem->GetBodyInterface().CreateAndAddBody(creation_settings, EActivation::Activate);
+	}
+	else
+	{
+		Ref<SoftBodySharedSettings> shared_settings = SoftBodyCreator::CreateCube(5, 0.5f * GetWorldScale());
+		for (SoftBodySharedSettings::Vertex &v : shared_settings->mVertices)
+		{
+			v.mInvMass = 0.025f;
+			(mShootObjectVelocity * GetCamera().mForward).StoreFloat3(&v.mVelocity);
+		}
+
+		// Confgure soft body
+		SoftBodyCreationSettings creation_settings(shared_settings, GetCamera().mPos);
+		creation_settings.mObjectLayer = Layers::MOVING;
+		creation_settings.mFriction = mShootObjectFriction;
+		creation_settings.mRestitution = mShootObjectRestitution;
+
+		// Create body
+		mPhysicsSystem->GetBodyInterface().CreateAndAddSoftBody(creation_settings, EActivation::Activate);
+	}
 }
 
 bool SamplesApp::CastProbe(float inProbeLength, float &outFraction, RVec3 &outPosition, BodyID &outID)
@@ -1381,7 +1408,7 @@ bool SamplesApp::CastProbe(float inProbeLength, float &outFraction, RVec3 &outPo
 			SoftBodyVertex tmp_vertex;
 			tmp_vertex.mInvMass = 1.0f;
 			tmp_vertex.mPosition = fraction * direction;
-			tmp_vertex.mVelocity = Vec3(0, -9.81f / 60.0f, 0);
+			tmp_vertex.mVelocity = 10.0f * direction;
 			tmp_vertex.mCollidingShapeIndex = -1;
 			tmp_vertex.mLargestPenetration = -FLT_MAX;
 			Array<SoftBodyVertex> vertices = { tmp_vertex };
@@ -1399,7 +1426,7 @@ bool SamplesApp::CastProbe(float inProbeLength, float &outFraction, RVec3 &outPo
 			for (const TransformedShape &ts : collector.mHits)
 			{
 				int colliding_shape_index = int(&ts - collector.mHits.data());
-				ts.mShape->CollideSoftBodyVertices((RMat44::sTranslation(-start) * ts.GetCenterOfMassTransform()).ToMat44(), vertices, 1.0f / 60.0f, Vec3(0, -0.5f * 9.81f / Square(60.0f), 0), colliding_shape_index);
+				ts.mShape->CollideSoftBodyVertices((RMat44::sTranslation(-start) * ts.GetCenterOfMassTransform()).ToMat44(), ts.GetShapeScale(), vertices, 1.0f / 60.0f, Vec3::sZero(), colliding_shape_index);
 				if (vertices[0].mCollidingShapeIndex == colliding_shape_index)
 				{
 					// To draw a plane, we need a point but CollideSoftBodyVertices doesn't provide one, so we use CollideShape with a tiny sphere to get the closest point and then project that onto the plane to draw the plane

Samples/SamplesApp.h

@@ -200,6 +200,7 @@ private:
 		Sphere,
 		ConvexHull,
 		ThinBar,
+		SoftBodyCube,
 	};
 
 	// Shoot object settings