Fixed crash when soft body collides with height field (#1238)

* Fixed crash when soft body collides with height field

When a height field is created where SampleCount / BlockSize is not a power of 2 and the a body touched the right or bottom border of the height field, the application would crash. The VisitRangeBlock implementation for CollideSoftBodyVertices did not ignore inside out bounding boxes and tried to read non-existing height field samples.

See report: https://github.com/godot-jolt/godot-jolt/issues/951#issuecomment-2321166739

Docs/ReleaseNotes.md

@@ -17,6 +17,7 @@ For breaking API changes see [this document](https://github.com/jrouwe/JoltPhysi
 * Fixed an issue where enhanced internal edge removal would throw away valid contacts when a dynamic compound shape is colliding with another mesh / box shape.
 * Fixed an issue where the bounding volume of a HeightFieldShape was not properly adjusted when calling SetHeights leading to missed collisions.
 * Workaround for CMake error 'CMake Error: No output files for WriteBuild!' when using the 'Ninja Multi-Config' generator.
+* When a height field was created where SampleCount / BlockSize is not a power of 2 and a soft body touched the right or bottom border of the height field, the application would crash.
 
 ## v5.1.0
 

Jolt/Physics/Collision/Shape/HeightFieldShape.cpp

@@ -1723,6 +1723,10 @@ public:
 		if (mShape->mHeightSamplesSize == 0)
 			return;
 
+		// Assert that an inside-out bounding box does not collide
+		JPH_IF_ENABLE_ASSERTS(UVec4 dummy = UVec4::sReplicate(0);)
+		JPH_ASSERT(ioVisitor.VisitRangeBlock(Vec4::sReplicate(-1.0e6f), Vec4::sReplicate(1.0e6f), Vec4::sReplicate(-1.0e6f), Vec4::sReplicate(1.0e6f), Vec4::sReplicate(-1.0e6f), Vec4::sReplicate(1.0e6f), dummy, 0) == 0);
+
 		// Precalculate values relating to sample count
 		uint32 sample_count = mShape->mSampleCount;
 		UVec4 sample_count_min_1 = UVec4::sReplicate(sample_count - 1);
@@ -2220,6 +2224,9 @@ void HeightFieldShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform,
 			// Get distance to vertex
 			Vec4 dist_sq = AABox4DistanceSqToPoint(mLocalPosition, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
 
+			// Clear distance for invalid bounds
+			dist_sq = Vec4::sSelect(Vec4::sReplicate(FLT_MAX), dist_sq, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY));
+
 			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
 			return SortReverseAndStore(dist_sq, mClosestDistanceSq, ioProperties, &mDistanceStack[inStackTop]);
 		}