Added function to update the materials of a height field (#934)

Docs/ReleaseNotes.md

@@ -24,6 +24,7 @@ For breaking API changes see [this document](https://github.com/jrouwe/JoltPhysi
 * Added ability to override the max tire impulse calculations for wheeled vehicles. See WheeledVehicleController::SetTireMaxImpulseCallback.
 * Added user data to CharacterVirtual.
 * Added fraction hint to PathConstraintPath::GetClosestPoint. This can be used to speed up the search along the curve and to disambiguate fractions in case a path reaches the same point multiple times (i.e. a figure-8).
+* Added ability to update the height field materials after creation.
 
 ### Improvements
 * Multithreading the SetupVelocityConstraints job. This was causing a bottleneck in the case that there are a lot of constraints but very few possible collisions.

Jolt/Math/Real.h

@@ -38,7 +38,7 @@ using RMat44Arg = Mat44Arg;
 // Put the 'real' operator in a namespace so that users can opt in to use it:
 // using namespace JPH::literals;
 namespace literals {
-	constexpr Real operator "" _r (long double inValue) { return Real(inValue); }
+	constexpr Real operator ""_r (long double inValue) { return Real(inValue); }
 };
 
 JPH_NAMESPACE_END

Jolt/Physics/Collision/Shape/HeightFieldShape.cpp

@@ -1129,6 +1129,193 @@ void HeightFieldShape::SetHeights(uint inX, uint inY, uint inSizeX, uint inSizeY
 #endif
 }
 
+void HeightFieldShape::GetMaterials(uint inX, uint inY, uint inSizeX, uint inSizeY, uint8 *outMaterials, uint 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, uint 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);
+	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
+					inAllocator.Free(material_remap_table, material_remap_table_size);
+					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); // 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;
+		}
+	}
+
+	// Free the remapping table
+	inAllocator.Free(material_remap_table, material_remap_table_size);
+	return true;
+}
+
 MassProperties HeightFieldShape::GetMassProperties() const
 {
 	// Object should always be static, return default mass properties

Jolt/Physics/Collision/Shape/HeightFieldShape.h

@@ -203,6 +203,30 @@ class JPH_EXPORT HeightFieldShape final : public Shape
 	/// @param inActiveEdgeCosThresholdAngle Cosine of the threshold angle (if the angle between the two triangles is bigger than this, the edge is active, note that a concave edge is always inactive).
 	void							SetHeights(uint inX, uint inY, uint inSizeX, uint inSizeY, const float *inHeights, uint inHeightsStride, TempAllocator &inAllocator, float inActiveEdgeCosThresholdAngle = 0.996195f);
 
+	/// Get the current list of materials, the indices returned by GetMaterials() will index into this list.
+	const PhysicsMaterialList &		GetMaterialList() const						{ return mMaterials; }
+
+	/// Get the material indices of a block of data.
+	/// @param inX Start X position, must in the range [0, mSampleCount - 1]
+	/// @param inY Start Y position, must in the range [0, mSampleCount - 1]
+	/// @param inSizeX Number of samples in X direction
+	/// @param inSizeY Number of samples in Y direction
+	/// @param outMaterials Returned material indices, must be at least inSizeX * inSizeY uint8s. Values are returned in x-major order.
+	/// @param inMaterialsStride Stride in uint8s between two consecutive rows of outMaterials.
+	void							GetMaterials(uint inX, uint inY, uint inSizeX, uint inSizeY, uint8 *outMaterials, uint inMaterialsStride) const;
+
+	/// Set the material indices of a block of data.
+	/// @param inX Start X position, must in the range [0, mSampleCount - 1]
+	/// @param inY Start Y position, must in the range [0, mSampleCount - 1]
+	/// @param inSizeX Number of samples in X direction
+	/// @param inSizeY Number of samples in Y direction
+	/// @param inMaterials The new material indices, must be at least inSizeX * inSizeY uint8s. Values are returned in x-major order.
+	/// @param inMaterialsStride Stride in uint8s between two consecutive rows of inMaterials.
+	/// @param inMaterialList The material list to use for the new material indices or nullptr if the material list should not be updated
+	/// @param inAllocator Allocator to use for temporary memory
+	/// @return True if the material indices were set, false if the total number of materials exceeded 256
+	bool							SetMaterials(uint inX, uint inY, uint inSizeX, uint inSizeY, const uint8 *inMaterials, uint inMaterialsStride, const PhysicsMaterialList *inMaterialList, TempAllocator &inAllocator);
+
 	// See Shape
 	virtual void					SaveBinaryState(StreamOut &inStream) const override;
 	virtual void					SaveMaterialState(PhysicsMaterialList &outMaterials) const override;

UnitTests/Physics/HeightFieldShapeTests.cpp

@@ -320,4 +320,145 @@ TEST_SUITE("HeightFieldShapeTests")
 					CHECK(verify_heights[idx] == original_heights[idx]); // We didn't modify this and it is outside of the affected range
 			}
 	}
