@bloomengine/engine 0.3.3 → 0.4.1

package/native/third_party/JoltPhysics/Jolt/Physics/Collision/Shape/HeightFieldShape.cpp

@@ -1,2754 +0,0 @@
-// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
-// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
-// SPDX-License-Identifier: MIT
-
-#include <Jolt/Jolt.h>
-
-#include <Jolt/Physics/Collision/Shape/HeightFieldShape.h>
-#include <Jolt/Physics/Collision/Shape/ConvexShape.h>
-#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
-#include <Jolt/Physics/Collision/Shape/SphereShape.h>
-#include <Jolt/Physics/Collision/RayCast.h>
-#include <Jolt/Physics/Collision/ShapeCast.h>
-#include <Jolt/Physics/Collision/CastResult.h>
-#include <Jolt/Physics/Collision/CollidePointResult.h>
-#include <Jolt/Physics/Collision/ShapeFilter.h>
-#include <Jolt/Physics/Collision/CastConvexVsTriangles.h>
-#include <Jolt/Physics/Collision/CastSphereVsTriangles.h>
-#include <Jolt/Physics/Collision/CollideConvexVsTriangles.h>
-#include <Jolt/Physics/Collision/CollideSphereVsTriangles.h>
-#include <Jolt/Physics/Collision/TransformedShape.h>
-#include <Jolt/Physics/Collision/ActiveEdges.h>
-#include <Jolt/Physics/Collision/CollisionDispatch.h>
-#include <Jolt/Physics/Collision/SortReverseAndStore.h>
-#include <Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h>
-#include <Jolt/Core/Profiler.h>
-#include <Jolt/Core/StringTools.h>
-#include <Jolt/Core/StreamIn.h>
-#include <Jolt/Core/StreamOut.h>
-#include <Jolt/Core/TempAllocator.h>
-#include <Jolt/Core/ScopeExit.h>
-#include <Jolt/Geometry/AABox4.h>
-#include <Jolt/Geometry/RayTriangle.h>
-#include <Jolt/Geometry/RayAABox.h>
-#include <Jolt/Geometry/OrientedBox.h>
-#include <Jolt/ObjectStream/TypeDeclarations.h>
-
-//#define JPH_DEBUG_HEIGHT_FIELD
-
-JPH_NAMESPACE_BEGIN
-
-#ifdef JPH_DEBUG_RENDERER
-bool HeightFieldShape::sDrawTriangleOutlines = false;
-#endif // JPH_DEBUG_RENDERER
-
-using namespace HeightFieldShapeConstants;
-
-JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(HeightFieldShapeSettings)
-{
-	JPH_ADD_BASE_CLASS(HeightFieldShapeSettings, ShapeSettings)
-
-	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, mMaterialsCapacity)
-	JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mSampleCount)
-	JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mBlockSize)
-	JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mBitsPerSample)
-	JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mMaterialIndices)
-	JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mMaterials)
-	JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mActiveEdgeCosThresholdAngle)
-}
-
-const uint HeightFieldShape::sGridOffsets[] =
-{
-	0,			// level:  0, max x/y:     0, offset: 0
-	1,			// level:  1, max x/y:     1, offset: 1
-	5,			// level:  2, max x/y:     3, offset: 1 + 4
-	21,			// level:  3, max x/y:     7, offset: 1 + 4 + 16
-	85,			// level:  4, max x/y:    15, offset: 1 + 4 + 16 + 64
-	341,		// level:  5, max x/y:    31, offset: 1 + 4 + 16 + 64 + 256
-	1365,		// level:  6, max x/y:    63, offset: 1 + 4 + 16 + 64 + 256 + 1024
-	5461,		// level:  7, max x/y:   127, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096
-	21845,		// level:  8, max x/y:   255, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ...
-	87381,		// level:  9, max x/y:   511, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ...
-	349525,		// level: 10, max x/y:  1023, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ...
-	1398101,	// level: 11, max x/y:  2047, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ...
-	5592405,	// level: 12, max x/y:  4095, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ...
-	22369621,	// level: 13, max x/y:  8191, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ...
-	89478485,	// level: 14, max x/y: 16383, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ...
-};
-
-HeightFieldShapeSettings::HeightFieldShapeSettings(const float *inSamples, Vec3Arg inOffset, Vec3Arg inScale, uint32 inSampleCount, const uint8 *inMaterialIndices, const PhysicsMaterialList &inMaterialList) :
-	mOffset(inOffset),
-	mScale(inScale),
-	mSampleCount(inSampleCount)
-{
-	mHeightSamples.assign(inSamples, inSamples + Square(inSampleCount));
-
-	if (!inMaterialList.empty() && inMaterialIndices != nullptr)
-	{
-		mMaterialIndices.assign(inMaterialIndices, inMaterialIndices + Square(inSampleCount - 1));
-		mMaterials = inMaterialList;
-	}
-	else
-	{
-		JPH_ASSERT(inMaterialList.empty());
-		JPH_ASSERT(inMaterialIndices == nullptr);
-	}
-}
-
-ShapeSettings::ShapeResult HeightFieldShapeSettings::Create() const
-{
-	if (mCachedResult.IsEmpty())
-		Ref<Shape> shape = new HeightFieldShape(*this, mCachedResult);
-	return mCachedResult;
-}
-
-void HeightFieldShapeSettings::DetermineMinAndMaxSample(float &outMinValue, float &outMaxValue, float &outQuantizationScale) const
-{
-	// Determine min and max value
-	outMinValue = mMinHeightValue;
-	outMaxValue = mMaxHeightValue;
-	for (float h : mHeightSamples)
-		if (h != cNoCollisionValue)
-		{
-			outMinValue = min(outMinValue, h);
-			outMaxValue = max(outMaxValue, h);
-		}
-
-	// Prevent dividing by zero by setting a minimal height difference
-	float height_diff = max(outMaxValue - outMinValue, 1.0e-6f);
-
-	// Calculate the scale factor to quantize to 16 bits
-	outQuantizationScale = float(cMaxHeightValue16) / height_diff;
-}
-
-uint32 HeightFieldShapeSettings::CalculateBitsPerSampleForError(float inMaxError) const
-{
-	// Start with 1 bit per sample
-	uint32 bits_per_sample = 1;
-
-	// Determine total range
-	float min_value, max_value, scale;
-	DetermineMinAndMaxSample(min_value, max_value, scale);
-	if (min_value < max_value)
-	{
-		// Loop over all blocks
-		for (uint y = 0; y < mSampleCount; y += mBlockSize)
-			for (uint x = 0; x < mSampleCount; x += mBlockSize)
-			{
-				// Determine min and max block value + take 1 sample border just like we do while building the hierarchical grids
-				float block_min_value = FLT_MAX, block_max_value = -FLT_MAX;
-				for (uint bx = x; bx < min(x + mBlockSize + 1, mSampleCount); ++bx)
-					for (uint by = y; by < min(y + mBlockSize + 1, mSampleCount); ++by)
-					{
-						float h = mHeightSamples[by * mSampleCount + bx];
-						if (h != cNoCollisionValue)
-						{
-							block_min_value = min(block_min_value, h);
-							block_max_value = max(block_max_value, h);
-						}
-					}
-
-				if (block_min_value < block_max_value)
-				{
-					// Quantize then dequantize block min/max value
-					block_min_value = min_value + floor((block_min_value - min_value) * scale) / scale;
-					block_max_value = min_value + ceil((block_max_value - min_value) * scale) / scale;
-					float block_height = block_max_value - block_min_value;
-
-					// Loop over the block again
-					for (uint bx = x; bx < x + mBlockSize; ++bx)
-						for (uint by = y; by < y + mBlockSize; ++by)
-						{
-							// Get the height
-							float height = mHeightSamples[by * mSampleCount + bx];
-							if (height != cNoCollisionValue)
-							{
-								for (;;)
-								{
-									// Determine bitmask for sample
-									uint32 sample_mask = (1 << bits_per_sample) - 1;
-
-									// Quantize
-									float quantized_height = floor((height - block_min_value) * float(sample_mask) / block_height);
-									quantized_height = Clamp(quantized_height, 0.0f, float(sample_mask - 1));
-
-									// Dequantize and check error
-									float dequantized_height = block_min_value + (quantized_height + 0.5f) * block_height / float(sample_mask);
-									if (abs(dequantized_height - height) <= inMaxError)
-										break;
-
-									// Not accurate enough, increase bits per sample
-									bits_per_sample++;
-
-									// Don't go above 8 bits per sample
-									if (bits_per_sample == 8)
-										return bits_per_sample;
-								}
-							}
-						}
-				}
-			}
-
-	}
-
-	return bits_per_sample;
-}
-
-void HeightFieldShape::CalculateActiveEdges(uint inX, uint inY, uint inSizeX, uint inSizeY, const float *inHeights, uint inHeightsStartX, uint inHeightsStartY, intptr_t inHeightsStride, float inHeightsScale, float inActiveEdgeCosThresholdAngle, TempAllocator &inAllocator)
-{
-	// Limit the block size so we don't allocate more than 64K memory from the temp allocator
-	uint block_size_x = min(inSizeX, 44u);
-	uint block_size_y = min(inSizeY, 44u);
-
-	// Allocate temporary buffer for normals
-	uint normals_size = 2 * (block_size_x + 1) * (block_size_y + 1) * sizeof(Vec3);
-	Vec3 *normals = (Vec3 *)inAllocator.Allocate(normals_size);
-	JPH_SCOPE_EXIT([&inAllocator, normals, normals_size]{ inAllocator.Free(normals, normals_size); });
-
-	// Update the edges in blocks
-	for (uint block_y = 0; block_y < inSizeY; block_y += block_size_y)
-		for (uint block_x = 0; block_x < inSizeX; block_x += block_size_x)
-		{
-			// Calculate the bottom right corner of the block
-			uint block_x_end = min(block_x + block_size_x, inSizeX);
-			uint block_y_end = min(block_y + block_size_y, inSizeY);
-
-			// If we're not at the first block in x, we need one extra column of normals to the left
-			uint normals_x_start, normals_x_skip;
-			if (block_x > 0)
-			{
-				normals_x_start = block_x - 1;
-				normals_x_skip = 2; // We need to skip over that extra column
-			}
-			else
-			{
-				normals_x_start = 0;
-				normals_x_skip = 0;
-			}
-
-			// If we're not at the last block in y, we need one extra row of normals at the bottom
-			uint normals_y_end = block_y_end < inSizeY? block_y_end + 1 : inSizeY;
-
-			// Calculate triangle normals and make normals zero for triangles that are missing
-			Vec3 *out_normal = normals;
-			for (uint y = block_y; y < normals_y_end; ++y)
-			{
-				for (uint x = normals_x_start; x < block_x_end; ++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)
-					{
-						// Calculate normal for lower left triangle (e.g. T1A)
-						float x1y2_h = height_samples[inHeightsStride];
-						if (x1y2_h != cNoCollisionValue)
-						{
-							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();
-
-						// Calculate normal for upper right triangle (e.g. T1B)
-						float x2y1_h = height_samples[1];
-						if (x2y1_h != cNoCollisionValue)
-						{
-							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;
-				}
-			}
-
-			// Number of vectors to skip to get to the next row of normals
-			uint normals_pitch = 2 * (block_x_end - normals_x_start);
-
-			// Calculate active edges
-			const Vec3 *in_normal = normals;
-			uint global_bit_pos = 3 * ((inY + block_y) * (mSampleCount - 1) + (inX + block_x));
-			for (uint y = block_y; y < block_y_end; ++y)
-			{
-				in_normal += normals_x_skip; // If we have an extra column to the left, skip it here, we'll read it with in_normal[-1] below
-
-				for (uint x = block_x; x < block_x_end; ++x)
-				{
-					// 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)
-					// 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[normals_pitch + 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 byte_pos = global_bit_pos >> 3;
-					uint bit_pos = global_bit_pos & 0b111;
-					JPH_ASSERT(byte_pos < mActiveEdgesSize);
-					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 - (block_x_end - block_x));
-			}
-		}
-}
-
-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
-	*/
-
-	// 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, 0xff, mActiveEdgesSize);
-
-	// 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)
-{
-	// We need to account for any rounding of the sample count to the nearest block size
-	uint in_count_min_1 = inSettings.mSampleCount - 1;
-	uint out_count_min_1 = mSampleCount - 1;
-
-	mNumBitsPerMaterialIndex = 32 - CountLeadingZeros(max((uint32)mMaterials.size(), inSettings.mMaterialsCapacity) - 1);
-	mMaterialIndices.resize(((Square(out_count_min_1) * mNumBitsPerMaterialIndex + 7) >> 3) + 1, 0); // Add 1 byte so we don't read out of bounds when reading an uint16
-
-	if (mMaterials.size() > 1)
-		for (uint y = 0; y < out_count_min_1; ++y)
-			for (uint x = 0; x < out_count_min_1; ++x)
-			{
-				// Read material
-				uint16 material_index = x < in_count_min_1 && y < in_count_min_1? uint16(inSettings.mMaterialIndices[x + y * in_count_min_1]) : 0;
-
-				// Calculate byte and bit position where the material index needs to go
-				uint sample_pos = x + y * out_count_min_1;
-				uint bit_pos = sample_pos * mNumBitsPerMaterialIndex;
-				uint byte_pos = bit_pos >> 3;
-				bit_pos &= 0b111;
-
-				// Write the material index
-				material_index <<= bit_pos;
-				JPH_ASSERT(byte_pos + 1 < mMaterialIndices.size());
-				mMaterialIndices[byte_pos] |= uint8(material_index);
-				mMaterialIndices[byte_pos + 1] |= uint8(material_index >> 8);
-			}
-}
-
-void HeightFieldShape::CacheValues()
-{
-	mSampleMask = uint8((uint32(1) << mBitsPerSample) - 1);
-}
-
-void HeightFieldShape::AllocateBuffers()
-{
-	uint num_blocks = GetNumBlocks();
-	uint max_stride = (num_blocks + 1) >> 1;
-	mRangeBlocksSize = sGridOffsets[sGetMaxLevel(num_blocks) - 1] + Square(max_stride);
-	mHeightSamplesSize = (mSampleCount * mSampleCount * mBitsPerSample + 7) / 8 + 1;
-	mActiveEdgesSize = (Square(mSampleCount - 1) * 3 + 7) / 8 + 1; // See explanation at HeightFieldShape::CalculateActiveEdges
-
-	JPH_ASSERT(mRangeBlocks == nullptr && mHeightSamples == nullptr && mActiveEdges == nullptr);
-	void *data = AlignedAllocate(mRangeBlocksSize * sizeof(RangeBlock) + mHeightSamplesSize + mActiveEdgesSize, alignof(RangeBlock));
-	mRangeBlocks = reinterpret_cast<RangeBlock *>(data);
-	mHeightSamples = reinterpret_cast<uint8 *>(mRangeBlocks + mRangeBlocksSize);
-	mActiveEdges = mHeightSamples + mHeightSamplesSize;
-}
-
-HeightFieldShape::HeightFieldShape(const HeightFieldShapeSettings &inSettings, ShapeResult &outResult) :
-	Shape(EShapeType::HeightField, EShapeSubType::HeightField, inSettings, outResult),
-	mOffset(inSettings.mOffset),
-	mScale(inSettings.mScale),
-	mSampleCount(((inSettings.mSampleCount + inSettings.mBlockSize - 1) / inSettings.mBlockSize) * inSettings.mBlockSize), // Round sample count to nearest block size
-	mBlockSize(inSettings.mBlockSize),
-	mBitsPerSample(uint8(inSettings.mBitsPerSample))
-{
-	CacheValues();
-
-	// Reserve a bigger materials list if requested
-	if (inSettings.mMaterialsCapacity > 0)
-		mMaterials.reserve(inSettings.mMaterialsCapacity);
-	mMaterials = inSettings.mMaterials;
-
-	// Check block size
-	if (mBlockSize < 2 || mBlockSize > 8)
-	{
-		outResult.SetError("HeightFieldShape: Block size must be in the range [2, 8]!");
-		return;
-	}
-
-	// Check bits per sample
-	if (inSettings.mBitsPerSample < 1 || inSettings.mBitsPerSample > 8)
-	{
-		outResult.SetError("HeightFieldShape: Bits per sample must be in the range [1, 8]!");
-		return;
-	}
-
-	// We stop at mBlockSize x mBlockSize height sample blocks
-	uint num_blocks = GetNumBlocks();
-
-	// We want at least 1 grid layer
-	if (num_blocks < 2)
-	{
-		outResult.SetError("HeightFieldShape: Sample count too low!");
-		return;
-	}
-
-	// Check that we don't overflow our 32 bit 'properties'
-	if (num_blocks > (1 << cNumBitsXY))
-	{
-		outResult.SetError("HeightFieldShape: Sample count too high!");
-		return;
-	}
-
-	// Check if we're not exceeding the amount of sub shape id bits
-	if (GetSubShapeIDBitsRecursive() > SubShapeID::MaxBits)
-	{
-		outResult.SetError("HeightFieldShape: Size exceeds the amount of available sub shape ID bits!");
-		return;
-	}
-
-	if (!mMaterials.empty())
-	{
-		// Validate materials
-		if (mMaterials.size() > 256)
-		{
-			outResult.SetError("Supporting max 256 materials per height field");
-			return;
-		}
-		for (uint8 s : inSettings.mMaterialIndices)
-			if (s >= mMaterials.size())
-			{
-				outResult.SetError(StringFormat("Material %u is beyond material list (size: %u)", s, (uint)mMaterials.size()));
-				return;
-			}
-	}
-	else
-	{
-		// No materials assigned, validate that no materials have been specified
-		if (!inSettings.mMaterialIndices.empty())
-		{
-			outResult.SetError("No materials present, mMaterialIndices should be empty");
-			return;
-		}
-	}
-
-	// Determine range
-	float min_value, max_value, scale;
-	inSettings.DetermineMinAndMaxSample(min_value, max_value, scale);
-	if (min_value > max_value)
-	{
-		// If there is no collision with this heightmap, leave everything empty
-		mMaterials.clear();
-		outResult.Set(this);
-		return;
-	}
-
-	// Allocate space for this shape
-	AllocateBuffers();
-
-	// Quantize to uint16
-	Array<uint16> quantized_samples;
-	quantized_samples.reserve(mSampleCount * mSampleCount);
-	for (uint y = 0; y < inSettings.mSampleCount; ++y)
-	{
-		for (uint x = 0; x < inSettings.mSampleCount; ++x)
-		{
-			float h = inSettings.mHeightSamples[x + y * inSettings.mSampleCount];
-			if (h == cNoCollisionValue)
-			{
-				quantized_samples.push_back(cNoCollisionValue16);
-			}
-			else
-			{
-				// Floor the quantized height to get a lower bound for the quantized value
-				int quantized_height = (int)floor(scale * (h - min_value));
-
-				// Ensure that the height says below the max height value so we can safely add 1 to get the upper bound for the quantized value
-				quantized_height = Clamp(quantized_height, 0, int(cMaxHeightValue16 - 1));
-
-				quantized_samples.push_back(uint16(quantized_height));
-			}
-		}
-		// Pad remaining columns with no collision
-		for (uint x = inSettings.mSampleCount; x < mSampleCount; ++x)
-			quantized_samples.push_back(cNoCollisionValue16);
-	}
-	// Pad remaining rows with no collision
-	for (uint y = inSettings.mSampleCount; y < mSampleCount; ++y)
-		for (uint x = 0; x < mSampleCount; ++x)
-			quantized_samples.push_back(cNoCollisionValue16);
-
-	// Update offset and scale to account for the compression to uint16
-	if (min_value <= max_value) // Only when there was collision
-	{
-		// In GetPosition we always add 0.5 to the quantized sample in order to reduce the average error.
-		// We want to be able to exactly quantize min_value (this is important in case the heightfield is entirely flat) so we subtract that value from min_value.
-		min_value -= 0.5f / (scale * mSampleMask);
-
-		mOffset.SetY(mOffset.GetY() + mScale.GetY() * min_value);
-	}
-	mScale.SetY(mScale.GetY() / scale);
-
-	// Calculate amount of grids
-	uint max_level = sGetMaxLevel(num_blocks);
-
-	// Temporary data structure used during creating of a hierarchy of grids
-	struct Range
-	{
-		uint16	mMin;
-		uint16	mMax;
-	};
-
-	// Reserve size for temporary range data + reserve 1 extra for a 1x1 grid that we won't store but use for calculating the bounding box
-	Array<Array<Range>> ranges;
-	ranges.resize(max_level + 1);
-
-	// Calculate highest detail grid by combining mBlockSize x mBlockSize height samples
-	Array<Range> *cur_range_vector = &ranges.back();
-	uint num_blocks_pow2 = GetNextPowerOf2(num_blocks); // We calculate the range blocks as if the heightfield was a power of 2, when we save the range blocks we'll ignore the extra samples (this makes downsampling easier)
-	cur_range_vector->resize(num_blocks_pow2 * num_blocks_pow2);
-	Range *range_dst = &cur_range_vector->front();
-	for (uint y = 0; y < num_blocks_pow2; ++y)
-		for (uint x = 0; x < num_blocks_pow2; ++x)
-		{
-			range_dst->mMin = 0xffff;
-			range_dst->mMax = 0;
-			uint max_bx = x == num_blocks_pow2 - 1? mBlockSize : mBlockSize + 1; // for interior blocks take 1 more because the triangles connect to the next block so we must include their height too
-			uint max_by = y == num_blocks_pow2 - 1? mBlockSize : mBlockSize + 1;
-			for (uint by = 0; by < max_by; ++by)
-				for (uint bx = 0; bx < max_bx; ++bx)
-				{
-					uint sx = x * mBlockSize + bx;
-					uint sy = y * mBlockSize + by;
-					if (sx < mSampleCount && sy < mSampleCount)
-					{
-						uint16 h = quantized_samples[sy * mSampleCount + sx];
-						if (h != cNoCollisionValue16)
-						{
-							range_dst->mMin = min(range_dst->mMin, h);
-							range_dst->mMax = max(range_dst->mMax, uint16(h + 1)); // Add 1 to the max so we know the real value is between mMin and mMax
-						}
-					}
-				}
-			++range_dst;
-		}
-
-	// Calculate remaining grids
-	for (uint n = num_blocks_pow2 >> 1; n >= 1; n >>= 1)
-	{
-		// Get source buffer
-		const Range *range_src = &cur_range_vector->front();
-
-		// Previous array element
-		--cur_range_vector;
-
-		// Make space for this grid
-		cur_range_vector->resize(n * n);
-
-		// Get target buffer
-		range_dst = &cur_range_vector->front();
-
-		// Combine the results of 2x2 ranges
-		for (uint y = 0; y < n; ++y)
-			for (uint x = 0; x < n; ++x)
-			{
-				range_dst->mMin = 0xffff;
-				range_dst->mMax = 0;
-				for (uint by = 0; by < 2; ++by)
-					for (uint bx = 0; bx < 2; ++bx)
-					{
-						const Range &r = range_src[(y * 2 + by) * n * 2 + x * 2 + bx];
-						range_dst->mMin = min(range_dst->mMin, r.mMin);
-						range_dst->mMax = max(range_dst->mMax, r.mMax);
-					}
-				++range_dst;
-			}
-	}
-	JPH_ASSERT(cur_range_vector == &ranges.front());
-
-	// Store global range for bounding box calculation
-	mMinSample = ranges[0][0].mMin;
-	mMaxSample = ranges[0][0].mMax;
-
-#ifdef JPH_ENABLE_ASSERTS
-	// Validate that we did not lose range along the way
-	uint16 minv = 0xffff, maxv = 0;
-	for (uint16 v : quantized_samples)
-		if (v != cNoCollisionValue16)
-		{
-			minv = min(minv, v);
-			maxv = max(maxv, uint16(v + 1));
-		}
-	JPH_ASSERT(mMinSample == minv && mMaxSample == maxv);
-#endif
-
-	// Now erase the first element, we need a 2x2 grid to start with
-	ranges.erase(ranges.begin());
-
-	// Create blocks
-	uint max_stride = (num_blocks + 1) >> 1;
-	RangeBlock *current_block = mRangeBlocks;
-	for (uint level = 0; level < ranges.size(); ++level)
-	{
-		JPH_ASSERT(uint(current_block - mRangeBlocks) == sGridOffsets[level]);
-
-		uint in_n = 1 << level;
-		uint out_n = min(in_n, max_stride); // At the most detailed level we store a non-power of 2 number of blocks
-
-		for (uint y = 0; y < out_n; ++y)
-			for (uint x = 0; x < out_n; ++x)
-			{
-				// Convert from 2x2 Range structure to 1 RangeBlock structure
-				RangeBlock &rb = *current_block++;
-				for (uint by = 0; by < 2; ++by)
-					for (uint bx = 0; bx < 2; ++bx)
-					{
-						uint src_pos = (y * 2 + by) * 2 * in_n + (x * 2 + bx);
-						uint dst_pos = by * 2 + bx;
-						rb.mMin[dst_pos] = ranges[level][src_pos].mMin;
-						rb.mMax[dst_pos] = ranges[level][src_pos].mMax;
-					}
-			}
-	}
-	JPH_ASSERT(uint32(current_block - mRangeBlocks) == mRangeBlocksSize);
-
-	// Quantize height samples
-	memset(mHeightSamples, 0, mHeightSamplesSize);
-	int sample = 0;
-	for (uint y = 0; y < mSampleCount; ++y)
-		for (uint x = 0; x < mSampleCount; ++x)
-		{
-			uint32 output_value;
-
-			float h = x < inSettings.mSampleCount && y < inSettings.mSampleCount? inSettings.mHeightSamples[x + y * inSettings.mSampleCount] : cNoCollisionValue;
-			if (h == cNoCollisionValue)
-			{
-				// No collision
-				output_value = mSampleMask;
-			}
-			else
-			{
-				// Get range of block so we know what range to compress to
-				uint bx = x / mBlockSize;
-				uint by = y / mBlockSize;
-				const Range &range = ranges.back()[by * num_blocks_pow2 + bx];
-				JPH_ASSERT(range.mMin < range.mMax);
-
-				// Quantize to mBitsPerSample bits, note that mSampleMask is reserved for indicating that there's no collision.
-				// We divide the range into mSampleMask segments and use the mid points of these segments as the quantized values.
-				// This results in a lower error than if we had quantized our data using the lowest point of all these segments.
-				float h_min = min_value + range.mMin / scale;
-				float h_delta = float(range.mMax - range.mMin) / scale;
-				float quantized_height = floor((h - h_min) * float(mSampleMask) / h_delta);
-				output_value = uint32(Clamp((int)quantized_height, 0, int(mSampleMask) - 1)); // mSampleMask is reserved as 'no collision value'
-			}
-
-			// Store the sample
-			uint byte_pos = sample >> 3;
-			uint bit_pos = sample & 0b111;
-			output_value <<= bit_pos;
-			JPH_ASSERT(byte_pos + 1 < mHeightSamplesSize);
-			mHeightSamples[byte_pos] |= uint8(output_value);
-			mHeightSamples[byte_pos + 1] |= uint8(output_value >> 8);
-			sample += inSettings.mBitsPerSample;
-		}
-
-	// Calculate the active edges
-	CalculateActiveEdges(inSettings);
-
-	// Compress material indices
-	if (mMaterials.size() > 1 || inSettings.mMaterialsCapacity > 1)
-		StoreMaterialIndices(inSettings);
-
-	outResult.Set(this);
-}
-
-HeightFieldShape::~HeightFieldShape()
-{
-	if (mRangeBlocks != nullptr)
-		AlignedFree(mRangeBlocks);
-}
-
-Ref<HeightFieldShape> HeightFieldShape::Clone() const
-{
-	Ref<HeightFieldShape> clone = new HeightFieldShape;
-	clone->SetUserData(GetUserData());
-
-	clone->mOffset = mOffset;
-	clone->mScale = mScale;
-	clone->mSampleCount = mSampleCount;
-	clone->mBlockSize = mBlockSize;
-	clone->mBitsPerSample = mBitsPerSample;
-	clone->mSampleMask = mSampleMask;
-	clone->mMinSample = mMinSample;
-	clone->mMaxSample = mMaxSample;
-
-	clone->AllocateBuffers();
-	memcpy(clone->mRangeBlocks, mRangeBlocks, mRangeBlocksSize * sizeof(RangeBlock) + mHeightSamplesSize + mActiveEdgesSize); // Copy the entire buffer in 1 go
-
-	clone->mMaterials.reserve(mMaterials.capacity()); // Ensure we keep the capacity of the original
-	clone->mMaterials = mMaterials;
-	clone->mMaterialIndices = mMaterialIndices;
-	clone->mNumBitsPerMaterialIndex = mNumBitsPerMaterialIndex;
-
-#ifdef JPH_DEBUG_RENDERER
-	clone->mGeometry = mGeometry;
-	clone->mCachedUseMaterialColors = mCachedUseMaterialColors;
-#endif // JPH_DEBUG_RENDERER
-
-	return clone;
-}
-
-inline void HeightFieldShape::sGetRangeBlockOffsetAndStride(uint inNumBlocks, uint inMaxLevel, uint &outRangeBlockOffset, uint &outRangeBlockStride)
-{
-	outRangeBlockOffset = sGridOffsets[inMaxLevel - 1];
-	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);
-
-	uint offset = inRangeBlockOffset + rby * inRangeBlockStride + rbx;
-	JPH_ASSERT(offset < mRangeBlocksSize);
-	outBlock = mRangeBlocks + offset;
-}
-
-inline void HeightFieldShape::GetBlockOffsetAndScale(uint inBlockX, uint inBlockY, uint inRangeBlockOffset, uint inRangeBlockStride, float &outBlockOffset, float &outBlockScale) const
-{
-	JPH_ASSERT(inBlockX < GetNumBlocks() && inBlockY < GetNumBlocks());
-
-	// Convert to location of range block
-	uint rbx = inBlockX >> 1;
-	uint rby = inBlockY >> 1;
-	uint n = ((inBlockY & 1) << 1) + (inBlockX & 1);
-
-	// Calculate offset and scale
-	uint offset = inRangeBlockOffset + rby * inRangeBlockStride + rbx;
-	JPH_ASSERT(offset < mRangeBlocksSize);
-	const RangeBlock &block = mRangeBlocks[offset];
-	outBlockOffset = float(block.mMin[n]);
-	outBlockScale = float(block.mMax[n] - block.mMin[n]) / float(mSampleMask);
-}
-
-inline uint8 HeightFieldShape::GetHeightSample(uint inX, uint inY) const
-{
-	JPH_ASSERT(inX < mSampleCount);
-	JPH_ASSERT(inY < mSampleCount);
-
-	// Determine bit position of sample
-	uint sample = (inY * mSampleCount + inX) * uint(mBitsPerSample);
-	uint byte_pos = sample >> 3;
-	uint bit_pos = sample & 0b111;
-
-	// Fetch the height sample value
-	JPH_ASSERT(byte_pos + 1 < mHeightSamplesSize);
-	const uint8 *height_samples = mHeightSamples + byte_pos;
-	uint16 height_sample = uint16(height_samples[0]) | uint16(uint16(height_samples[1]) << 8);
-	return uint8(height_sample >> bit_pos) & mSampleMask;
-}
-
-inline Vec3 HeightFieldShape::GetPosition(uint inX, uint inY, float inBlockOffset, float inBlockScale, bool &outNoCollision) const
-{
-	// Get quantized value
-	uint8 height_sample = GetHeightSample(inX, inY);
-	outNoCollision = height_sample == mSampleMask;
-
-	// Add 0.5 to the quantized value to minimize the error (see constructor)
-	return mOffset + mScale * Vec3(float(inX), inBlockOffset + (0.5f + height_sample) * inBlockScale, float(inY));
-}
-
-Vec3 HeightFieldShape::GetPosition(uint inX, uint inY) const
-{
-	// Test if there are any samples
-	if (mHeightSamplesSize == 0)
-		return mOffset + mScale * Vec3(float(inX), 0.0f, float(inY));
-
-	// Get block location
-	uint bx = inX / mBlockSize;
-	uint by = inY / mBlockSize;
-
-	// 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);
-
-	float offset, scale;
-	GetBlockOffsetAndScale(bx, by, range_block_offset, range_block_stride, offset, scale);
-
-	bool no_collision;
-	return GetPosition(inX, inY, offset, scale, no_collision);
-}
-
-bool HeightFieldShape::IsNoCollision(uint inX, uint inY) const
-{
-	return mHeightSamplesSize == 0 || GetHeightSample(inX, inY) == mSampleMask;
-}
-
-bool HeightFieldShape::ProjectOntoSurface(Vec3Arg inLocalPosition, Vec3 &outSurfacePosition, SubShapeID &outSubShapeID) const
-{
-	// Check if we have collision
-	if (mHeightSamplesSize == 0)
-		return false;
-
-	// Convert coordinate to integer space
-	Vec3 integer_space = (inLocalPosition - mOffset) / mScale;
-
-	// Get x coordinate and fraction
-	float x_frac = integer_space.GetX();
-	if (x_frac < 0.0f || x_frac >= mSampleCount - 1)
-		return false;
-	uint x = (uint)floor(x_frac);
-	x_frac -= x;
-
-	// Get y coordinate and fraction
-	float y_frac = integer_space.GetZ();
-	if (y_frac < 0.0f || y_frac >= mSampleCount - 1)
-		return false;
-	uint y = (uint)floor(y_frac);
-	y_frac -= y;
-
-	// If one of the diagonal points doesn't have collision, we don't have a height at this location
-	if (IsNoCollision(x, y) || IsNoCollision(x + 1, y + 1))
-		return false;
-
-	if (y_frac >= x_frac)
-	{
-		// Left bottom triangle, test the 3rd point
-		if (IsNoCollision(x, y + 1))
-			return false;
-
-		// Interpolate height value
-		Vec3 v1 = GetPosition(x, y);
-		Vec3 v2 = GetPosition(x, y + 1);
-		Vec3 v3 = GetPosition(x + 1, y + 1);
-		outSurfacePosition = v1 + y_frac * (v2 - v1) + x_frac * (v3 - v2);
-		SubShapeIDCreator creator;
-		outSubShapeID = EncodeSubShapeID(creator, x, y, 0);
-		return true;
-	}
-	else
-	{
-		// Right top triangle, test the third point
-		if (IsNoCollision(x + 1, y))
-			return false;
-
-		// Interpolate height value
-		Vec3 v1 = GetPosition(x, y);
-		Vec3 v2 = GetPosition(x + 1, y + 1);
-		Vec3 v3 = GetPosition(x + 1, y);
-		outSurfacePosition = v1 + y_frac * (v2 - v3) + x_frac * (v3 - v1);
-		SubShapeIDCreator creator;
-		outSubShapeID = EncodeSubShapeID(creator, x, y, 1);
-		return true;
-	}
-}
-
-void HeightFieldShape::GetHeights(uint inX, uint inY, uint inSizeX, uint inSizeY, float *outHeights, intptr_t 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 (mHeightSamplesSize == 0)
-	{
-		// 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, intptr_t inHeightsStride, TempAllocator &inAllocator, float inActiveEdgeCosThresholdAngle)
-{
-	if (inSizeX == 0 || inSizeY == 0)
-		return;
-
-	JPH_ASSERT(mHeightSamplesSize > 0);
-	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;
-	intptr_t heights_stride;
-	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;
-		heights_stride = heights_size_x;
-
-		// 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_stride = 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_stride + 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_stride + 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 < mHeightSamplesSize);
-					uint8 *height_samples = mHeightSamples + 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_stride, 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);
-
-		// We'll be processing 2x2 blocks below so we need the start coordinates to be even and we extend the number of blocks to correct for that
-		if (block_start_x & 1) { --block_start_x; ++num_blocks_x; }
-		if (block_start_y & 1) { --block_start_y; ++num_blocks_y; }
-
-		// Loop over all affected blocks
-		uint block_end_x = block_start_x + num_blocks_x;
-		uint block_end_y = block_start_y + num_blocks_y;
-		for (uint block_y = block_start_y; block_y < block_end_y; block_y += 2)
-			for (uint block_x = block_start_x; block_x < block_end_x; block_x += 2)
-			{
-				// Get source range block
-				RangeBlock *src_range_block;
-				uint index_in_src_block;
-				GetRangeBlock(block_x, 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_x >> 1, 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
-}
-
-void HeightFieldShape::GetMaterials(uint inX, uint inY, uint inSizeX, uint inSizeY, uint8 *outMaterials, intptr_t inMaterialsStride) const
-{
-	if (inSizeX == 0 || inSizeY == 0)
-		return;
-
-	if (mMaterialIndices.empty())
-	{
-		// Return all 0's
-		for (uint y = 0; y < inSizeY; ++y)
-		{
-			uint8 *out_indices = outMaterials + y * inMaterialsStride;
-			for (uint x = 0; x < inSizeX; ++x)
-				*out_indices++ = 0;
-		}
-		return;
-	}
-
-	JPH_ASSERT(inX < mSampleCount && inY < mSampleCount);
-	JPH_ASSERT(inX + inSizeX < mSampleCount && inY + inSizeY < mSampleCount);
-
-	uint count_min_1 = mSampleCount - 1;
-	uint16 material_index_mask = uint16((1 << mNumBitsPerMaterialIndex) - 1);
-
-	for (uint y = 0; y < inSizeY; ++y)
-	{
-		// Calculate input position
-		uint bit_pos = (inX + (inY + y) * count_min_1) * mNumBitsPerMaterialIndex;
-		const uint8 *in_indices = mMaterialIndices.data() + (bit_pos >> 3);
-		bit_pos &= 0b111;
-
-		// Calculate output position
-		uint8 *out_indices = outMaterials + y * inMaterialsStride;
-
-		for (uint x = 0; x < inSizeX; ++x)
-		{
-			// Get material index
-			uint16 material_index = uint16(in_indices[0]) + uint16(uint16(in_indices[1]) << 8);
-			material_index >>= bit_pos;
-			material_index &= material_index_mask;
-			*out_indices = uint8(material_index);
-
-			// Go to the next index
-			bit_pos += mNumBitsPerMaterialIndex;
-			in_indices += bit_pos >> 3;
-			bit_pos &= 0b111;
-			++out_indices;
-		}
-	}
-}
-
-bool HeightFieldShape::SetMaterials(uint inX, uint inY, uint inSizeX, uint inSizeY, const uint8 *inMaterials, intptr_t inMaterialsStride, const PhysicsMaterialList *inMaterialList, TempAllocator &inAllocator)
-{
-	if (inSizeX == 0 || inSizeY == 0)
-		return true;
-
-	JPH_ASSERT(inX < mSampleCount && inY < mSampleCount);
-	JPH_ASSERT(inX + inSizeX < mSampleCount && inY + inSizeY < mSampleCount);
-
-	// Remap materials
-	uint material_remap_table_size = uint(inMaterialList != nullptr? inMaterialList->size() : mMaterials.size());
-	uint8 *material_remap_table = (uint8 *)inAllocator.Allocate(material_remap_table_size);
-	JPH_SCOPE_EXIT([&inAllocator, material_remap_table, material_remap_table_size]{ inAllocator.Free(material_remap_table, material_remap_table_size); });
-	if (inMaterialList != nullptr)
-	{
-		// Conservatively reserve more space if the incoming material list is bigger
-		if (inMaterialList->size() > mMaterials.size())
-			mMaterials.reserve(inMaterialList->size());
-
-		// Create a remap table
-		uint8 *remap_entry = material_remap_table;
-		for (const PhysicsMaterial *material : *inMaterialList)
-		{
-			// Try to find it in the existing list
-			PhysicsMaterialList::const_iterator it = std::find(mMaterials.begin(), mMaterials.end(), material);
-			if (it != mMaterials.end())
-			{
-				// Found it, calculate index
-				*remap_entry = uint8(it - mMaterials.begin());
-			}
-			else
-			{
-				// Not found, add it
-				if (mMaterials.size() >= 256)
-				{
-					// We can't have more than 256 materials since we use uint8 as indices
-					return false;
-				}
-				*remap_entry = uint8(mMaterials.size());
-				mMaterials.push_back(material);
-			}
-			++remap_entry;
-		}
-	}
-	else
-	{
-		// No remapping
-		for (uint i = 0; i < material_remap_table_size; ++i)
-			material_remap_table[i] = uint8(i);
-	}
-
-	if (mMaterials.size() == 1)
-	{
-		// Only 1 material, we don't need to store the material indices
-		return true;
-	}
-
-	// Check if we need to resize the material indices array
-	uint count_min_1 = mSampleCount - 1;
-	uint32 new_bits_per_material_index = 32 - CountLeadingZeros((uint32)mMaterials.size() - 1);
-	JPH_ASSERT(mNumBitsPerMaterialIndex <= 8 && new_bits_per_material_index <= 8);
-	if (new_bits_per_material_index > mNumBitsPerMaterialIndex)
-	{
-		// Resize the material indices array
-		mMaterialIndices.resize(((Square(count_min_1) * new_bits_per_material_index + 7) >> 3) + 1, 0); // Add 1 byte so we don't read out of bounds when reading an uint16
-
-		// Calculate old and new mask
-		uint16 old_material_index_mask = uint16((1 << mNumBitsPerMaterialIndex) - 1);
-		uint16 new_material_index_mask = uint16((1 << new_bits_per_material_index) - 1);
-
-		// Loop through the array backwards to avoid overwriting data
-		int in_bit_pos = (count_min_1 * count_min_1 - 1) * mNumBitsPerMaterialIndex;
-		const uint8 *in_indices = mMaterialIndices.data() + (in_bit_pos >> 3);
-		in_bit_pos &= 0b111;
-		int out_bit_pos = (count_min_1 * count_min_1 - 1) * new_bits_per_material_index;
-		uint8 *out_indices = mMaterialIndices.data() + (out_bit_pos >> 3);
-		out_bit_pos &= 0b111;
-
-		while (out_indices >= mMaterialIndices.data())
-		{
-			// Read the material index
-			uint16 material_index = uint16(in_indices[0]) + uint16(uint16(in_indices[1]) << 8);
-			material_index >>= in_bit_pos;
-			material_index &= old_material_index_mask;
-
-			// Write the material index
-			uint16 output_data = uint16(out_indices[0]) + uint16(uint16(out_indices[1]) << 8);
-			output_data &= ~(new_material_index_mask << out_bit_pos);
-			output_data |= material_index << out_bit_pos;
-			out_indices[0] = uint8(output_data);
-			out_indices[1] = uint8(output_data >> 8);
-
-			// Go to the previous index
-			in_bit_pos -= int(mNumBitsPerMaterialIndex);
-			in_indices += in_bit_pos >> 3;
-			in_bit_pos &= 0b111;
-			out_bit_pos -= int(new_bits_per_material_index);
-			out_indices += out_bit_pos >> 3;
-			out_bit_pos &= 0b111;
-		}
-
-		// Accept the new bits per material index
-		mNumBitsPerMaterialIndex = new_bits_per_material_index;
-	}
-
-	uint16 material_index_mask = uint16((1 << mNumBitsPerMaterialIndex) - 1);
-	for (uint y = 0; y < inSizeY; ++y)
-	{
-		// Calculate input position
-		const uint8 *in_indices = inMaterials + y * inMaterialsStride;
-
-		// Calculate output position
-		uint bit_pos = (inX + (inY + y) * count_min_1) * mNumBitsPerMaterialIndex;
-		uint8 *out_indices = mMaterialIndices.data() + (bit_pos >> 3);
-		bit_pos &= 0b111;
-
-		for (uint x = 0; x < inSizeX; ++x)
-		{
-			// Update material
-			uint16 output_data = uint16(out_indices[0]) + uint16(uint16(out_indices[1]) << 8);
-			output_data &= ~(material_index_mask << bit_pos);
-			output_data |= material_remap_table[*in_indices] << bit_pos;
-			out_indices[0] = uint8(output_data);
-			out_indices[1] = uint8(output_data >> 8);
-
-			// Go to the next index
-			in_indices++;
-			bit_pos += mNumBitsPerMaterialIndex;
-			out_indices += bit_pos >> 3;
-			bit_pos &= 0b111;
-		}
-	}
-
-	return true;
-}
-
-MassProperties HeightFieldShape::GetMassProperties() const
-{
-	// Object should always be static, return default mass properties
-	return MassProperties();
-}
-
-const PhysicsMaterial *HeightFieldShape::GetMaterial(uint inX, uint inY) const
-{
-	if (mMaterials.empty())
-		return PhysicsMaterial::sDefault;
-	if (mMaterials.size() == 1)
-		return mMaterials[0];
-
-	uint count_min_1 = mSampleCount - 1;
-	JPH_ASSERT(inX < count_min_1);
-	JPH_ASSERT(inY < count_min_1);
-
-	// Calculate at which bit the material index starts
-	uint bit_pos = (inX + inY * count_min_1) * mNumBitsPerMaterialIndex;
-	uint byte_pos = bit_pos >> 3;
-	bit_pos &= 0b111;
-
-	// Read the material index
-	JPH_ASSERT(byte_pos + 1 < mMaterialIndices.size());
-	const uint8 *material_indices = mMaterialIndices.data() + byte_pos;
-	uint16 material_index = uint16(material_indices[0]) + uint16(uint16(material_indices[1]) << 8);
-	material_index >>= bit_pos;
-	material_index &= (1 << mNumBitsPerMaterialIndex) - 1;
-
-	// Return the material
-	return mMaterials[material_index];
-}
-
-uint HeightFieldShape::GetSubShapeIDBits() const
-{
-	// Need to store X, Y and 1 extra bit to specify the triangle number in the quad
-	return 2 * (32 - CountLeadingZeros(mSampleCount - 1)) + 1;
-}
-
-SubShapeID HeightFieldShape::EncodeSubShapeID(const SubShapeIDCreator &inCreator, uint inX, uint inY, uint inTriangle) const
-{
-	return inCreator.PushID((inX + inY * mSampleCount) * 2 + inTriangle, GetSubShapeIDBits()).GetID();
-}
-
-void HeightFieldShape::DecodeSubShapeID(const SubShapeID &inSubShapeID, uint &outX, uint &outY, uint &outTriangle) const
-{
-	// Decode sub shape id
-	SubShapeID remainder;
-	uint32 id = inSubShapeID.PopID(GetSubShapeIDBits(), remainder);
-	JPH_ASSERT(remainder.IsEmpty(), "Invalid subshape ID");
-
-	// Get triangle index
-	outTriangle = id & 1;
-	id >>= 1;
-
-	// Fetch the x and y coordinate
-	outX = id % mSampleCount;
-	outY = id / mSampleCount;
-}
-
-void HeightFieldShape::GetSubShapeCoordinates(const SubShapeID &inSubShapeID, uint &outX, uint &outY, uint &outTriangleIndex) const
-{
-	DecodeSubShapeID(inSubShapeID, outX, outY, outTriangleIndex);
-}
-
-const PhysicsMaterial *HeightFieldShape::GetMaterial(const SubShapeID &inSubShapeID) const
-{
-	// Decode ID
-	uint x, y, triangle;
-	DecodeSubShapeID(inSubShapeID, x, y, triangle);
-
-	// Fetch the material
-	return GetMaterial(x, y);
-}
-
-Vec3 HeightFieldShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
-{
-	// Decode ID
-	uint x, y, triangle;
-	DecodeSubShapeID(inSubShapeID, x, y, triangle);
-
-	// Fetch vertices that both triangles share
-	Vec3 x1y1 = GetPosition(x, y);
-	Vec3 x2y2 = GetPosition(x + 1, y + 1);
-
-	// Get normal depending on which triangle was selected
-	Vec3 normal;
-	if (triangle == 0)
-	{
-		Vec3 x1y2 = GetPosition(x, y + 1);
-		normal = (x2y2 - x1y2).Cross(x1y1 - x1y2);
-	}
-	else
-	{
-		Vec3 x2y1 = GetPosition(x + 1, y);
-		normal = (x1y1 - x2y1).Cross(x2y2 - x2y1);
-	}
-
-	return normal.Normalized();
-}
-
-void HeightFieldShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
-{
-	// Decode ID
-	uint x, y, triangle;
-	DecodeSubShapeID(inSubShapeID, x, y, triangle);
-
-	// Fetch the triangle
-	outVertices.resize(3);
-	outVertices[0] = GetPosition(x, y);
-	Vec3 v2 = GetPosition(x + 1, y + 1);
-	if (triangle == 0)
-	{
-		outVertices[1] = GetPosition(x, y + 1);
-		outVertices[2] = v2;
-	}
-	else
-	{
-		outVertices[1] = v2;
-		outVertices[2] = GetPosition(x + 1, y);
-	}
-
-	// Flip triangle if scaled inside out
-	if (ScaleHelpers::IsInsideOut(inScale))
-		std::swap(outVertices[1], outVertices[2]);
-
-	// Transform to world space
-	Mat44 transform = inCenterOfMassTransform.PreScaled(inScale);
-	for (Vec3 &v : outVertices)
-		v = transform * v;
-}
-
-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
-		uint bit_pos = 3 * (inX + inY * (mSampleCount - 1));
-		uint byte_pos = bit_pos >> 3;
-		bit_pos &= 0b111;
-		JPH_ASSERT(byte_pos + 1 < mActiveEdgesSize);
-		const uint8 *active_edges = mActiveEdges + byte_pos;
-		uint16 edge_flags = uint16(active_edges[0]) + uint16(uint16(active_edges[1]) << 8);
-		return uint8(edge_flags >> bit_pos) & 0b111;
-	}
-	else
-	{
-		// 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 - 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;
-	}
-}
-
-AABox HeightFieldShape::GetLocalBounds() const
-{
-	if (mMinSample == cNoCollisionValue16)
-	{
-		// This whole height field shape doesn't have any collision, return the center point
-		Vec3 center = mOffset + 0.5f * mScale * Vec3(float(mSampleCount - 1), 0.0f, float(mSampleCount - 1));
-		return AABox(center, center);
-	}
-	else
-	{
-		// Bounding box based on min and max sample height
-		Vec3 bmin = mOffset + mScale * Vec3(0.0f, float(mMinSample), 0.0f);
-		Vec3 bmax = mOffset + mScale * Vec3(float(mSampleCount - 1), float(mMaxSample), float(mSampleCount - 1));
-		return AABox(bmin, bmax);
-	}
-}
-
-#ifdef JPH_DEBUG_RENDERER
-void HeightFieldShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
-{
-	// Don't draw anything if we don't have any collision
-	if (mHeightSamplesSize == 0)
-		return;
-
-	// Reset the batch if we switch coloring mode
-	if (mCachedUseMaterialColors != inUseMaterialColors)
-	{
-		mGeometry.clear();
-		mCachedUseMaterialColors = inUseMaterialColors;
-	}
-
-	if (mGeometry.empty())
-	{
-		// Divide terrain in triangle batches of max 64x64x2 triangles to allow better culling of the terrain
-		uint32 block_size = min<uint32>(mSampleCount, 64);
-		for (uint32 by = 0; by < mSampleCount; by += block_size)
-			for (uint32 bx = 0; bx < mSampleCount; bx += block_size)
-			{
-				// Create vertices for a block
-				Array<DebugRenderer::Triangle> triangles;
-				triangles.resize(block_size * block_size * 2);
-				DebugRenderer::Triangle *out_tri = &triangles[0];
-				for (uint32 y = by, max_y = min(by + block_size, mSampleCount - 1); y < max_y; ++y)
-					for (uint32 x = bx, max_x = min(bx + block_size, mSampleCount - 1); x < max_x; ++x)
-						if (!IsNoCollision(x, y) && !IsNoCollision(x + 1, y + 1))
-						{
-							Vec3 x1y1 = GetPosition(x, y);
-							Vec3 x2y2 = GetPosition(x + 1, y + 1);
-							Color color = inUseMaterialColors? GetMaterial(x, y)->GetDebugColor() : Color::sWhite;
-
-							if (!IsNoCollision(x, y + 1))
-							{
-								Vec3 x1y2 = GetPosition(x, y + 1);
-
-								x1y1.StoreFloat3(&out_tri->mV[0].mPosition);
-								x1y2.StoreFloat3(&out_tri->mV[1].mPosition);
-								x2y2.StoreFloat3(&out_tri->mV[2].mPosition);
-
-								Vec3 normal = (x2y2 - x1y2).Cross(x1y1 - x1y2).Normalized();
-								for (DebugRenderer::Vertex &v : out_tri->mV)
-								{
-									v.mColor = color;
-									v.mUV = Float2(0, 0);
-									normal.StoreFloat3(&v.mNormal);
-								}
-
-								++out_tri;
-							}
-
-							if (!IsNoCollision(x + 1, y))
-							{
-								Vec3 x2y1 = GetPosition(x + 1, y);
-
-								x1y1.StoreFloat3(&out_tri->mV[0].mPosition);
-								x2y2.StoreFloat3(&out_tri->mV[1].mPosition);
-								x2y1.StoreFloat3(&out_tri->mV[2].mPosition);
-
-								Vec3 normal = (x1y1 - x2y1).Cross(x2y2 - x2y1).Normalized();
-								for (DebugRenderer::Vertex &v : out_tri->mV)
-								{
-									v.mColor = color;
-									v.mUV = Float2(0, 0);
-									normal.StoreFloat3(&v.mNormal);
-								}
-
-								++out_tri;
-							}
-						}
-
-				// Resize triangles array to actual amount of triangles written
-				size_t num_triangles = out_tri - &triangles[0];
-				triangles.resize(num_triangles);
-
-				// Create batch
-				if (num_triangles > 0)
-					mGeometry.push_back(new DebugRenderer::Geometry(inRenderer->CreateTriangleBatch(triangles), DebugRenderer::sCalculateBounds(&triangles[0].mV[0], int(3 * num_triangles))));
-			}
-	}
-
-	// Get transform including scale
-	RMat44 transform = inCenterOfMassTransform.PreScaled(inScale);
-
-	// Test if the shape is scaled inside out
-	DebugRenderer::ECullMode cull_mode = ScaleHelpers::IsInsideOut(inScale)? DebugRenderer::ECullMode::CullFrontFace : DebugRenderer::ECullMode::CullBackFace;
-
-	// Determine the draw mode
-	DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid;
-
-	// Draw the geometry
-	for (const DebugRenderer::GeometryRef &b : mGeometry)
-		inRenderer->DrawGeometry(transform, inColor, b, cull_mode, DebugRenderer::ECastShadow::On, draw_mode);
-
-	if (sDrawTriangleOutlines)
-	{
-		struct Visitor
-		{
-			JPH_INLINE explicit		Visitor(const HeightFieldShape *inShape, DebugRenderer *inRenderer, RMat44Arg inTransform) :
-				mShape(inShape),
-				mRenderer(inRenderer),
-				mTransform(inTransform)
-			{
-			}
-
-			JPH_INLINE bool			ShouldAbort() const
-			{
-				return false;
-			}
-
-			JPH_INLINE bool			ShouldVisitRangeBlock([[maybe_unused]] int inStackTop) const
-			{
-				return true;
-			}
-
-			JPH_INLINE int			VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const
-			{
-				UVec4 valid = Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY);
-				return CountAndSortTrues(valid, ioProperties);
-			}
-
-			JPH_INLINE void			VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) const
-			{
-				// Determine active edges
-				uint8 active_edges = mShape->GetEdgeFlags(inX, inY, inTriangle);
-
-				// Loop through edges
-				Vec3 v[] = { inV0, inV1, inV2 };
-				for (uint edge_idx = 0; edge_idx < 3; ++edge_idx)
-				{
-					RVec3 v1 = mTransform * v[edge_idx];
-					RVec3 v2 = mTransform * v[(edge_idx + 1) % 3];
-
-					// Draw active edge as a green arrow, other edges as grey
-					if (active_edges & (1 << edge_idx))
-						mRenderer->DrawArrow(v1, v2, Color::sGreen, 0.01f);
-					else
-						mRenderer->DrawLine(v1, v2, Color::sGrey);
-				}
-			}
-
-			const HeightFieldShape *mShape;
-			DebugRenderer *			mRenderer;
-			RMat44					mTransform;
-		};
-
-		Visitor visitor(this, inRenderer, inCenterOfMassTransform.PreScaled(inScale));
-		WalkHeightField(visitor);
-	}
-}
-#endif // JPH_DEBUG_RENDERER
-
-class HeightFieldShape::DecodingContext
-{
-public:
-	JPH_INLINE explicit			DecodingContext(const HeightFieldShape *inShape) :
-		mShape(inShape)
-	{
-		static_assert(std::size(sGridOffsets) == cNumBitsXY + 1, "Offsets array is not long enough");
-
-		// Construct root stack entry
-		mPropertiesStack[0] = 0; // level: 0, x: 0, y: 0
-	}
-
-	template <class Visitor>
-	JPH_INLINE void				WalkHeightField(Visitor &ioVisitor)
-	{
-		// Early out if there's no collision
-		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);
-
-		// Precalculate values relating to block size
-		uint32 block_size = mShape->mBlockSize;
-		uint32 block_size_plus_1 = block_size + 1;
-		uint num_blocks = mShape->GetNumBlocks();
-		uint num_blocks_min_1 = num_blocks - 1;
-		uint max_level = HeightFieldShape::sGetMaxLevel(num_blocks);
-		uint32 max_stride = (num_blocks + 1) >> 1;
-
-		// Precalculate range block offset and stride for GetBlockOffsetAndScale
-		uint range_block_offset, range_block_stride;
-		sGetRangeBlockOffsetAndStride(num_blocks, max_level, range_block_offset, range_block_stride);
-
-		// Allocate space for vertices and 'no collision' flags
-		int array_size = Square(block_size_plus_1);
-		Vec3 *vertices = reinterpret_cast<Vec3 *>(JPH_STACK_ALLOC(array_size * sizeof(Vec3)));
-		bool *no_collision = reinterpret_cast<bool *>(JPH_STACK_ALLOC(array_size * sizeof(bool)));
-
-		// Splat offsets
-		Vec4 ox = mShape->mOffset.SplatX();
-		Vec4 oy = mShape->mOffset.SplatY();
-		Vec4 oz = mShape->mOffset.SplatZ();
-
-		// Splat scales
-		Vec4 sx = mShape->mScale.SplatX();
-		Vec4 sy = mShape->mScale.SplatY();
-		Vec4 sz = mShape->mScale.SplatZ();
-
-		do
-		{
-			// Decode properties
-			uint32 properties_top = mPropertiesStack[mTop];
-			uint32 x = properties_top & cMaskBitsXY;
-			uint32 y = (properties_top >> cNumBitsXY) & cMaskBitsXY;
-			uint32 level = properties_top >> cLevelShift;
-
-			if (level >= max_level)
-			{
-				// Determine actual range of samples (minus one because we eventually want to iterate over the triangles, not the samples)
-				uint32 min_x = x * block_size;
-				uint32 max_x = min_x + block_size;
-				uint32 min_y = y * block_size;
-				uint32 max_y = min_y + block_size;
-
-				// Decompress vertices of block at (x, y)
-				Vec3 *dst_vertex = vertices;
-				bool *dst_no_collision = no_collision;
-				float block_offset, block_scale;
-				mShape->GetBlockOffsetAndScale(x, y, range_block_offset, range_block_stride, block_offset, block_scale);
-				for (uint32 v_y = min_y; v_y < max_y; ++v_y)
-				{
-					for (uint32 v_x = min_x; v_x < max_x; ++v_x)
-					{
-						*dst_vertex = mShape->GetPosition(v_x, v_y, block_offset, block_scale, *dst_no_collision);
-						++dst_vertex;
-						++dst_no_collision;
-					}
-
-					// Skip last column, these values come from a different block
-					++dst_vertex;
-					++dst_no_collision;
-				}
-
-				// Decompress block (x + 1, y)
-				uint32 max_x_decrement = 0;
-				if (x < num_blocks_min_1)
-				{
-					dst_vertex = vertices + block_size;
-					dst_no_collision = no_collision + block_size;
-					mShape->GetBlockOffsetAndScale(x + 1, y, range_block_offset, range_block_stride, block_offset, block_scale);
-					for (uint32 v_y = min_y; v_y < max_y; ++v_y)
-					{
-						*dst_vertex = mShape->GetPosition(max_x, v_y, block_offset, block_scale, *dst_no_collision);
-						dst_vertex += block_size_plus_1;
-						dst_no_collision += block_size_plus_1;
-					}
-				}
-				else
-					max_x_decrement = 1; // We don't have a next block, one less triangle to test
-
-				// Decompress block (x, y + 1)
-				if (y < num_blocks_min_1)
-				{
-					uint start = block_size * block_size_plus_1;
-					dst_vertex = vertices + start;
-					dst_no_collision = no_collision + start;
-					mShape->GetBlockOffsetAndScale(x, y + 1, range_block_offset, range_block_stride, block_offset, block_scale);
-					for (uint32 v_x = min_x; v_x < max_x; ++v_x)
-					{
-						*dst_vertex = mShape->GetPosition(v_x, max_y, block_offset, block_scale, *dst_no_collision);
-						++dst_vertex;
-						++dst_no_collision;
-					}
-
-					// Decompress single sample of block at (x + 1, y + 1)
-					if (x < num_blocks_min_1)
-					{
-						mShape->GetBlockOffsetAndScale(x + 1, y + 1, range_block_offset, range_block_stride, block_offset, block_scale);
-						*dst_vertex = mShape->GetPosition(max_x, max_y, block_offset, block_scale, *dst_no_collision);
-					}
-				}
-				else
-					--max_y; // We don't have a next block, one less triangle to test
-
-				// Update max_x (we've been using it so we couldn't update it earlier)
-				max_x -= max_x_decrement;
-
-				// We're going to divide the vertices in 4 blocks to do one more runtime sub-division, calculate the ranges of those blocks
-				struct Range
-				{
-					uint32 mMinX, mMinY, mNumTrianglesX, mNumTrianglesY;
-				};
-				uint32 half_block_size = block_size >> 1;
-				uint32 block_size_x = max_x - min_x - half_block_size;
-				uint32 block_size_y = max_y - min_y - half_block_size;
-				Range ranges[] =
-				{
-					{ 0, 0,									half_block_size, half_block_size },
-					{ half_block_size, 0,					block_size_x, half_block_size },
-					{ 0, half_block_size,					half_block_size, block_size_y },
-					{ half_block_size, half_block_size,		block_size_x, block_size_y },
-				};
-
-				// Calculate the min and max of each of the blocks
-				Mat44 block_min, block_max;
-				for (int block = 0; block < 4; ++block)
-				{
-					// Get the range for this block
-					const Range &range = ranges[block];
-					uint32 start = range.mMinX + range.mMinY * block_size_plus_1;
-					uint32 size_x_plus_1 = range.mNumTrianglesX + 1;
-					uint32 size_y_plus_1 = range.mNumTrianglesY + 1;
-
-					// Calculate where to start reading
-					const Vec3 *src_vertex = vertices + start;
-					const bool *src_no_collision = no_collision + start;
-					uint32 stride = block_size_plus_1 - size_x_plus_1;
-
-					// Start range with a very large inside-out box
-					Vec3 value_min = Vec3::sReplicate(cLargeFloat);
-					Vec3 value_max = Vec3::sReplicate(-cLargeFloat);
-
-					// Loop over the samples to determine the min and max of this block
-					for (uint32 block_y = 0; block_y < size_y_plus_1; ++block_y)
-					{
-						for (uint32 block_x = 0; block_x < size_x_plus_1; ++block_x)
-						{
-							if (!*src_no_collision)
-							{
-								value_min = Vec3::sMin(value_min, *src_vertex);
-								value_max = Vec3::sMax(value_max, *src_vertex);
-							}
-							++src_vertex;
-							++src_no_collision;
-						}
-						src_vertex += stride;
-						src_no_collision += stride;
-					}
-					block_min.SetColumn4(block, Vec4(value_min));
-					block_max.SetColumn4(block, Vec4(value_max));
-				}
-
-			#ifdef JPH_DEBUG_HEIGHT_FIELD
-				// Draw the bounding boxes of the sub-nodes
-				for (int block = 0; block < 4; ++block)
-				{
-					AABox bounds(block_min.GetColumn3(block), block_max.GetColumn3(block));
-					if (bounds.IsValid())
-						DebugRenderer::sInstance->DrawWireBox(bounds, Color::sYellow);
-				}
-			#endif // JPH_DEBUG_HEIGHT_FIELD
-
-				// Transpose so we have the mins and maxes of each of the blocks in rows instead of columns
-				Mat44 transposed_min = block_min.Transposed();
-				Mat44 transposed_max = block_max.Transposed();
-
-				// Check which blocks collide
-				// Note: At this point we don't use our own stack but we do allow the visitor to use its own stack
-				// to store collision distances so that we can still early out when no closer hits have been found.
-				UVec4 colliding_blocks(0, 1, 2, 3);
-				int num_results = ioVisitor.VisitRangeBlock(transposed_min.GetColumn4(0), transposed_min.GetColumn4(1), transposed_min.GetColumn4(2), transposed_max.GetColumn4(0), transposed_max.GetColumn4(1), transposed_max.GetColumn4(2), colliding_blocks, mTop);
-
-				// Loop through the results backwards (closest first)
-				int result = num_results - 1;
-				while (result >= 0)
-				{
-					// Calculate the min and max of this block
-					uint32 block = colliding_blocks[result];
-					const Range &range = ranges[block];
-					uint32 block_min_x = min_x + range.mMinX;
-					uint32 block_max_x = block_min_x + range.mNumTrianglesX;
-					uint32 block_min_y = min_y + range.mMinY;
-					uint32 block_max_y = block_min_y + range.mNumTrianglesY;
-
-					// Loop triangles
-					for (uint32 v_y = block_min_y; v_y < block_max_y; ++v_y)
-						for (uint32 v_x = block_min_x; v_x < block_max_x; ++v_x)
-						{
-							// Get first vertex
-							const int offset = (v_y - min_y) * block_size_plus_1 + (v_x - min_x);
-							const Vec3 *start_vertex = vertices + offset;
-							const bool *start_no_collision = no_collision + offset;
-
-							// Check if vertices shared by both triangles have collision
-							if (!start_no_collision[0] && !start_no_collision[block_size_plus_1 + 1])
-							{
-								// Loop 2 triangles
-								for (uint t = 0; t < 2; ++t)
-								{
-									// Determine triangle vertices
-									Vec3 v0, v1, v2;
-									if (t == 0)
-									{
-										// Check third vertex
-										if (start_no_collision[block_size_plus_1])
-											continue;
-
-										// Get vertices for triangle
-										v0 = start_vertex[0];
-										v1 = start_vertex[block_size_plus_1];
-										v2 = start_vertex[block_size_plus_1 + 1];
-									}
-									else
-									{
-										// Check third vertex
-										if (start_no_collision[1])
-											continue;
-
-										// Get vertices for triangle
-										v0 = start_vertex[0];
-										v1 = start_vertex[block_size_plus_1 + 1];
-										v2 = start_vertex[1];
-									}
-
-								#ifdef JPH_DEBUG_HEIGHT_FIELD
-									DebugRenderer::sInstance->DrawWireTriangle(RVec3(v0), RVec3(v1), RVec3(v2), Color::sWhite);
-								#endif
-
-									// Call visitor
-									ioVisitor.VisitTriangle(v_x, v_y, t, v0, v1, v2);
-
-									// Check if we're done
-									if (ioVisitor.ShouldAbort())
-										return;
-								}
-							}
-						}
-
-					// Fetch next block until we find one that the visitor wants to see
-					do
-						--result;
-					while (result >= 0 && !ioVisitor.ShouldVisitRangeBlock(mTop + result));
-				}
-			}
-			else
-			{
-				// Visit child grid
-				uint32 stride = min(1U << level, max_stride); // At the most detailed level we store a non-power of 2 number of blocks
-				uint32 offset = sGridOffsets[level] + stride * y + x;
-
-				// Decode min/max height
-				JPH_ASSERT(offset < mShape->mRangeBlocksSize);
-				UVec4 block = UVec4::sLoadInt4Aligned(reinterpret_cast<const uint32 *>(&mShape->mRangeBlocks[offset]));
-				Vec4 bounds_miny = oy + sy * block.Expand4Uint16Lo().ToFloat();
-				Vec4 bounds_maxy = oy + sy * block.Expand4Uint16Hi().ToFloat();
-
-				// Calculate size of one cell at this grid level
-				UVec4 internal_cell_size = UVec4::sReplicate(block_size << (max_level - level - 1)); // subtract 1 from level because we have an internal grid of 2x2
-
-				// Calculate min/max x and z
-				UVec4 two_x = UVec4::sReplicate(2 * x); // multiply by two because we have an internal grid of 2x2
-				Vec4 bounds_minx = ox + sx * (internal_cell_size * (two_x + UVec4(0, 1, 0, 1))).ToFloat();
-				Vec4 bounds_maxx = ox + sx * UVec4::sMin(internal_cell_size * (two_x + UVec4(1, 2, 1, 2)), sample_count_min_1).ToFloat();
-
-				UVec4 two_y = UVec4::sReplicate(2 * y);
-				Vec4 bounds_minz = oz + sz * (internal_cell_size * (two_y + UVec4(0, 0, 1, 1))).ToFloat();
-				Vec4 bounds_maxz = oz + sz * UVec4::sMin(internal_cell_size * (two_y + UVec4(1, 1, 2, 2)), sample_count_min_1).ToFloat();
-
-				// Calculate properties of child blocks
-				UVec4 properties = UVec4::sReplicate(((level + 1) << cLevelShift) + (y << (cNumBitsXY + 1)) + (x << 1)) + UVec4(0, 1, 1 << cNumBitsXY, (1 << cNumBitsXY) + 1);
-
-			#ifdef JPH_DEBUG_HEIGHT_FIELD
-				// Draw boxes
-				for (int i = 0; i < 4; ++i)
-				{
-					AABox b(Vec3(bounds_minx[i], bounds_miny[i], bounds_minz[i]), Vec3(bounds_maxx[i], bounds_maxy[i], bounds_maxz[i]));
-					if (b.IsValid())
-						DebugRenderer::sInstance->DrawWireBox(b, Color::sGreen);
-				}
-			#endif
-
-				// Check which sub nodes to visit
-				int num_results = ioVisitor.VisitRangeBlock(bounds_minx, bounds_miny, bounds_minz, bounds_maxx, bounds_maxy, bounds_maxz, properties, mTop);
-
-				// Push them onto the stack
-				JPH_ASSERT(mTop + 4 < cStackSize);
-				properties.StoreInt4(&mPropertiesStack[mTop]);
-				mTop += num_results;
-			}
-
-			// Check if we're done
-			if (ioVisitor.ShouldAbort())
-				return;
-
-			// Fetch next node until we find one that the visitor wants to see
-			do
-				--mTop;
-			while (mTop >= 0 && !ioVisitor.ShouldVisitRangeBlock(mTop));
-		}
-		while (mTop >= 0);
-	}
-
-	// This can be used to have the visitor early out (ioVisitor.ShouldAbort() returns true) and later continue again (call WalkHeightField() again)
-	JPH_INLINE bool				IsDoneWalking() const
-	{
-		return mTop < 0;
-	}
-
-private:
-	const HeightFieldShape *	mShape;
-	int							mTop = 0;
-	uint32						mPropertiesStack[cStackSize];
-};
-
-template <class Visitor>
-void HeightFieldShape::WalkHeightField(Visitor &ioVisitor) const
-{
-	DecodingContext ctx(this);
-	ctx.WalkHeightField(ioVisitor);
-}
-
-bool HeightFieldShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
-{
-	JPH_PROFILE_FUNCTION();
-
-	struct Visitor
-	{
-		JPH_INLINE explicit		Visitor(const HeightFieldShape *inShape, const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) :
-			mHit(ioHit),
-			mRayOrigin(inRay.mOrigin),
-			mRayDirection(inRay.mDirection),
-			mRayInvDirection(inRay.mDirection),
-			mShape(inShape),
-			mSubShapeIDCreator(inSubShapeIDCreator)
-		{
-		}
-
-		JPH_INLINE bool			ShouldAbort() const
-		{
-			return mHit.mFraction <= 0.0f;
-		}
-
-		JPH_INLINE bool			ShouldVisitRangeBlock(int inStackTop) const
-		{
-			return mDistanceStack[inStackTop] < mHit.mFraction;
-		}
-
-		JPH_INLINE int			VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
-		{
-			// Test bounds of 4 children
-			Vec4 distance = RayAABox4(mRayOrigin, mRayInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
-
-			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
-			return SortReverseAndStore(distance, mHit.mFraction, ioProperties, &mDistanceStack[inStackTop]);
-		}
-
-		JPH_INLINE void			VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
-		{
-			float fraction = RayTriangle(mRayOrigin, mRayDirection, inV0, inV1, inV2);
-			if (fraction < mHit.mFraction)
-			{
-				// It's a closer hit
-				mHit.mFraction = fraction;
-				mHit.mSubShapeID2 = mShape->EncodeSubShapeID(mSubShapeIDCreator, inX, inY, inTriangle);
-				mReturnValue = true;
-			}
-		}
-
-		RayCastResult &			mHit;
-		Vec3					mRayOrigin;
-		Vec3					mRayDirection;
-		RayInvDirection			mRayInvDirection;
-		const HeightFieldShape *mShape;
-		SubShapeIDCreator		mSubShapeIDCreator;
-		bool					mReturnValue = false;
-		float					mDistanceStack[cStackSize];
-	};
-
-	Visitor visitor(this, inRay, inSubShapeIDCreator, ioHit);
-	WalkHeightField(visitor);
-
-	return visitor.mReturnValue;
-}
-
-void HeightFieldShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const
-{
-	JPH_PROFILE_FUNCTION();
-
-	// Test shape filter
-	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
-		return;
-
-	struct Visitor
-	{
-		JPH_INLINE explicit		Visitor(const HeightFieldShape *inShape, const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector) :
-			mCollector(ioCollector),
-			mRayOrigin(inRay.mOrigin),
-			mRayDirection(inRay.mDirection),
-			mRayInvDirection(inRay.mDirection),
-			mBackFaceMode(inRayCastSettings.mBackFaceModeTriangles),
-			mShape(inShape),
-			mSubShapeIDCreator(inSubShapeIDCreator)
-		{
-		}
-
-		JPH_INLINE bool			ShouldAbort() const
-		{
-			return mCollector.ShouldEarlyOut();
-		}
-
-		JPH_INLINE bool			ShouldVisitRangeBlock(int inStackTop) const
-		{
-			return mDistanceStack[inStackTop] < mCollector.GetEarlyOutFraction();
-		}
-
-		JPH_INLINE int			VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
-		{
-			// Test bounds of 4 children
-			Vec4 distance = RayAABox4(mRayOrigin, mRayInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
-
-			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
-			return SortReverseAndStore(distance, mCollector.GetEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
-		}
-
-		JPH_INLINE void			VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) const
-		{
-			// Back facing check
-			if (mBackFaceMode == EBackFaceMode::IgnoreBackFaces && (inV2 - inV0).Cross(inV1 - inV0).Dot(mRayDirection) < 0)
-				return;
-
-			// Check the triangle
-			float fraction = RayTriangle(mRayOrigin, mRayDirection, inV0, inV1, inV2);
-			if (fraction < mCollector.GetEarlyOutFraction())
-			{
-				RayCastResult hit;
-				hit.mBodyID = TransformedShape::sGetBodyID(mCollector.GetContext());
-				hit.mFraction = fraction;
-				hit.mSubShapeID2 = mShape->EncodeSubShapeID(mSubShapeIDCreator, inX, inY, inTriangle);
-				mCollector.AddHit(hit);
-			}
-		}
-
-		CastRayCollector &		mCollector;
-		Vec3					mRayOrigin;
-		Vec3					mRayDirection;
-		RayInvDirection			mRayInvDirection;
-		EBackFaceMode			mBackFaceMode;
-		const HeightFieldShape *mShape;
-		SubShapeIDCreator		mSubShapeIDCreator;
-		float					mDistanceStack[cStackSize];
-	};
-
-	Visitor visitor(this, inRay, inRayCastSettings, inSubShapeIDCreator, ioCollector);
-	WalkHeightField(visitor);
-}
-
-void HeightFieldShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
-{
-	// A height field doesn't have volume, so we can't test insideness
-}
-
-void HeightFieldShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
-{
-	JPH_PROFILE_FUNCTION();
-
-	struct Visitor : public CollideSoftBodyVerticesVsTriangles
-	{
-		using CollideSoftBodyVerticesVsTriangles::CollideSoftBodyVerticesVsTriangles;
-
-		JPH_INLINE bool	ShouldAbort() const
-		{
-			return false;
-		}
-
-		JPH_INLINE bool	ShouldVisitRangeBlock([[maybe_unused]] int inStackTop) const
-		{
-			return mDistanceStack[inStackTop] < mClosestDistanceSq;
-		}
-
-		JPH_INLINE int	VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
-		{
-			// Scale the bounding boxes of this node
-			Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
-			AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
-
-			// Get distance to vertex
-			Vec4 dist_sq = AABox4DistanceSqToPoint(mLocalPosition, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
-
-			// 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]);
-		}
-
-		JPH_INLINE void	VisitTriangle([[maybe_unused]] uint inX, [[maybe_unused]] uint inY, [[maybe_unused]] uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
-		{
-			ProcessTriangle(inV0, inV1, inV2);
-		}
-
-		float			mDistanceStack[cStackSize];
-	};
-
-	Visitor visitor(inCenterOfMassTransform, inScale);
-
-	for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v)
-		if (v.GetInvMass() > 0.0f)
-		{
-			visitor.StartVertex(v);
-			WalkHeightField(visitor);
-			visitor.FinishVertex(v, 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)
-{
-	JPH_PROFILE_FUNCTION();
-
-	struct Visitor : public CastConvexVsTriangles
-	{
-		using CastConvexVsTriangles::CastConvexVsTriangles;
-
-		JPH_INLINE bool				ShouldAbort() const
-		{
-			return mCollector.ShouldEarlyOut();
-		}
-
-		JPH_INLINE bool				ShouldVisitRangeBlock(int inStackTop) const
-		{
-			return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction();
-		}
-
-		JPH_INLINE int				VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
-		{
-			// Scale the bounding boxes of this node
-			Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
-			AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
-
-			// Enlarge them by the casted shape's box extents
-			AABox4EnlargeWithExtent(mBoxExtent, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
-
-			// Test bounds of 4 children
-			Vec4 distance = RayAABox4(mBoxCenter, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
-
-			// Clear distance for invalid bounds
-			distance = Vec4::sSelect(Vec4::sReplicate(FLT_MAX), distance, 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(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
-		}
-
-		JPH_INLINE void				VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
-		{
-			// Create sub shape id for this part
-			SubShapeID triangle_sub_shape_id = mShape2->EncodeSubShapeID(mSubShapeIDCreator2, inX, inY, inTriangle);
-
-			// Determine active edges
-			uint8 active_edges = mShape2->GetEdgeFlags(inX, inY, inTriangle);
-
-			Cast(inV0, inV1, inV2, active_edges, triangle_sub_shape_id);
-		}
-
-		const HeightFieldShape *	mShape2;
-		RayInvDirection				mInvDirection;
-		Vec3						mBoxCenter;
-		Vec3						mBoxExtent;
-		SubShapeIDCreator			mSubShapeIDCreator2;
-		float						mDistanceStack[cStackSize];
-	};
-
-	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::HeightField);
-	const HeightFieldShape *shape = static_cast<const HeightFieldShape *>(inShape);
-
-	Visitor visitor(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector);
-	visitor.mShape2 = shape;
-	visitor.mInvDirection.Set(inShapeCast.mDirection);
-	visitor.mBoxCenter = inShapeCast.mShapeWorldBounds.GetCenter();
-	visitor.mBoxExtent = inShapeCast.mShapeWorldBounds.GetExtent();
-	visitor.mSubShapeIDCreator2 = inSubShapeIDCreator2;
-	shape->WalkHeightField(visitor);
-}
-
-void HeightFieldShape::sCastSphereVsHeightField(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)
-{
-	JPH_PROFILE_FUNCTION();
-
-	struct Visitor : public CastSphereVsTriangles
-	{
-		using CastSphereVsTriangles::CastSphereVsTriangles;
-
-		JPH_INLINE bool				ShouldAbort() const
-		{
-			return mCollector.ShouldEarlyOut();
-		}
-
-		JPH_INLINE bool				ShouldVisitRangeBlock(int inStackTop) const
-		{
-			return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction();
-		}
-
-		JPH_INLINE int				VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
-		{
-			// Scale the bounding boxes of this node
-			Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
-			AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
-
-			// Enlarge them by the radius of the sphere
-			AABox4EnlargeWithExtent(Vec3::sReplicate(mRadius), bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
-
-			// Test bounds of 4 children
-			Vec4 distance = RayAABox4(mStart, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
-
-			// Clear distance for invalid bounds
-			distance = Vec4::sSelect(Vec4::sReplicate(FLT_MAX), distance, 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(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
-		}
-
-		JPH_INLINE void				VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
-		{
-			// Create sub shape id for this part
-			SubShapeID triangle_sub_shape_id = mShape2->EncodeSubShapeID(mSubShapeIDCreator2, inX, inY, inTriangle);
-
-			// Determine active edges
-			uint8 active_edges = mShape2->GetEdgeFlags(inX, inY, inTriangle);
-
-			Cast(inV0, inV1, inV2, active_edges, triangle_sub_shape_id);
-		}
-
-		const HeightFieldShape *	mShape2;
-		RayInvDirection				mInvDirection;
-		SubShapeIDCreator			mSubShapeIDCreator2;
-		float						mDistanceStack[cStackSize];
-	};
-
-	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::HeightField);
-	const HeightFieldShape *shape = static_cast<const HeightFieldShape *>(inShape);
-
-	Visitor visitor(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector);
-	visitor.mShape2 = shape;
-	visitor.mInvDirection.Set(inShapeCast.mDirection);
-	visitor.mSubShapeIDCreator2 = inSubShapeIDCreator2;
-	shape->WalkHeightField(visitor);
-}
-
-struct HeightFieldShape::HSGetTrianglesContext
-{
-			HSGetTrianglesContext(const HeightFieldShape *inShape, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) :
-		mDecodeCtx(inShape),
-		mShape(inShape),
-		mLocalBox(Mat44::sInverseRotationTranslation(inRotation, inPositionCOM), inBox),
-		mHeightFieldScale(inScale),
-		mLocalToWorld(Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale)),
-		mIsInsideOut(ScaleHelpers::IsInsideOut(inScale))
-	{
-	}
-
-	bool	ShouldAbort() const
-	{
-		return mShouldAbort;
-	}
-
-	bool	ShouldVisitRangeBlock([[maybe_unused]] int inStackTop) const
-	{
-		return true;
-	}
-
-	int		VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const
-	{
-		// Scale the bounding boxes of this node
-		Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
-		AABox4Scale(mHeightFieldScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
-
-		// Test which nodes collide
-		UVec4 collides = AABox4VsBox(mLocalBox, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
-
-		// Filter out invalid bounding boxes
-		collides = UVec4::sAnd(collides, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY));
-
-		return CountAndSortTrues(collides, ioProperties);
-	}
-
-	void	VisitTriangle(uint inX, uint inY, [[maybe_unused]] uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
-	{
-		// When the buffer is full and we cannot process the triangles, abort the height field walk. The next time GetTrianglesNext is called we will continue here.
-		if (mNumTrianglesFound + 1 > mMaxTrianglesRequested)
-		{
-			mShouldAbort = true;
-			return;
-		}
-
-		// Store vertices as Float3
-		if (mIsInsideOut)
-		{
-			// Reverse vertices
-			(mLocalToWorld * inV0).StoreFloat3(mTriangleVertices++);
-			(mLocalToWorld * inV2).StoreFloat3(mTriangleVertices++);
-			(mLocalToWorld * inV1).StoreFloat3(mTriangleVertices++);
-		}
-		else
-		{
-			// Normal scale
-			(mLocalToWorld * inV0).StoreFloat3(mTriangleVertices++);
-			(mLocalToWorld * inV1).StoreFloat3(mTriangleVertices++);
-			(mLocalToWorld * inV2).StoreFloat3(mTriangleVertices++);
-		}
-
-		// Decode material
-		if (mMaterials != nullptr)
-			*mMaterials++ = mShape->GetMaterial(inX, inY);
-
-		// Accumulate triangles found
-		mNumTrianglesFound++;
-	}
-
-	DecodingContext				mDecodeCtx;
-	const HeightFieldShape *	mShape;
-	OrientedBox					mLocalBox;
-	Vec3						mHeightFieldScale;
-	Mat44						mLocalToWorld;
-	int							mMaxTrianglesRequested;
-	Float3 *					mTriangleVertices;
-	int							mNumTrianglesFound;
-	const PhysicsMaterial **	mMaterials;
-	bool						mShouldAbort;
-	bool						mIsInsideOut;
-};
-
-void HeightFieldShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const
-{
-	static_assert(sizeof(HSGetTrianglesContext) <= sizeof(GetTrianglesContext), "GetTrianglesContext too small");
-	JPH_ASSERT(IsAligned(&ioContext, alignof(HSGetTrianglesContext)));
-
-	new (&ioContext) HSGetTrianglesContext(this, inBox, inPositionCOM, inRotation, inScale);
-}
-
-int HeightFieldShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const
-{
-	static_assert(cGetTrianglesMinTrianglesRequested >= 1, "cGetTrianglesMinTrianglesRequested is too small");
-	JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested);
-
-	// Check if we're done
-	HSGetTrianglesContext &context = (HSGetTrianglesContext &)ioContext;
-	if (context.mDecodeCtx.IsDoneWalking())
-		return 0;
-
-	// Store parameters on context
-	context.mMaxTrianglesRequested = inMaxTrianglesRequested;
-	context.mTriangleVertices = outTriangleVertices;
-	context.mMaterials = outMaterials;
-	context.mShouldAbort = false; // Reset the abort flag
-	context.mNumTrianglesFound = 0;
-
-	// Continue (or start) walking the height field
-	context.mDecodeCtx.WalkHeightField(context);
-	return context.mNumTrianglesFound;
-}
-
-void HeightFieldShape::sCollideConvexVsHeightField(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)
-{
-	JPH_PROFILE_FUNCTION();
-
-	// Get the shapes
-	JPH_ASSERT(inShape1->GetType() == EShapeType::Convex);
-	JPH_ASSERT(inShape2->GetType() == EShapeType::HeightField);
-	const ConvexShape *shape1 = static_cast<const ConvexShape *>(inShape1);
-	const HeightFieldShape *shape2 = static_cast<const HeightFieldShape *>(inShape2);
-
-	struct Visitor : public CollideConvexVsTriangles
-	{
-		using CollideConvexVsTriangles::CollideConvexVsTriangles;
-
-		JPH_INLINE bool				ShouldAbort() const
-		{
-			return mCollector.ShouldEarlyOut();
-		}
-
-		JPH_INLINE bool				ShouldVisitRangeBlock([[maybe_unused]] int inStackTop) const
-		{
-			return true;
-		}
-
-		JPH_INLINE int				VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const
-		{
-			// Scale the bounding boxes of this node
-			Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
-			AABox4Scale(mScale2, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
-
-			// Test which nodes collide
-			UVec4 collides = AABox4VsBox(mBoundsOf1InSpaceOf2, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
-
-			// Filter out invalid bounding boxes
-			collides = UVec4::sAnd(collides, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY));
-
-			return CountAndSortTrues(collides, ioProperties);
-		}
-
-		JPH_INLINE void				VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
-		{
-			// Create ID for triangle
-			SubShapeID triangle_sub_shape_id = mShape2->EncodeSubShapeID(mSubShapeIDCreator2, inX, inY, inTriangle);
-
-			// Determine active edges
-			uint8 active_edges = mShape2->GetEdgeFlags(inX, inY, inTriangle);
-
-			Collide(inV0, inV1, inV2, active_edges, triangle_sub_shape_id);
-		}
-
-		const HeightFieldShape *	mShape2;
-		SubShapeIDCreator			mSubShapeIDCreator2;
-	};
-
-	Visitor visitor(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector);
-	visitor.mShape2 = shape2;
-	visitor.mSubShapeIDCreator2 = inSubShapeIDCreator2;
-	shape2->WalkHeightField(visitor);
-}
-
-void HeightFieldShape::sCollideSphereVsHeightField(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)
-{
-	JPH_PROFILE_FUNCTION();
-
-	// Get the shapes
-	JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::Sphere);
-	JPH_ASSERT(inShape2->GetType() == EShapeType::HeightField);
-	const SphereShape *shape1 = static_cast<const SphereShape *>(inShape1);
-	const HeightFieldShape *shape2 = static_cast<const HeightFieldShape *>(inShape2);
-
-	struct Visitor : public CollideSphereVsTriangles
-	{
-		using CollideSphereVsTriangles::CollideSphereVsTriangles;
-
-		JPH_INLINE bool				ShouldAbort() const
-		{
-			return mCollector.ShouldEarlyOut();
-		}
-
-		JPH_INLINE bool				ShouldVisitRangeBlock([[maybe_unused]] int inStackTop) const
-		{
-			return true;
-		}
-
-		JPH_INLINE int				VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const
-		{
-			// Scale the bounding boxes of this node
-			Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
-			AABox4Scale(mScale2, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
-
-			// Test which nodes collide
-			UVec4 collides = AABox4VsSphere(mSphereCenterIn2, mRadiusPlusMaxSeparationSq, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
-
-			// Filter out invalid bounding boxes
-			collides = UVec4::sAnd(collides, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY));
-
-			return CountAndSortTrues(collides, ioProperties);
-		}
-
-		JPH_INLINE void				VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
-		{
-			// Create ID for triangle
-			SubShapeID triangle_sub_shape_id = mShape2->EncodeSubShapeID(mSubShapeIDCreator2, inX, inY, inTriangle);
-
-			// Determine active edges
-			uint8 active_edges = mShape2->GetEdgeFlags(inX, inY, inTriangle);
-
-			Collide(inV0, inV1, inV2, active_edges, triangle_sub_shape_id);
-		}
-
-		const HeightFieldShape *	mShape2;
-		SubShapeIDCreator			mSubShapeIDCreator2;
-	};
-
-	Visitor visitor(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector);
-	visitor.mShape2 = shape2;
-	visitor.mSubShapeIDCreator2 = inSubShapeIDCreator2;
-	shape2->WalkHeightField(visitor);
-}
-
-void HeightFieldShape::SaveBinaryState(StreamOut &inStream) const
-{
-	Shape::SaveBinaryState(inStream);
-
-	inStream.Write(mOffset);
-	inStream.Write(mScale);
-	inStream.Write(mSampleCount);
-	inStream.Write(mBlockSize);
-	inStream.Write(mBitsPerSample);
-	inStream.Write(mMinSample);
-	inStream.Write(mMaxSample);
-	inStream.Write(mMaterialIndices);
-	inStream.Write(mNumBitsPerMaterialIndex);
-
-	if (mRangeBlocks != nullptr)
-	{
-		inStream.Write(true);
-		inStream.WriteBytes(mRangeBlocks, mRangeBlocksSize * sizeof(RangeBlock) + mHeightSamplesSize + mActiveEdgesSize);
-	}
-	else
-	{
-		inStream.Write(false);
-	}
-}
-
-void HeightFieldShape::RestoreBinaryState(StreamIn &inStream)
-{
-	Shape::RestoreBinaryState(inStream);
-
-	inStream.Read(mOffset);
-	inStream.Read(mScale);
-	inStream.Read(mSampleCount);
-	inStream.Read(mBlockSize);
-	inStream.Read(mBitsPerSample);
-	inStream.Read(mMinSample);
-	inStream.Read(mMaxSample);
-	inStream.Read(mMaterialIndices);
-	inStream.Read(mNumBitsPerMaterialIndex);
-
-	// We don't have the exact number of reserved materials anymore, but ensure that our array is big enough
-	// TODO: Next time when we bump the binary serialization format of this class we should store the capacity and allocate the right amount, for now we accept a little bit of waste
-	mMaterials.reserve(PhysicsMaterialList::size_type(1) << mNumBitsPerMaterialIndex);
-
-	CacheValues();
-
-	bool has_heights = false;
-	inStream.Read(has_heights);
-	if (has_heights)
-	{
-		AllocateBuffers();
-		inStream.ReadBytes(mRangeBlocks, mRangeBlocksSize * sizeof(RangeBlock) + mHeightSamplesSize + mActiveEdgesSize);
-	}
-}
-
-void HeightFieldShape::SaveMaterialState(PhysicsMaterialList &outMaterials) const
-{
-	outMaterials = mMaterials;
-}
-
-void HeightFieldShape::RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials)
-{
-	mMaterials.assign(inMaterials, inMaterials + inNumMaterials);
-}
-
-Shape::Stats HeightFieldShape::GetStats() const
-{
-	return Stats(
-		sizeof(*this)
-			+ mMaterials.size() * sizeof(Ref<PhysicsMaterial>)
-			+ mRangeBlocksSize * sizeof(RangeBlock)
-			+ mHeightSamplesSize * sizeof(uint8)
-			+ mActiveEdgesSize * sizeof(uint8)
-			+ mMaterialIndices.size() * sizeof(uint8),
-		mHeightSamplesSize == 0? 0 : Square(mSampleCount - 1) * 2);
-}
-
-void HeightFieldShape::sRegister()
-{
-	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::HeightField);
-	f.mConstruct = []() -> Shape * { return new HeightFieldShape; };
-	f.mColor = Color::sPurple;
-
-	for (EShapeSubType s : sConvexSubShapeTypes)
-	{
-		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::HeightField, sCollideConvexVsHeightField);
-		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::HeightField, sCastConvexVsHeightField);
-
-		CollisionDispatch::sRegisterCastShape(EShapeSubType::HeightField, s, CollisionDispatch::sReversedCastShape);
-		CollisionDispatch::sRegisterCollideShape(EShapeSubType::HeightField, s, CollisionDispatch::sReversedCollideShape);
-	}
-
-	// Specialized collision functions
-	CollisionDispatch::sRegisterCollideShape(EShapeSubType::Sphere, EShapeSubType::HeightField, sCollideSphereVsHeightField);
-	CollisionDispatch::sRegisterCastShape(EShapeSubType::Sphere, EShapeSubType::HeightField, sCastSphereVsHeightField);
-}
-
-JPH_NAMESPACE_END