Added ability to update a height field after creation (#732)

- Also added a demo showing real time deformation of the terrain.
- Added function to wake up bodies in a axis aligned box

.github/workflows/determinism_check.yml

@@ -2,7 +2,7 @@ name: Determinism Check
 
 env:
     CONVEX_VS_MESH_HASH: '0x10139effe747511'
-    RAGDOLL_HASH: '0x93470b2bd0a04d75'
+    RAGDOLL_HASH: '0xeb0afdf14f49c318'
 
 on:
   push:

Jolt/Physics/Body/BodyInterface.cpp

@@ -5,6 +5,7 @@
 #include <Jolt/Jolt.h>
 
 #include <Jolt/Physics/Collision/BroadPhase/BroadPhase.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
 #include <Jolt/Physics/Body/Body.h>
 #include <Jolt/Physics/Body/BodyManager.h>
 #include <Jolt/Physics/Body/BodyInterface.h>
@@ -218,6 +219,13 @@ void BodyInterface::ActivateBodies(const BodyID *inBodyIDs, int inNumber)
 	mBodyManager->ActivateBodies(inBodyIDs, inNumber);
 }
 
+void BodyInterface::ActivateBodiesInAABox(const AABox &inBox, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter)
+{
+	AllHitCollisionCollector<CollideShapeBodyCollector> collector;
+	mBroadPhase->CollideAABox(inBox, collector, inBroadPhaseLayerFilter, inObjectLayerFilter);
+	ActivateBodies(collector.mHits.data(), (int)collector.mHits.size());
+}
+
 void BodyInterface::DeactivateBody(const BodyID &inBodyID)
 {
 	BodyLockWrite lock(*mBodyLockInterface, inBodyID);

Jolt/Physics/Body/BodyInterface.h

@@ -26,6 +26,8 @@ class SubShapeID;
 class Shape;
 class TwoBodyConstraintSettings;
 class TwoBodyConstraint;
+class BroadPhaseLayerFilter;
+class AABox;
 
 /// Class that provides operations on bodies using a body ID. Note that if you need to do multiple operations on a single body, it is more efficient to lock the body once and combine the operations.
 /// All quantities are in world space unless otherwise specified.
@@ -123,6 +125,7 @@ public:
 	///@{
 	void						ActivateBody(const BodyID &inBodyID);
 	void						ActivateBodies(const BodyID *inBodyIDs, int inNumber);
+	void						ActivateBodiesInAABox(const AABox &inBox, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter);
 	void						DeactivateBody(const BodyID &inBodyID);
 	void						DeactivateBodies(const BodyID *inBodyIDs, int inNumber);
 	bool						IsActive(const BodyID &inBodyID) const;

Jolt/Physics/Collision/Shape/HeightFieldShape.cpp

@@ -25,6 +25,7 @@
 #include <Jolt/Core/StringTools.h>
 #include <Jolt/Core/StreamIn.h>
 #include <Jolt/Core/StreamOut.h>
+#include <Jolt/Core/TempAllocator.h>
 #include <Jolt/Geometry/AABox4.h>
 #include <Jolt/Geometry/RayTriangle.h>
 #include <Jolt/Geometry/RayAABox.h>
@@ -48,6 +49,8 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(HeightFieldShapeSettings)
 	JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mHeightSamples)
 	JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mOffset)
 	JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mScale)
+	JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mMinHeightValue)
+	JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mMaxHeightValue)
 	JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mSampleCount)
 	JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mBlockSize)
 	JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mBitsPerSample)