+
+	TEST_CASE("TestSetMaterials")
+	{
+		constexpr uint cSampleCount = 32;
+
+		PhysicsMaterialRefC material_0 = new PhysicsMaterialSimple("Material 0", Color::sGetDistinctColor(0));
+		PhysicsMaterialRefC material_1 = new PhysicsMaterialSimple("Material 1", Color::sGetDistinctColor(1));
+		PhysicsMaterialRefC material_2 = new PhysicsMaterialSimple("Material 2", Color::sGetDistinctColor(2));
+		PhysicsMaterialRefC material_3 = new PhysicsMaterialSimple("Material 3", Color::sGetDistinctColor(3));
+		PhysicsMaterialRefC material_4 = new PhysicsMaterialSimple("Material 4", Color::sGetDistinctColor(4));
+		PhysicsMaterialRefC material_5 = new PhysicsMaterialSimple("Material 5", Color::sGetDistinctColor(5));
+
+		// Create height field with a single material
+		HeightFieldShapeSettings settings;
+		settings.mSampleCount = cSampleCount;
+		settings.mBitsPerSample = 8;
+		settings.mBlockSize = 4;
+		settings.mHeightSamples.resize(Square(cSampleCount));
+		for (float &h : settings.mHeightSamples)
+			h = 0.0f;
+		settings.mMaterials.push_back(material_0);
+		settings.mMaterialIndices.resize(Square(cSampleCount - 1));
+		for (uint8 &m : settings.mMaterialIndices)
+			m = 0;
+
+		// Store the current state
+		Array<const PhysicsMaterial *> current_state;
+		current_state.resize(Square(cSampleCount - 1));
+		for (const PhysicsMaterial *&m : current_state)
+			m = material_0;
+
+		// Create shape
+		Ref<Shape> shape = settings.Create().Get();
+		HeightFieldShape *height_field = static_cast<HeightFieldShape *>(shape.GetPtr());
+
+		// Check that the material is set
+		auto check_materials = [height_field, &current_state]() {
+			const PhysicsMaterialList &material_list = height_field->GetMaterialList();
+
+			uint sample_count_min_1 = height_field->GetSampleCount() - 1;
+
+			Array<uint8> material_indices;
+			material_indices.resize(Square(sample_count_min_1));
+			height_field->GetMaterials(0, 0, sample_count_min_1, sample_count_min_1, material_indices.data(), sample_count_min_1);
+
+			for (uint i = 0; i < (uint)current_state.size(); ++i)
+				CHECK(current_state[i] == material_list[material_indices[i]]);
+		};
+		check_materials();
+
+		// Function to randomize materials
+		auto update_materials = [height_field, &current_state](uint inStartX, uint inStartY, uint inSizeX, uint inSizeY, const PhysicsMaterialList *inMaterialList) {
+			TempAllocatorMalloc temp_allocator;
+
+			const PhysicsMaterialList &material_list = inMaterialList != nullptr? *inMaterialList : height_field->GetMaterialList();
+
+			UnitTestRandom random;
+			uniform_int_distribution<uint> index_distribution(0, uint(material_list.size()) - 1);
+
+			uint sample_count_min_1 = height_field->GetSampleCount() - 1;
+
+			Array<uint8> patched_materials;
+			patched_materials.resize(inSizeX * inSizeY);
+			for (uint y = 0; y < inSizeY; ++y)
+				for (uint x = 0; x < inSizeX; ++x)
+				{
+					// Initialize the patch
+					uint8 index = uint8(index_distribution(random));
+					patched_materials[y * inSizeX + x] = index;
+
+					// Update reference state
+					current_state[(inStartY + y) * sample_count_min_1 + inStartX + x] = material_list[index];
+				}
+			CHECK(height_field->SetMaterials(inStartX, inStartY, inSizeX, inSizeY, patched_materials.data(), inSizeX, inMaterialList, temp_allocator));
+		};
+
+		{
+			// Add material 1
+			PhysicsMaterialList patched_materials_list;
+			patched_materials_list.push_back(material_1);
+			patched_materials_list.push_back(material_0);
+			update_materials(4, 16, 16, 8, &patched_materials_list);
+			check_materials();
+		}
+
+		{
+			// Add material 2
+			PhysicsMaterialList patched_materials_list;
+			patched_materials_list.push_back(material_0);
+			patched_materials_list.push_back(material_2);
+			update_materials(8, 16, 16, 8, &patched_materials_list);
+			check_materials();
+		}
+
+		{
+			// Add material 3
+			PhysicsMaterialList patched_materials_list;
+			patched_materials_list.push_back(material_0);
+			patched_materials_list.push_back(material_1);
+			patched_materials_list.push_back(material_2);
+			patched_materials_list.push_back(material_3);
+			update_materials(8, 8, 16, 8, &patched_materials_list);
+			check_materials();
+		}
+
+		{
+			// Add material 4
+			PhysicsMaterialList patched_materials_list;
+			patched_materials_list.push_back(material_0);
+			patched_materials_list.push_back(material_1);
+			patched_materials_list.push_back(material_4);
+			patched_materials_list.push_back(material_2);
+			patched_materials_list.push_back(material_3);
+			update_materials(0, 0, 30, 30, &patched_materials_list);
+			check_materials();
+		}
+
+		{
+			// Add material 5
+			PhysicsMaterialList patched_materials_list;
+			patched_materials_list.push_back(material_4);
+			patched_materials_list.push_back(material_3);
+			patched_materials_list.push_back(material_0);
+			patched_materials_list.push_back(material_1);
+			patched_materials_list.push_back(material_2);
+			patched_materials_list.push_back(material_5);
+			update_materials(1, 1, 30, 30, &patched_materials_list);
+			check_materials();
+		}
+
+		{
+			// Update materials without new material list
+			update_materials(2, 5, 10, 15, nullptr);
+			check_materials();
+		}
+
+		// Check materials using GetMaterial call
+		for (uint y = 0; y < cSampleCount - 1; ++y)
+			for (uint x = 0; x < cSampleCount - 1; ++x)
+				CHECK(height_field->GetMaterial(x, y) == current_state[y * (cSampleCount - 1) + x]);
+	}
 }