@@ -106,8 +109,8 @@ ShapeSettings::ShapeResult HeightFieldShapeSettings::Create() const
 void HeightFieldShapeSettings::DetermineMinAndMaxSample(float &outMinValue, float &outMaxValue, float &outQuantizationScale) const
 {
 	// Determine min and max value
-	outMinValue = FLT_MAX;
-	outMaxValue = -FLT_MAX;
+	outMinValue = mMinHeightValue;
+	outMaxValue = mMaxHeightValue;
 	for (float h : mHeightSamples)
 		if (h != cNoCollisionValue)
 		{
@@ -195,80 +198,153 @@ uint32 HeightFieldShapeSettings::CalculateBitsPerSampleForError(float inMaxError
 	return bits_per_sample;
 }
 
-void HeightFieldShape::CalculateActiveEdges(const HeightFieldShapeSettings &inSettings)
+void HeightFieldShape::CalculateActiveEdges(uint inX, uint inY, uint inSizeX, uint inSizeY, const float *inHeights, uint inHeightsStartX, uint inHeightsStartY, uint inHeightsStride, float inHeightsScale, float inActiveEdgeCosThresholdAngle, TempAllocator &inAllocator)
 {
-	// Store active edges. The triangles are organized like this:
-	//  +       +
-	//  | \ T1B | \ T2B
-	// e0   e2  |   \
-	//  | T1A \ | T2A \
-	//  +--e1---+-------+
-	//  | \ T3B | \ T4B
-	//  |   \   |   \
-	//  | T3A \ | T4A \
-	//  +-------+-------+
-	// We store active edges e0 .. e2 as bits 0 .. 2.
-	// We store triangles horizontally then vertically (order T1A, T2A, T3A and T4A).
-	// The top edge and right edge of the heightfield are always active so we do not need to store them,
-	// therefore we only need to store (mSampleCount - 1)^2 * 3-bit
-	// The triangles T1B, T2B, T3B and T4B do not need to be stored, their active edges can be constructed from adjacent triangles.
-	// Add 1 byte padding so we can always read 1 uint16 to get the bits that cross an 8 bit boundary
-	uint count_min_1 = mSampleCount - 1;
-	uint count_min_1_sq = Square(count_min_1);
-	mActiveEdges.resize((count_min_1_sq * 3 + 7) / 8 + 1);
-	memset(&mActiveEdges[0], 0, mActiveEdges.size());
+	// Allocate temporary buffer for normals
+	uint normals_size = 2 * inSizeX * inSizeY * sizeof(Vec3);
+	Vec3 *normals = (Vec3 *)inAllocator.Allocate(normals_size);
 
 	// Calculate triangle normals and make normals zero for triangles that are missing
-	Array<Vec3> normals;
-	normals.resize(2 * count_min_1_sq);
-	memset(&normals[0], 0, normals.size() * sizeof(Vec3));
-	for (uint y = 0; y < count_min_1; ++y)
-		for (uint x = 0; x < count_min_1; ++x)
-			if (!IsNoCollision(x, y) && !IsNoCollision(x + 1, y + 1))
+	Vec3 *out_normal = normals;
+	for (uint y = 0; y < inSizeY; ++y)
+		for (uint x = 0; x < inSizeX; ++x)
+		{
+			// Get height on diagonal
+			const float *height_samples = inHeights + (inY - inHeightsStartY + y) * inHeightsStride + (inX - inHeightsStartX + x);
+			float x1y1_h = height_samples[0];
+			float x2y2_h = height_samples[inHeightsStride + 1];
+			if (x1y1_h != cNoCollisionValue && x2y2_h != cNoCollisionValue)
 			{
-				Vec3 x1y1 = GetPosition(x, y);
-				Vec3 x2y2 = GetPosition(x + 1, y + 1);
-
-				uint offset = 2 * (count_min_1 * y + x);
-
-				if (!IsNoCollision(x, y + 1))
+				// Calculate normal for lower left triangle (e.g. T1A)
+				float x1y2_h = height_samples[inHeightsStride];
+				if (x1y2_h != cNoCollisionValue)
 				{
-					Vec3 x1y2 = GetPosition(x, y + 1);
-					normals[offset] = (x2y2 - x1y2).Cross(x1y1 - x1y2).Normalized();
+					Vec3 x2y2_minus_x1y2(mScale.GetX(), inHeightsScale * (x2y2_h - x1y2_h), 0);
+					Vec3 x1y1_minus_x1y2(0, inHeightsScale * (x1y1_h - x1y2_h), -mScale.GetZ());
+					out_normal[0] = x2y2_minus_x1y2.Cross(x1y1_minus_x1y2).Normalized();
 				}
+				else
+					out_normal[0] = Vec3::sZero();
 
-				if (!IsNoCollision(x + 1, y))
+				// Calculate normal for upper right triangle (e.g. T1B)
+				float x2y1_h = height_samples[1];
+				if (x2y1_h != cNoCollisionValue)
 				{
-					Vec3 x2y1 = GetPosition(x + 1, y);
-					normals[offset + 1] = (x1y1 - x2y1).Cross(x2y2 - x2y1).Normalized();
+					Vec3 x1y1_minus_x2y1(-mScale.GetX(), inHeightsScale * (x1y1_h - x2y1_h), 0);
+					Vec3 x2y2_minus_x2y1(0, inHeightsScale * (x2y2_h - x2y1_h), mScale.GetZ());
+					out_normal[1] = x1y1_minus_x2y1.Cross(x2y2_minus_x2y1).Normalized();
 				}
+				else
+					out_normal[1] = Vec3::sZero();
+			}
+			else
+			{
+				out_normal[0] = Vec3::sZero();
+				out_normal[1] = Vec3::sZero();
 			}
 
+			out_normal += 2;
+		}
+
 	// Calculate active edges
-	for (uint y = 0; y < count_min_1; ++y)
-		for (uint x = 0; x < count_min_1; ++x)
+	const Vec3 *in_normal = normals;
+	uint global_bit_pos = 3 * (inY * (mSampleCount - 1) + inX);
+	for (uint y = 0; y < inSizeY; ++y)
+	{
+		for (uint x = 0; x < inSizeX; ++x)
 		{
-			// Calculate vertex positions.
-			// We don't check 'no colliding' since those normals will be zero and sIsEdgeActive will return true
-			Vec3 x1y1 = GetPosition(x, y);
-			Vec3 x1y2 = GetPosition(x, y + 1);
-			Vec3 x2y2 = GetPosition(x + 1, y + 1);
+			// Get vertex heights
+			const float *height_samples = inHeights + (inY - inHeightsStartY + y) * inHeightsStride + (inX - inHeightsStartX + x);
+			float x1y1_h = height_samples[0];
+			float x1y2_h = height_samples[inHeightsStride];
+			float x2y2_h = height_samples[inHeightsStride + 1];
+			bool x1y1_valid = x1y1_h != cNoCollisionValue;
+			bool x1y2_valid = x1y2_h != cNoCollisionValue;
+			bool x2y2_valid = x2y2_h != cNoCollisionValue;
 
 			// Calculate the edge flags (3 bits)
-			uint offset = 2 * (count_min_1 * y + x);
-			bool edge0_active = x == 0 || ActiveEdges::IsEdgeActive(normals[offset], normals[offset - 1], x1y2 - x1y1, inSettings.mActiveEdgeCosThresholdAngle);
-			bool edge1_active = y == count_min_1 - 1 || ActiveEdges::IsEdgeActive(normals[offset], normals[offset + 2 * count_min_1 + 1], x2y2 - x1y2, inSettings.mActiveEdgeCosThresholdAngle);
-			bool edge2_active = ActiveEdges::IsEdgeActive(normals[offset], normals[offset + 1], x1y1 - x2y2, inSettings.mActiveEdgeCosThresholdAngle);
-			uint16 edge_flags = (edge0_active? 0b001 : 0) | (edge1_active? 0b010 : 0) | (edge2_active? 0b100 : 0);
+			// See diagram in the next function for the edge numbering
+			uint16 edge_mask = 0b111;
+			uint16 edge_flags = 0;
+
+			// Edge 0
+			if (x == 0)
+				edge_mask &= 0b110; // We need normal x - 1 which we didn't calculate, don't update this edge
+			else if (x1y1_valid && x1y2_valid)
+			{
+				Vec3 edge0_direction(0, inHeightsScale * (x1y2_h - x1y1_h), mScale.GetZ());
+				if (ActiveEdges::IsEdgeActive(in_normal[0], in_normal[-1], edge0_direction, inActiveEdgeCosThresholdAngle))
+					edge_flags |= 0b001;
+			}
+
+			// Edge 1
+			if (y == inSizeY - 1)
+				edge_mask &= 0b101; // We need normal y + 1 which we didn't calculate, don't update this edge
+			else if (x1y2_valid && x2y2_valid)
+			{
+				Vec3 edge1_direction(mScale.GetX(), inHeightsScale * (x2y2_h - x1y2_h), 0);
+				if (ActiveEdges::IsEdgeActive(in_normal[0], in_normal[2 * inSizeX + 1], edge1_direction, inActiveEdgeCosThresholdAngle))
+					edge_flags |= 0b010;
+			}
+
+			// Edge 2
+			if (x1y1_valid && x2y2_valid)
+			{
+				Vec3 edge2_direction(-mScale.GetX(), inHeightsScale * (x1y1_h - x2y2_h), -mScale.GetZ());
+				if (ActiveEdges::IsEdgeActive(in_normal[0], in_normal[1], edge2_direction, inActiveEdgeCosThresholdAngle))
+					edge_flags |= 0b100;
+			}
 
 			// Store the edge flags in the array
-			uint bit_pos = 3 * (y * count_min_1 + x);
-			uint byte_pos = bit_pos >> 3;
-			bit_pos &= 0b111;
-			edge_flags <<= bit_pos;
-			mActiveEdges[byte_pos] |= uint8(edge_flags);
-			mActiveEdges[byte_pos + 1] |= uint8(edge_flags >> 8);
+			uint byte_pos = global_bit_pos >> 3;
+			uint bit_pos = global_bit_pos & 0b111;
+			uint8 *edge_flags_ptr = &mActiveEdges[byte_pos];
+			uint16 combined_edge_flags = uint16(edge_flags_ptr[0]) | uint16(uint16(edge_flags_ptr[1]) << 8);
+			combined_edge_flags &= ~(edge_mask << bit_pos);
+			combined_edge_flags |= edge_flags << bit_pos;
+			edge_flags_ptr[0] = uint8(combined_edge_flags);
+			edge_flags_ptr[1] = uint8(combined_edge_flags >> 8);
+
+			in_normal += 2;
+			global_bit_pos += 3;
 		}
+
+		global_bit_pos += 3 * (mSampleCount - 1 - inSizeX);
+	}
+
+	// Free temporary buffer for normals
+	inAllocator.Free(normals, normals_size);
+}
+
+void HeightFieldShape::CalculateActiveEdges(const HeightFieldShapeSettings &inSettings)
+{
+	// Store active edges. The triangles are organized like this:
+	//     x --->
+	// 
+	// y   +       +
+	//     | \ T1B | \ T2B
+	// |  e0   e2  |   \
+	// |   | T1A \ | T2A \
+	// V   +--e1---+-------+
+	//     | \ T3B | \ T4B
+	//     |   \   |   \
+	//     | T3A \ | T4A \
+	//     +-------+-------+
+	// We store active edges e0 .. e2 as bits 0 .. 2.
+	// We store triangles horizontally then vertically (order T1A, T2A, T3A and T4A).
+	// The top edge and right edge of the heightfield are always active so we do not need to store them,
+	// therefore we only need to store (mSampleCount - 1)^2 * 3-bit
+	// The triangles T1B, T2B, T3B and T4B do not need to be stored, their active edges can be constructed from adjacent triangles.
+	// Add 1 byte padding so we can always read 1 uint16 to get the bits that cross an 8 bit boundary
+	mActiveEdges.resize((Square(mSampleCount - 1) * 3 + 7) / 8 + 1);
+
+	// Make all edges active (if mSampleCount is bigger than inSettings.mSampleCount we need to fill up the padding,
+	// also edges at x = 0 and y = inSettings.mSampleCount - 1 are not updated)
+	memset(mActiveEdges.data(), 0xff, mActiveEdges.size());
+
+	// Now clear the edges that are not active
+	TempAllocatorMalloc allocator;
+	CalculateActiveEdges(0, 0, inSettings.mSampleCount - 1, inSettings.mSampleCount - 1, inSettings.mHeightSamples.data(), 0, 0, inSettings.mSampleCount, inSettings.mScale.GetY(), inSettings.mActiveEdgeCosThresholdAngle, allocator);
 }
 
 void HeightFieldShape::StoreMaterialIndices(const HeightFieldShapeSettings &inSettings)
@@ -615,6 +691,18 @@ inline void HeightFieldShape::sGetRangeBlockOffsetAndStride(uint inNumBlocks, ui
 	outRangeBlockStride = (inNumBlocks + 1) >> 1;
 }
 
+inline void HeightFieldShape::GetRangeBlock(uint inBlockX, uint inBlockY, uint inRangeBlockOffset, uint inRangeBlockStride, RangeBlock *&outBlock, uint &outIndexInBlock)
+{
+	JPH_ASSERT(inBlockX < GetNumBlocks() && inBlockY < GetNumBlocks());
+
+	// Convert to location of range block
+	uint rbx = inBlockX >> 1;
+	uint rby = inBlockY >> 1;
+	outIndexInBlock = ((inBlockY & 1) << 1) + (inBlockX & 1);
+
+	outBlock = &mRangeBlocks[inRangeBlockOffset + rby * inRangeBlockStride + rbx];
+}
+
 inline void HeightFieldShape::GetBlockOffsetAndScale(uint inBlockX, uint inBlockY, uint inRangeBlockOffset, uint inRangeBlockStride, float &outBlockOffset, float &outBlockScale) const
 {
 	JPH_ASSERT(inBlockX < GetNumBlocks() && inBlockY < GetNumBlocks());
@@ -743,6 +831,301 @@ bool HeightFieldShape::ProjectOntoSurface(Vec3Arg inLocalPosition, Vec3 &outSurf
 	}
 }
 
+void HeightFieldShape::GetHeights(uint inX, uint inY, uint inSizeX, uint inSizeY, float *outHeights, uint inHeightsStride) const
+{
+	if (inSizeX == 0 || inSizeY == 0)
+		return;
+
+	JPH_ASSERT(inX % mBlockSize == 0 && inY % mBlockSize == 0);
+	JPH_ASSERT(inX < mSampleCount && inY < mSampleCount);
+	JPH_ASSERT(inX + inSizeX <= mSampleCount && inY + inSizeY <= mSampleCount);
+
+	// Test if there are any samples
+	if (mHeightSamples.empty())
+	{
+		// No samples, return the offset
+		float offset = mOffset.GetY();
+		for (uint y = 0; y < inSizeY; ++y, outHeights += inHeightsStride)
+			for (uint x = 0; x < inSizeX; ++x)
+				outHeights[x] = offset;
+	}
+	else
+	{
+		// Calculate offset and stride
+		uint num_blocks = GetNumBlocks();
+		uint range_block_offset, range_block_stride;
+		sGetRangeBlockOffsetAndStride(num_blocks, sGetMaxLevel(num_blocks), range_block_offset, range_block_stride);
+
+		// Loop over blocks
+		uint block_start_x = inX / mBlockSize;
+		uint block_start_y = inY / mBlockSize;
+		uint num_blocks_x = inSizeX / mBlockSize;
+		uint num_blocks_y = inSizeY / mBlockSize;
+		for (uint block_y = 0; block_y < num_blocks_y; ++block_y)
+			for (uint block_x = 0; block_x < num_blocks_x; ++block_x)
+			{
+				// Get offset and scale for block
+				float offset, scale;
+				GetBlockOffsetAndScale(block_start_x + block_x, block_start_y + block_y, range_block_offset, range_block_stride, offset, scale);
+
+				// Adjust by global offset and scale
+				// Note: This is the math applied in GetPosition() written out to reduce calculations in the inner loop
+				scale *= mScale.GetY();
+				offset = mOffset.GetY() + mScale.GetY() * offset + 0.5f * scale;
+
+				// Loop over samples in block
+				for (uint sample_y = 0; sample_y < mBlockSize; ++sample_y)
+					for (uint sample_x = 0; sample_x < mBlockSize; ++sample_x)
+					{
+						// Calculate output coordinate
+						uint output_x = block_x * mBlockSize + sample_x;
+						uint output_y = block_y * mBlockSize + sample_y;
+
+						// Get quantized value
+						uint8 height_sample = GetHeightSample(inX + output_x, inY + output_y);
+
+						// Dequantize
+						float h = height_sample != mSampleMask? offset + height_sample * scale : cNoCollisionValue;
+						outHeights[output_y * inHeightsStride + output_x] = h;
+					}
+			}
+	}
+}
+
+void HeightFieldShape::SetHeights(uint inX, uint inY, uint inSizeX, uint inSizeY, const float *inHeights, uint inHeightsStride, TempAllocator &inAllocator, float inActiveEdgeCosThresholdAngle)
+{
+	if (inSizeX == 0 || inSizeY == 0)
+		return;
+
+	JPH_ASSERT(!mHeightSamples.empty());
+	JPH_ASSERT(inX % mBlockSize == 0 && inY % mBlockSize == 0);
+	JPH_ASSERT(inX < mSampleCount && inY < mSampleCount);
+	JPH_ASSERT(inX + inSizeX <= mSampleCount && inY + inSizeY <= mSampleCount);
+
+	// If we have a block in negative x/y direction, we will affect its range so we need to take it into account
+	bool need_temp_heights = false;
+	uint affected_x = inX;
+	uint affected_y = inY;
+	uint affected_size_x = inSizeX;
+	uint affected_size_y = inSizeY;
+	if (inX > 0) { affected_x -= mBlockSize; affected_size_x += mBlockSize; need_temp_heights = true; }
+	if (inY > 0) { affected_y -= mBlockSize; affected_size_y += mBlockSize; need_temp_heights = true; }
+
+	// If we have a block in positive x/y direction, our ranges are affected by it so we need to take it into account
+	uint heights_size_x = affected_size_x;
+	uint heights_size_y = affected_size_y;
+	if (inX + inSizeX < mSampleCount) { heights_size_x += mBlockSize; need_temp_heights = true; }
+	if (inY + inSizeY < mSampleCount) { heights_size_y += mBlockSize; need_temp_heights = true; }
+
+	// Get heights for affected area
+	const float *heights;
+	float *temp_heights;
+	if (need_temp_heights)
+	{
+		// Fetch the surrounding height data (note we're forced to recompress this data with a potentially different range so there will be some precision loss here)
+		temp_heights = (float *)inAllocator.Allocate(heights_size_x * heights_size_y * sizeof(float));
+		heights = temp_heights;
+
+		// We need to fill in the following areas:
+		// 
+		// +-----------------+
+		// |        2        |
+		// |---+---------+---|
+		// |   |         |   |
+		// | 3 |    1    | 4 |
+		// |   |         |   |
+		// |---+---------+---|
+		// |        5        |
+		// +-----------------+
+		//
+		// 1. The area that is affected by the new heights (we just copy these)
+		// 2-5. These areas are either needed to calculate the range of the affected blocks or they need to be recompressed with a different range
+		uint offset_x = inX - affected_x;
+		uint offset_y = inY - affected_y;
+
+		// Area 2
+		GetHeights(affected_x, affected_y, heights_size_x, offset_y, temp_heights, heights_size_x);
+		float *area3_start = temp_heights + offset_y * heights_size_x;
+
+		// Area 3
+		GetHeights(affected_x, inY, offset_x, inSizeY, area3_start, heights_size_x);
+
+		// Area 1
+		float *area1_start = area3_start + offset_x;
+		for (uint y = 0; y < inSizeY; ++y, area1_start += heights_size_x, inHeights += inHeightsStride)
+			memcpy(area1_start, inHeights, inSizeX * sizeof(float));
+
+		// Area 4
+		uint area4_x = inX + inSizeX;
+		GetHeights(area4_x, inY, affected_x + heights_size_x - area4_x, inSizeY, area3_start + area4_x - affected_x, heights_size_x);
+
+		// Area 5
+		uint area5_y = inY + inSizeY;
+		float *area5_start = temp_heights + (area5_y - affected_y) * heights_size_x;
+		GetHeights(affected_x, area5_y, heights_size_x, affected_y + heights_size_y - area5_y, area5_start, heights_size_x);
+	}
+	else
+	{
+		// We can directly use the input buffer because there are no extra edges to take into account
+		heights = inHeights;
+		heights_size_x = inHeightsStride;
+		temp_heights = nullptr;
+	}
+
+	// Calculate offset and stride
+	uint num_blocks = GetNumBlocks();
+	uint range_block_offset, range_block_stride;
+	uint max_level = sGetMaxLevel(num_blocks);
+	sGetRangeBlockOffsetAndStride(num_blocks, max_level, range_block_offset, range_block_stride);
+
+	// Loop over blocks
+	uint block_start_x = affected_x / mBlockSize;
+	uint block_start_y = affected_y / mBlockSize;
+	uint num_blocks_x = affected_size_x / mBlockSize;
+	uint num_blocks_y = affected_size_y / mBlockSize;
+	for (uint block_y = 0, sample_start_y = 0; block_y < num_blocks_y; ++block_y, sample_start_y += mBlockSize)
+		for (uint block_x = 0, sample_start_x = 0; block_x < num_blocks_x; ++block_x, sample_start_x += mBlockSize)
+		{
+			// Determine quantized min and max value for block
+			// Note that we need to include 1 extra row in the positive x/y direction to account for connecting triangles
+			int min_value = 0xffff;
+			int max_value = 0;
+			uint sample_x_end = min(sample_start_x + mBlockSize + 1, mSampleCount - affected_x);
+			uint sample_y_end = min(sample_start_y + mBlockSize + 1, mSampleCount - affected_y);
+			for (uint sample_y = sample_start_y; sample_y < sample_y_end; ++sample_y)
+				for (uint sample_x = sample_start_x; sample_x < sample_x_end; ++sample_x)
+				{
+					float h = heights[sample_y * heights_size_x + sample_x];
+					if (h != cNoCollisionValue)
+					{
+						int quantized_height = Clamp((int)floor((h - mOffset.GetY()) / mScale.GetY()), 0, int(cMaxHeightValue16 - 1));
+						min_value = min(min_value, quantized_height);
+						max_value = max(max_value, quantized_height + 1);
+					}
+				}
+			if (min_value > max_value)
+				min_value = max_value = cNoCollisionValue16;
+
+			// Update range for block
+			RangeBlock *range_block;
+			uint index_in_block;
+			GetRangeBlock(block_start_x + block_x, block_start_y + block_y, range_block_offset, range_block_stride, range_block, index_in_block);
+			range_block->mMin[index_in_block] = uint16(min_value);
+			range_block->mMax[index_in_block] = uint16(max_value);
+
+			// Get offset and scale for block
+			float offset_block = float(min_value);
+			float scale_block = float(max_value - min_value) / float(mSampleMask);
+
+			// Calculate scale and offset using the formula used in GetPosition() solved for the quantized height (excluding 0.5 because we round down while quantizing)
+			float scale = scale_block * mScale.GetY();
+			float offset = mOffset.GetY() + offset_block * mScale.GetY();
+
+			// Loop over samples in block
+			sample_x_end = sample_start_x + mBlockSize;
+			sample_y_end = sample_start_y + mBlockSize;
+			for (uint sample_y = sample_start_y; sample_y < sample_y_end; ++sample_y)
+				for (uint sample_x = sample_start_x; sample_x < sample_x_end; ++sample_x)
+				{
+					// Quantize height
+					float h = heights[sample_y * heights_size_x + sample_x];
+					uint8 quantized_height = h != cNoCollisionValue? uint8(Clamp((int)floor((h - offset) / scale), 0, int(mSampleMask) - 1)) : mSampleMask;
+
+					// Determine bit position of sample
+					uint sample = ((affected_y + sample_y) * mSampleCount + affected_x + sample_x) * uint(mBitsPerSample);
+					uint byte_pos = sample >> 3;
+					uint bit_pos = sample & 0b111;
+
+					// Update the height value sample
+					JPH_ASSERT(byte_pos + 1 < mHeightSamples.size());
+					uint8 *height_samples = mHeightSamples.data() + byte_pos;
+					uint16 height_sample = uint16(height_samples[0]) | uint16(uint16(height_samples[1]) << 8);
+					height_sample &= ~(uint16(mSampleMask) << bit_pos);
+					height_sample |= uint16(quantized_height) << bit_pos;
+					height_samples[0] = uint8(height_sample);
+					height_samples[1] = uint8(height_sample >> 8);
+				}
+		}
+
+	// Update active edges
+	// Note that we must take an extra row on all sides to account for connecting triangles
+	uint ae_x = inX > 1? inX - 2 : 0;
+	uint ae_y = inY > 1? inY - 2 : 0;
+	uint ae_sx = min(inX + inSizeX + 1, mSampleCount - 1) - ae_x;
+	uint ae_sy = min(inY + inSizeY + 1, mSampleCount - 1) - ae_y;
+	CalculateActiveEdges(ae_x, ae_y, ae_sx, ae_sy, heights, affected_x, affected_y, heights_size_x, 1.0f, inActiveEdgeCosThresholdAngle, inAllocator);
+
+	// Free temporary buffer
+	if (temp_heights != nullptr)
+		inAllocator.Free(temp_heights, heights_size_x * heights_size_y * sizeof(float));
+
+	// Update hierarchy of range blocks
+	while (max_level > 1)
+	{
+		// Get offset and stride for destination blocks
+		uint dst_range_block_offset, dst_range_block_stride;
+		sGetRangeBlockOffsetAndStride(num_blocks >> 1, max_level - 1, dst_range_block_offset, dst_range_block_stride);
+
+		// If we're starting halfway through a 2x2 block, we need to process one extra block since we take steps of 2 blocks below
+		uint block_x_end = (block_start_x & 1) && block_start_x + num_blocks_x < num_blocks? num_blocks_x + 1 : num_blocks_x;
+		uint block_y_end = (block_start_y & 1) && block_start_y + num_blocks_y < num_blocks? num_blocks_y + 1 : num_blocks_y;
+
+		// Loop over all affected blocks
+		for (uint block_y = 0; block_y < block_y_end; block_y += 2)
+			for (uint block_x = 0; block_x < block_x_end; block_x += 2)
+			{
+				// Get source range block
+				RangeBlock *src_range_block;
+				uint index_in_src_block;
+				GetRangeBlock(block_start_x + block_x, block_start_y + block_y, range_block_offset, range_block_stride, src_range_block, index_in_src_block);
+
+				// Determine quantized min and max value for the entire 2x2 block
+				uint16 min_value = 0xffff;
+				uint16 max_value = 0;
+				for (uint i = 0; i < 4; ++i)
+					if (src_range_block->mMin[i] != cNoCollisionValue16)
+					{
+						min_value = min(min_value, src_range_block->mMin[i]);
+						max_value = max(max_value, src_range_block->mMax[i]);
+					}
+
+				// Write to destination block
+				RangeBlock *dst_range_block;
+				uint index_in_dst_block;
+				GetRangeBlock((block_start_x + block_x) >> 1, (block_start_y + block_y) >> 1, dst_range_block_offset, dst_range_block_stride, dst_range_block, index_in_dst_block);
+				dst_range_block->mMin[index_in_dst_block] = uint16(min_value);
+				dst_range_block->mMax[index_in_dst_block] = uint16(max_value);
+			}
+
+		// Go up one level
+		--max_level;
+		num_blocks >>= 1;
+		block_start_x >>= 1;
+		block_start_y >>= 1;
+		num_blocks_x = min((num_blocks_x + 1) >> 1, num_blocks);
+		num_blocks_y = min((num_blocks_y + 1) >> 1, num_blocks);
+
+		// Update stride and offset for source to old destination
+		range_block_offset = dst_range_block_offset;
+		range_block_stride = dst_range_block_stride;
+	}
+
+	// Calculate new min and max sample for the entire height field
+	mMinSample = 0xffff;
+	mMaxSample = 0;
+	for (uint i = 0; i < 4; ++i)
+		if (mRangeBlocks[0].mMin[i] != cNoCollisionValue16)
+		{
+			mMinSample = min(mMinSample, mRangeBlocks[0].mMin[i]);
+			mMaxSample = max(mMaxSample, mRangeBlocks[0].mMax[i]);
+		}
+
+#ifdef JPH_DEBUG_RENDERER
+	// Invalidate temporary rendering data
+	mGeometry.clear();
+#endif
+}
+
 MassProperties HeightFieldShape::GetMassProperties() const
 {
 	// Object should always be static, return default mass properties
@@ -872,6 +1255,8 @@ void HeightFieldShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg
 
 inline uint8 HeightFieldShape::GetEdgeFlags(uint inX, uint inY, uint inTriangle) const
 {
+	JPH_ASSERT(inX < mSampleCount - 1 && inY < mSampleCount - 1);
+
 	if (inTriangle == 0)
 	{
 		// The edge flags for this triangle are directly stored, find the right 3 bits
@@ -887,7 +1272,7 @@ inline uint8 HeightFieldShape::GetEdgeFlags(uint inX, uint inY, uint inTriangle)
 	{
 		// We don't store this triangle directly, we need to look at our three neighbours to construct the edge flags
 		uint8 edge0 = (GetEdgeFlags(inX, inY, 0) & 0b100) != 0? 0b001 : 0; // Diagonal edge
-		uint8 edge1 = inX == mSampleCount - 1 || (GetEdgeFlags(inX + 1, inY, 0) & 0b001) != 0? 0b010 : 0; // Vertical edge
+		uint8 edge1 = inX == mSampleCount - 2 || (GetEdgeFlags(inX + 1, inY, 0) & 0b001) != 0? 0b010 : 0; // Vertical edge
 		uint8 edge2 = inY == 0 || (GetEdgeFlags(inX, inY - 1, 0) & 0b010) != 0? 0b100 : 0; // Horizontal edge
 		return edge0 | edge1 | edge2;
 	}

Jolt/Physics/Collision/Shape/HeightFieldShape.h

@@ -14,24 +14,25 @@ JPH_NAMESPACE_BEGIN
 
 class ConvexShape;
 class CollideShapeSettings;
+class TempAllocator;
 
 /// Constants for HeightFieldShape, this was moved out of the HeightFieldShape because of a linker bug
 namespace HeightFieldShapeConstants
 {
 	/// Value used to create gaps in the height field
-	constexpr float			cNoCollisionValue = FLT_MAX;
+	constexpr float					cNoCollisionValue = FLT_MAX;
 
 	/// Stack size to use during WalkHeightField
-	constexpr int			cStackSize = 128;
+	constexpr int					cStackSize = 128;
 
 	/// A position in the hierarchical grid is defined by a level (which grid), x and y position. We encode this in a single uint32 as: level << 28 | y << 14 | x
-	constexpr uint			cNumBitsXY = 14;
-	constexpr uint			cMaskBitsXY = (1 << cNumBitsXY) - 1;
-	constexpr uint			cLevelShift = 2 * cNumBitsXY;
+	constexpr uint					cNumBitsXY = 14;
+	constexpr uint					cMaskBitsXY = (1 << cNumBitsXY) - 1;
+	constexpr uint					cLevelShift = 2 * cNumBitsXY;
 
 	/// When height samples are converted to 16 bit:
-	constexpr uint16		cNoCollisionValue16 = 0xffff;		///< This is the magic value for 'no collision'
-	constexpr uint16		cMaxHeightValue16 = 0xfffe;			///< This is the maximum allowed height value
+	constexpr uint16				cNoCollisionValue16 = 0xffff;				///< This is the magic value for 'no collision'
+	constexpr uint16				cMaxHeightValue16 = 0xfffe;					///< This is the maximum allowed height value
 };
 
 /// Class that constructs a HeightFieldShape
@@ -71,6 +72,12 @@ public:
 	Vec3							mScale = Vec3::sReplicate(1.0f);
 	uint32							mSampleCount = 0;
 
+	/// Artifical minimal value of mHeightSamples, used for compression and can be used to update the terrain after creating with lower height values. If there are any lower values in mHeightSamples, this value will be ignored.
+	float							mMinHeightValue = FLT_MAX;
+
+	/// Artifical maximum value of mHeightSamples, used for compression and can be used to update the terrain after creating with higher height values. If there are any higher values in mHeightSamples, this value will be ignored.
+	float							mMaxHeightValue = -FLT_MAX;
+
 	/// The heightfield is divided in blocks of mBlockSize * mBlockSize * 2 triangles and the acceleration structure culls blocks only,
 	/// bigger block sizes reduce memory consumption but also reduce query performance. Sensible values are [2, 8], does not need to be
 	/// a power of 2. Note that at run-time we'll perform one more grid subdivision, so the effective block size is half of what is provided here.
@@ -81,7 +88,10 @@ public:
 	/// Also note that increasing mBlockSize saves more memory than reducing the amount of bits per sample.
 	uint32							mBitsPerSample = 8;
 
+	/// An array of mSampleCount^2 height samples. Samples are stored in row major order, so the sample at (x, y) is at index y * mSampleCount + x.
 	Array<float>					mHeightSamples;
+
+	/// An array of (mSampleCount - 1)^2 material indices.
 	Array<uint8>					mMaterialIndices;
 
 	/// The materials of square at (x, y) is: mMaterials[mMaterialIndices[x + y * (mSampleCount - 1)]]
@@ -90,7 +100,7 @@ public:
 	/// Cosine of the threshold angle (if the angle between the two triangles is bigger than this, the edge is active, note that a concave edge is always inactive).
 	/// Setting this value too small can cause ghost collisions with edges, setting it too big can cause depenetration artifacts (objects not depenetrating quickly).
 	/// Valid ranges are between cos(0 degrees) and cos(90 degrees). The default value is cos(5 degrees).
-	float							mActiveEdgeCosThresholdAngle = 0.996195f;							// cos(5 degrees)
+	float							mActiveEdgeCosThresholdAngle = 0.996195f;	// cos(5 degrees)
 };
 
 /// A height field shape. Cannot be used as a dynamic object.
@@ -104,16 +114,22 @@ public:
 									HeightFieldShape(const HeightFieldShapeSettings &inSettings, ShapeResult &outResult);
 
 	// See Shape::MustBeStatic
-	virtual bool					MustBeStatic() const override										{ return true; }
+	virtual bool					MustBeStatic() const override				{ return true; }
+
+	/// Get the size of the height field. Note that this will always be rounded up to the nearest multiple of GetBlockSize().
+	inline uint						GetSampleCount() const						{ return mSampleCount; }
+
+	/// Get the size of a block
+	inline uint						GetBlockSize() const						{ return mBlockSize; }
 
 	// See Shape::GetLocalBounds
 	virtual AABox					GetLocalBounds() const override;
 
 	// See Shape::GetSubShapeIDBitsRecursive
-	virtual uint					GetSubShapeIDBitsRecursive() const override							{ return GetSubShapeIDBits(); }
+	virtual uint					GetSubShapeIDBitsRecursive() const override	{ return GetSubShapeIDBits(); }
 
 	// See Shape::GetInnerRadius
-	virtual float					GetInnerRadius() const override										{ return 0.0f; }
+	virtual float					GetInnerRadius() const override				{ return 0.0f; }
 
 	// See Shape::GetMassProperties
 	virtual MassProperties			GetMassProperties() const override;
@@ -165,6 +181,28 @@ public:
 	/// When there is no surface position (because of a hole or because the point is outside the heightfield) the function will return false.
 	bool							ProjectOntoSurface(Vec3Arg inLocalPosition, Vec3 &outSurfacePosition, SubShapeID &outSubShapeID) const;
 
+	/// Get the height values of a block of data.
+	/// Note that the height values are decompressed so will be slightly different from what the shape was originally created with.
+	/// @param inX Start X position, must be a multiple of mBlockSize and in the range [0, mSampleCount - 1]
+	/// @param inY Start Y position, must be a multiple of mBlockSize and in the range [0, mSampleCount - 1]
+	/// @param inSizeX Number of samples in X direction, must be a multiple of mBlockSize and in the range [0, mSampleCount - inX]
+	/// @param inSizeY Number of samples in Y direction, must be a multiple of mBlockSize and in the range [0, mSampleCount - inX]
+	/// @param outHeights Returned height values, must be at least inSizeX * inSizeY floats. Values are returned in x-major order and can be cNoCollisionValue.
+	/// @param inHeightsStride Stride in floats between two consecutive rows of outHeights.
+	void							GetHeights(uint inX, uint inY, uint inSizeX, uint inSizeY, float *outHeights, uint inHeightsStride) const;
+
+	/// Set the height values of a block of data.
+	/// Note that this requires decompressing and recompressing a border of size mBlockSize in the negative x/y direction so will cause some precision loss.
+	/// @param inX Start X position, must be a multiple of mBlockSize and in the range [0, mSampleCount - 1]
+	/// @param inY Start Y position, must be a multiple of mBlockSize and in the range [0, mSampleCount - 1]
+	/// @param inSizeX Number of samples in X direction, must be a multiple of mBlockSize and in the range [0, mSampleCount - inX]
+	/// @param inSizeY Number of samples in Y direction, must be a multiple of mBlockSize and in the range [0, mSampleCount - inX]
+	/// @param inHeights The new height values to set, must be an array of inSizeX * inSizeY floats, can be cNoCollisionValue.
+	/// @param inHeightsStride Stride in floats between two consecutive rows of outHeights.
+	/// @param inAllocator Allocator to use for temporary memory
+	/// @param inActiveEdgeCosThresholdAngle Cosine of the threshold angle (if the angle between the two triangles is bigger than this, the edge is active, note that a concave edge is always inactive).
+	void							SetHeights(uint inX, uint inY, uint inSizeX, uint inSizeY, const float *inHeights, uint inHeightsStride, TempAllocator &inAllocator, float inActiveEdgeCosThresholdAngle = 0.996195f);
+
 	// See Shape
 	virtual void					SaveBinaryState(StreamOut &inStream) const override;
 	virtual void					SaveMaterialState(PhysicsMaterialList &outMaterials) const override;
@@ -174,7 +212,7 @@ public:
 	virtual Stats					GetStats() const override;
 
 	// See Shape::GetVolume
-	virtual float					GetVolume() const override											{ return 0; }
+	virtual float					GetVolume() const override					{ return 0; }
 
 #ifdef JPH_DEBUG_RENDERER
 	// Settings
@@ -189,12 +227,15 @@ protected:
 	virtual void					RestoreBinaryState(StreamIn &inStream) override;
 
 private:
-	class							DecodingContext;						///< Context class for walking through all nodes of a heightfield
-	struct							HSGetTrianglesContext;					///< Context class for GetTrianglesStart/Next
+	class							DecodingContext;							///< Context class for walking through all nodes of a heightfield
+	struct							HSGetTrianglesContext;						///< Context class for GetTrianglesStart/Next
 
 	/// Calculate commonly used values and store them in the shape
 	void							CacheValues();
 
+	/// Calculate bit mask for all active edges in the heightfield for a specific region
+	void							CalculateActiveEdges(uint inX, uint inY, uint inSizeX, uint inSizeY, const float *inHeights, uint inHeightsStartX, uint inHeightsStartY, uint inHeightsStride, float inHeightsScale, float inActiveEdgeCosThresholdAngle, TempAllocator &inAllocator);
+
 	/// Calculate bit mask for all active edges in the heightfield
 	void							CalculateActiveEdges(const HeightFieldShapeSettings &inSettings);
 
@@ -202,10 +243,10 @@ private:
 	void							StoreMaterialIndices(const HeightFieldShapeSettings &inSettings);
 
 	/// Get the amount of horizontal/vertical blocks
-	inline uint						GetNumBlocks() const					{ return mSampleCount / mBlockSize; }
+	inline uint						GetNumBlocks() const						{ return mSampleCount / mBlockSize; }
 
 	/// Get the maximum level (amount of grids) of the tree
-	static inline uint				sGetMaxLevel(uint inNumBlocks)			{ return 32 - CountLeadingZeros(inNumBlocks - 1); }
+	static inline uint				sGetMaxLevel(uint inNumBlocks)				{ return 32 - CountLeadingZeros(inNumBlocks - 1); }
 
 	/// Get the range block offset and stride for GetBlockOffsetAndScale
 	static inline void				sGetRangeBlockOffsetAndStride(uint inNumBlocks, uint inMaxLevel, uint &outRangeBlockOffset, uint &outRangeBlockStride);
@@ -246,6 +287,9 @@ private:
 		uint16						mMax[4];
 	};
 
+	/// For block (inBlockX, inBlockY) get get the range block and the entry in the range block
+	inline void						GetRangeBlock(uint inBlockX, uint inBlockY, uint inRangeBlockOffset, uint inRangeBlockStride, RangeBlock *&outBlock, uint &outIndexInBlock);
+
 	/// Offset of first RangedBlock in grid per level
 	static const uint				sGridOffsets[];
 
@@ -255,25 +299,25 @@ private:
 	Vec3							mScale = Vec3::sReplicate(1.0f);
 
 	/// Height data
-	uint32							mSampleCount = 0;					///< See HeightFieldShapeSettings::mSampleCount
-	uint32							mBlockSize = 2;						///< See HeightFieldShapeSettings::mBlockSize
-	uint8							mBitsPerSample = 8;					///< See HeightFieldShapeSettings::mBitsPerSample
-	uint8							mSampleMask = 0xff;					///< All bits set for a sample: (1 << mBitsPerSample) - 1, used to indicate that there's no collision
-	uint16							mMinSample = HeightFieldShapeConstants::cNoCollisionValue16;	///< Min and max value in mHeightSamples quantized to 16 bit, for calculating bounding box
+	uint32							mSampleCount = 0;							///< See HeightFieldShapeSettings::mSampleCount
+	uint32							mBlockSize = 2;								///< See HeightFieldShapeSettings::mBlockSize
+	uint8							mBitsPerSample = 8;							///< See HeightFieldShapeSettings::mBitsPerSample
+	uint8							mSampleMask = 0xff;							///< All bits set for a sample: (1 << mBitsPerSample) - 1, used to indicate that there's no collision
+	uint16							mMinSample = HeightFieldShapeConstants::cNoCollisionValue16; ///< Min and max value in mHeightSamples quantized to 16 bit, for calculating bounding box
 	uint16							mMaxSample = HeightFieldShapeConstants::cNoCollisionValue16;
-	Array<RangeBlock>				mRangeBlocks;						///< Hierarchical grid of range data describing the height variations within 1 block. The grid for level <level> starts at offset sGridOffsets[<level>]
-	Array<uint8>					mHeightSamples;						///< mBitsPerSample-bit height samples. Value [0, mMaxHeightValue] maps to highest detail grid in mRangeBlocks [mMin, mMax]. mNoCollisionValue is reserved to indicate no collision.
-	Array<uint8>					mActiveEdges;						///< (mSampleCount - 1)^2 * 3-bit active edge flags.
+	Array<RangeBlock>				mRangeBlocks;								///< Hierarchical grid of range data describing the height variations within 1 block. The grid for level <level> starts at offset sGridOffsets[<level>]
+	Array<uint8>					mHeightSamples;								///< mBitsPerSample-bit height samples. Value [0, mMaxHeightValue] maps to highest detail grid in mRangeBlocks [mMin, mMax]. mNoCollisionValue is reserved to indicate no collision.
+	Array<uint8>					mActiveEdges;								///< (mSampleCount - 1)^2 * 3-bit active edge flags.
 
 	/// Materials
-	PhysicsMaterialList				mMaterials;							///< The materials of square at (x, y) is: mMaterials[mMaterialIndices[x + y * (mSampleCount - 1)]]
-	Array<uint8>					mMaterialIndices;					///< Compressed to the minimum amount of bits per material index (mSampleCount - 1) * (mSampleCount - 1) * mNumBitsPerMaterialIndex bits of data
-	uint32							mNumBitsPerMaterialIndex = 0;		///< Number of bits per material index
+	PhysicsMaterialList				mMaterials;									///< The materials of square at (x, y) is: mMaterials[mMaterialIndices[x + y * (mSampleCount - 1)]]
+	Array<uint8>					mMaterialIndices;							///< Compressed to the minimum amount of bits per material index (mSampleCount - 1) * (mSampleCount - 1) * mNumBitsPerMaterialIndex bits of data
+	uint32							mNumBitsPerMaterialIndex = 0;				///< Number of bits per material index
 
 #ifdef JPH_DEBUG_RENDERER
 	/// Temporary rendering data
 	mutable Array<DebugRenderer::GeometryRef> mGeometry;
-	mutable bool					mCachedUseMaterialColors = false;	///< This is used to regenerate the triangle batch if the drawing settings change
+	mutable bool					mCachedUseMaterialColors = false;			///< This is used to regenerate the triangle batch if the drawing settings change
 #endif // JPH_DEBUG_RENDERER
 };
 

Samples/Samples.cmake

@@ -211,6 +211,8 @@ set(SAMPLES_SRC_FILES
 	${SAMPLES_ROOT}/Tests/Shapes/BoxShapeTest.h
 	${SAMPLES_ROOT}/Tests/Shapes/CapsuleShapeTest.cpp
 	${SAMPLES_ROOT}/Tests/Shapes/CapsuleShapeTest.h
+	${SAMPLES_ROOT}/Tests/Shapes/DeformedHeightFieldShapeTest.cpp
+	${SAMPLES_ROOT}/Tests/Shapes/DeformedHeightFieldShapeTest.h
 	${SAMPLES_ROOT}/Tests/Shapes/StaticCompoundShapeTest.cpp
 	${SAMPLES_ROOT}/Tests/Shapes/StaticCompoundShapeTest.h
 	${SAMPLES_ROOT}/Tests/Shapes/MutableCompoundShapeTest.cpp

Samples/SamplesApp.cpp

@@ -198,6 +198,7 @@ JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, TriangleShapeTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, ConvexHullShapeTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, MeshShapeTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, HeightFieldShapeTest)
+JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, DeformedHeightFieldShapeTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, RotatedTranslatedShapeTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, OffsetCenterOfMassShapeTest)
 
@@ -211,6 +212,7 @@ static TestNameAndRTTI sShapeTests[] =
 	{ "Convex Hull Shape",					JPH_RTTI(ConvexHullShapeTest) },
 	{ "Mesh Shape",							JPH_RTTI(MeshShapeTest) },
 	{ "Height Field Shape",					JPH_RTTI(HeightFieldShapeTest) },
+	{ "Deformed Height Field Shape",		JPH_RTTI(DeformedHeightFieldShapeTest) },
 	{ "Static Compound Shape",				JPH_RTTI(StaticCompoundShapeTest) },
 	{ "Mutable Compound Shape",				JPH_RTTI(MutableCompoundShapeTest) },
 	{ "Triangle Shape",						JPH_RTTI(TriangleShapeTest) },

Samples/Tests/Shapes/DeformedHeightFieldShapeTest.cpp

@@ -0,0 +1,125 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/Shapes/DeformedHeightFieldShapeTest.h>
+#include <Math/Perlin.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/ShapeCast.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(DeformedHeightFieldShapeTest)
+{
+	JPH_ADD_BASE_CLASS(DeformedHeightFieldShapeTest, Test)
+}
+
+void DeformedHeightFieldShapeTest::Initialize()
+{
+	constexpr float cCellSize = 1.0f;
+	constexpr float cMaxHeight = 2.5f;
+	constexpr float cSphereRadius = 2.0f;
+
+	// Create height samples
+	mHeightSamples.resize(cSampleCount * cSampleCount);
+	for (int y = 0; y < cSampleCount; ++y)
+		for (int x = 0; x < cSampleCount; ++x)
+			mHeightSamples[y * cSampleCount + x] = cMaxHeight * PerlinNoise3(float(x) * 8.0f / cSampleCount, 0, float(y) * 8.0f / cSampleCount, 256, 256, 256);
+
+	// Determine scale and offset of the terrain
+	Vec3 offset(-0.5f * cCellSize * cSampleCount, 0, -0.5f * cCellSize * cSampleCount);
+	Vec3 scale(cCellSize, 1.0f, cCellSize);
+
+	// Create height field
+	HeightFieldShapeSettings settings(mHeightSamples.data(), offset, scale, cSampleCount);
+	settings.mBlockSize = cBlockSize;
+	settings.mBitsPerSample = 8;
+	settings.mMinHeightValue = -15.0f;
+	mHeightField = static_cast<HeightFieldShape *>(settings.Create().Get().GetPtr());
+	mHeightFieldID = mBodyInterface->CreateAndAddBody(BodyCreationSettings(mHeightField, RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+
+	// Spheres on top of the terrain
+	RefConst<Shape> sphere_shape = new SphereShape(cSphereRadius);
+	for (float t = 0.2f; t < 12.4f; t += 0.1f)
+	{
+		// Get the center of the path
+		Vec3 center = offset + GetPathCenter(t);
+
+		// Cast a ray onto the terrain
+		RShapeCast shape_cast(sphere_shape, Vec3::sReplicate(1.0f), RMat44::sTranslation(RVec3(0, 10, 0) + center), Vec3(0, -20, 0));
+		ClosestHitCollisionCollector<CastShapeCollector> collector;
+		mPhysicsSystem->GetNarrowPhaseQuery().CastShape(shape_cast, { }, RVec3::sZero(), collector);
+		if (collector.mHit.mBodyID2 == mHeightFieldID)
+		{
+			// Create sphere on terrain
+			BodyCreationSettings bcs(sphere_shape, shape_cast.GetPointOnRay(collector.mHit.mFraction), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+			mBodyInterface->CreateAndAddBody(bcs, EActivation::DontActivate);
+		}
+	}
+}
+
+Vec3 DeformedHeightFieldShapeTest::GetPathCenter(float inTime) const
+{
+	constexpr float cOffset = 5.0f;
+	constexpr float cRadiusX = 60.0f;
+	constexpr float cRadiusY = 25.0f;
+	constexpr float cFallOff = 0.1f;
+	constexpr float cAngularSpeed = 2.0f;
+	constexpr float cDisplacementSpeed = 10.0f;
+
+	float fall_off = exp(-cFallOff * inTime);
+	float angle = cAngularSpeed * inTime;
+	return Vec3(cRadiusX * Cos(angle) * fall_off + 64.0f, 0, cOffset + cDisplacementSpeed * inTime + cRadiusY * Sin(angle) * fall_off);
+}
+
+void DeformedHeightFieldShapeTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
+{
+	constexpr float cPitRadius = 6.0f;
+	constexpr float cPitHeight = 1.0f;
+	constexpr float cSpeedScale = 2.0f;
+
+	// Calculate center of pit
+	Vec3 center = GetPathCenter(cSpeedScale * mTime);
+	mTime += inParams.mDeltaTime;
+
+	// Calculate affected area
+	int start_x = max((int)floor(center.GetX() - cPitRadius) & ~cBlockMask, 0);
+	int start_y = max((int)floor(center.GetZ() - cPitRadius) & ~cBlockMask, 0);
+	int count_x = min(((int)ceil(center.GetX() + cPitRadius) + cBlockMask) & ~cBlockMask, cSampleCount) - start_x;
+	int count_y = min(((int)ceil(center.GetZ() + cPitRadius) + cBlockMask) & ~cBlockMask, cSampleCount) - start_y;
+
+	if (count_x > 0 && count_y > 0)
+	{
+		// Remember COM before we change the height field
+		Vec3 old_com = mHeightField->GetCenterOfMass();
+
+		// A function to calculate the delta height at a certain distance from the center of the pit
+		constexpr float cHalfPi = 0.5f * JPH_PI;
+		auto pit_shape = [=](float inDistanceX, float inDistanceY) { return Cos(min(sqrt(Square(inDistanceX) + Square(inDistanceY)) * cHalfPi / cPitRadius, cHalfPi)); };
+
+		AABox affected_area;
+		for (int y = 0; y < count_y; ++y)
+			for (int x = 0; x < count_x; ++x)
+			{
+				// Update the height field
+				float delta = pit_shape(float(start_x) + x - center.GetX(), float(start_y) + y - center.GetZ()) * cPitHeight;
+				mHeightSamples[(start_y + y) * cSampleCount + start_x + x] -= delta;
+
+				// Keep track of affected area to wake up bodies
+				affected_area.Encapsulate(mHeightField->GetPosition(start_x + x, start_y + y));
+			}
+		mHeightField->SetHeights(start_x, start_y, count_x, count_y, mHeightSamples.data() + start_y * cSampleCount + start_x, cSampleCount, *mTempAllocator);
+
+		// Notify the shape that it has changed its bounding box
+		mBodyInterface->NotifyShapeChanged(mHeightFieldID, old_com, false, EActivation::DontActivate);
+
+		// Activate bodies in the affected area (a change in the height field doesn't wake up bodies)
+		affected_area.ExpandBy(Vec3::sReplicate(0.1f));
+		DefaultBroadPhaseLayerFilter broadphase_layer_filter = mPhysicsSystem->GetDefaultBroadPhaseLayerFilter(Layers::MOVING);
+		DefaultObjectLayerFilter object_layer_filter = mPhysicsSystem->GetDefaultLayerFilter(Layers::MOVING);
+		mBodyInterface->ActivateBodiesInAABox(affected_area, broadphase_layer_filter, object_layer_filter);
+	}
+}

Samples/Tests/Shapes/DeformedHeightFieldShapeTest.h

@@ -0,0 +1,49 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+#include <Jolt/Physics/Collision/Shape/HeightFieldShape.h>
+
+// This test shows how to deform a height field shape after it has been created
+class DeformedHeightFieldShapeTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, DeformedHeightFieldShapeTest)
+
+	// Initialize the test
+	virtual void			Initialize() override;
+
+	// Update the test, called before the physics update
+	virtual void			PrePhysicsUpdate(const PreUpdateParams &inParams) override;
+
+	// Test will never be deterministic since we're modifying the height field shape and not saving it
+	virtual bool			IsDeterministic() const override							{ return false; }
+
+private:
+	// Get the center of the path at time inTime, this follows a path that resembles the Jolt logo
+	Vec3					GetPathCenter(float inTime) const;
+
+	// Size of the terrarin
+	static constexpr int	cSampleCount = 128;
+
+	// Size of a block in the terrain
+	static constexpr int	cBlockSize = 4;
+
+	// Bits to mask out index within a block
+	static constexpr int	cBlockMask = cBlockSize - 1;
+
+	// The list of original height samples, we keep this to avoid precision loss of repeatedly decompressing and recompressing height samples
+	Array<float>			mHeightSamples;
+
+	// The height field shape
+	Ref<HeightFieldShape>	mHeightField;
+
+	// ID of the height field body
+	BodyID					mHeightFieldID;
+
+	// Current time
+	float					mTime = 0.0f;
+};

UnitTests/Physics/HeightFieldShapeTests.cpp

@@ -196,4 +196,128 @@ TEST_SUITE("HeightFieldShapeTests")
 		CHECK(stats.mNumTriangles == 0);
 		CHECK(stats.mSizeBytes == sizeof(HeightFieldShape));
 	}
+
+	TEST_CASE("TestGetHeights")
+	{
+		const float cMinHeight = -5.0f;
+		const float cMaxHeight = 10.0f;
+		const uint cSampleCount = 32;
+		const uint cNoCollisionIndex = 10;
+
+		UnitTestRandom random;
+		uniform_real_distribution<float> height_distribution(cMinHeight, cMaxHeight);
+
+		// Create height field with random samples
+		HeightFieldShapeSettings settings;
+		settings.mOffset = Vec3(0.3f, 0.5f, 0.7f);
+		settings.mScale = Vec3(1.1f, 1.2f, 1.3f);
+		settings.mSampleCount = cSampleCount;
+		settings.mBitsPerSample = 8;
+		settings.mBlockSize = 4;
+		settings.mHeightSamples.resize(Square(cSampleCount));
+		for (float &h : settings.mHeightSamples)
+			h = height_distribution(random);
+
+		// Add 1 sample that has no collision
+		settings.mHeightSamples[cNoCollisionIndex] = HeightFieldShapeConstants::cNoCollisionValue;
+
+		// Create shape
+		ShapeRefC shape = settings.Create().Get();
+		const HeightFieldShape *height_field = static_cast<const HeightFieldShape *>(shape.GetPtr());
+
+		{
+			// Check that the GetHeights function returns the same values as the original height samples
+			Array<float> sampled_heights;
+			sampled_heights.resize(Square(cSampleCount));
+			height_field->GetHeights(0, 0, cSampleCount, cSampleCount, sampled_heights.data(), cSampleCount);
+			for (uint i = 0; i < Square(cSampleCount); ++i)
+				if (i == cNoCollisionIndex)
+					CHECK(sampled_heights[i] == HeightFieldShapeConstants::cNoCollisionValue);
+				else
+					CHECK_APPROX_EQUAL(sampled_heights[i], settings.mOffset.GetY() + settings.mScale.GetY() * settings.mHeightSamples[i], 0.05f);
+		}
+
+		{
+			// With a random height field the max error is going to be limited by the amount of bits we have per sample as we will not get any benefit from a reduced range per block
+			float tolerance = (cMaxHeight - cMinHeight) / ((1 << settings.mBitsPerSample) - 2);
+
+			// Check a sub rect of the height field
+			uint sx = 4, sy = 8, cx = 16, cy = 8;
+			Array<float> sampled_heights;
+			sampled_heights.resize(cx * cy);
+			height_field->GetHeights(sx, sy, cx, cy, sampled_heights.data(), cx);
+			for (uint y = 0; y < cy; ++y)
+				for (uint x = 0; x < cx; ++x)
+					CHECK_APPROX_EQUAL(sampled_heights[y * cx + x], settings.mOffset.GetY() + settings.mScale.GetY() * settings.mHeightSamples[(sy + y) * cSampleCount + sx + x], tolerance);
+		}
+	}
+
+	TEST_CASE("TestSetHeights")
+	{
+		const float cMinHeight = -5.0f;
+		const float cMaxHeight = 10.0f;
+		const uint cSampleCount = 32;
+
+		UnitTestRandom random;
+		uniform_real_distribution<float> height_distribution(cMinHeight, cMaxHeight);
+
+		// Create height field with random samples
+		HeightFieldShapeSettings settings;
+		settings.mOffset = Vec3(0.3f, 0.5f, 0.7f);
+		settings.mScale = Vec3(1.1f, 1.2f, 1.3f);
+		settings.mSampleCount = cSampleCount;
+		settings.mBitsPerSample = 8;
+		settings.mBlockSize = 4;
+		settings.mHeightSamples.resize(Square(cSampleCount));
+		settings.mMinHeightValue = cMinHeight;
+		settings.mMaxHeightValue = cMaxHeight;
+		for (float &h : settings.mHeightSamples)
+			h = height_distribution(random);
+
+		// Create shape
+		Ref<Shape> shape = settings.Create().Get();
+		HeightFieldShape *height_field = static_cast<HeightFieldShape *>(shape.GetPtr());
+
+		// Get the original (quantized) heights
+		Array<float> original_heights;
+		original_heights.resize(Square(cSampleCount));
+		height_field->GetHeights(0, 0, cSampleCount, cSampleCount, original_heights.data(), cSampleCount);
+
+		// Create new data for height field
+		Array<float> patched_heights;
+		uint sx = 4, sy = 16, cx = 16, cy = 8;
+		patched_heights.resize(cx * cy);
+		for (uint y = 0; y < cy; ++y)
+			for (uint x = 0; x < cx; ++x)
+				patched_heights[y * cx + x] = height_distribution(random);
+
+		// Add 1 sample that has no collision
+		uint no_collision_idx = (sy + 1) * cSampleCount + sx + 2;
+		patched_heights[1 * cx + 2] = HeightFieldShapeConstants::cNoCollisionValue;
+
+		// Update the height field
+		TempAllocatorMalloc temp_allocator;
+		height_field->SetHeights(sx, sy, cx, cy, patched_heights.data(), cx, temp_allocator);
+
+		// With a random height field the max error is going to be limited by the amount of bits we have per sample as we will not get any benefit from a reduced range per block
+		float tolerance = (cMaxHeight - cMinHeight) / ((1 << settings.mBitsPerSample) - 2);
+
+		// Check a sub rect of the height field
+		Array<float> verify_heights;
+		verify_heights.resize(cSampleCount * cSampleCount);
+		height_field->GetHeights(0, 0, cSampleCount, cSampleCount, verify_heights.data(), cSampleCount);
+		for (uint y = 0; y < cSampleCount; ++y)
+			for (uint x = 0; x < cSampleCount; ++x)
+			{
+				uint idx = y * cSampleCount + x;
+				if (idx == no_collision_idx)
+					CHECK(verify_heights[idx] == HeightFieldShapeConstants::cNoCollisionValue);
+				else if (x >= sx && x < sx + cx && y >= sy && y < sy + cy)
+					CHECK_APPROX_EQUAL(verify_heights[y * cSampleCount + x], patched_heights[(y - sy) * cx + x - sx], tolerance);
+				else if (x >= sx - settings.mBlockSize && x < sx + cx && y >= sy - settings.mBlockSize && y < sy + cy)
+					CHECK_APPROX_EQUAL(verify_heights[idx], original_heights[idx], tolerance); // We didn't modify this but it has been quantized again
+				else
+					CHECK(verify_heights[idx] == original_heights[idx]); // We didn't modify this and it is outside of the affected range
+			}
+	}
 }