jolt_height_map_shape_3d.cpp

/**************************************************************************/
/*  jolt_height_map_shape_3d.cpp                                          */
/**************************************************************************/
/*                         This file is part of:                          */
/*                             GODOT ENGINE                               */
/*                        https://godotengine.org                         */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
/*                                                                        */
/* Permission is hereby granted, free of charge, to any person obtaining  */
/* a copy of this software and associated documentation files (the        */
/* "Software"), to deal in the Software without restriction, including    */
/* without limitation the rights to use, copy, modify, merge, publish,    */
/* distribute, sublicense, and/or sell copies of the Software, and to     */
/* permit persons to whom the Software is furnished to do so, subject to  */
/* the following conditions:                                              */
/*                                                                        */
/* The above copyright notice and this permission notice shall be         */
/* included in all copies or substantial portions of the Software.        */
/*                                                                        */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
/**************************************************************************/

#include "jolt_height_map_shape_3d.h"

#include "../jolt_project_settings.h"
#include "../misc/jolt_type_conversions.h"

#include "Jolt/Physics/Collision/Shape/HeightFieldShape.h"
#include "Jolt/Physics/Collision/Shape/MeshShape.h"

namespace {

bool _is_vertex_hole(const JPH::VertexList &p_vertices, int p_index) {
	const float height = p_vertices[(size_t)p_index].y;
	return height == FLT_MAX || Math::is_nan(height);
}

bool _is_triangle_hole(const JPH::VertexList &p_vertices, int p_index0, int p_index1, int p_index2) {
	return _is_vertex_hole(p_vertices, p_index0) || _is_vertex_hole(p_vertices, p_index1) || _is_vertex_hole(p_vertices, p_index2);
}

} // namespace

JPH::ShapeRefC JoltHeightMapShape3D::_build() const {
	const int height_count = (int)heights.size();
	if (unlikely(height_count == 0)) {
		return nullptr;
	}

	ERR_FAIL_COND_V_MSG(height_count != width * depth, nullptr, vformat("Failed to build Jolt Physics height map shape with %s. Height count must be the product of width and depth. This shape belongs to %s.", to_string(), _owners_to_string()));
	ERR_FAIL_COND_V_MSG(width < 2 || depth < 2, nullptr, vformat("Failed to build Jolt Physics height map shape with %s. The height map must be at least 2x2. This shape belongs to %s.", to_string(), _owners_to_string()));

	if (width != depth) {
		return JoltShape3D::with_double_sided(_build_mesh(), true);
	}

	const int block_size = 2; // Default of JPH::HeightFieldShapeSettings::mBlockSize
	const int block_count = width / block_size;

	if (block_count < 2) {
		return JoltShape3D::with_double_sided(_build_mesh(), true);
	}

	return JoltShape3D::with_double_sided(_build_height_field(), true);
}

JPH::ShapeRefC JoltHeightMapShape3D::_build_height_field() const {
	const int quad_count_x = width - 1;
	const int quad_count_y = depth - 1;

	const float offset_x = (float)-quad_count_x / 2.0f;
	const float offset_y = (float)-quad_count_y / 2.0f;

	// Jolt triangulates the height map differently from how Godot Physics does it, so we mirror the shape along the
	// Z-axis to get the desired triangulation and reverse the rows to undo the mirroring.

	LocalVector<float> heights_rev;
	heights_rev.resize(heights.size());

	const real_t *heights_ptr = heights.ptr();
	float *heights_rev_ptr = heights_rev.ptr();

	for (int z = 0; z < depth; ++z) {
		const int z_rev = (depth - 1) - z;

		const real_t *row = heights_ptr + ptrdiff_t(z * width);
		float *row_rev = heights_rev_ptr + ptrdiff_t(z_rev * width);

		for (int x = 0; x < width; ++x) {
			const real_t height = row[x];

			// Godot has undocumented (accidental?) support for holes by passing NaN as the height value, whereas Jolt
			// uses `FLT_MAX` instead, so we translate any NaN to `FLT_MAX` in order to be drop-in compatible.
			row_rev[x] = Math::is_nan(height) ? FLT_MAX : (float)height;
		}
	}

	JPH::HeightFieldShapeSettings shape_settings(heights_rev.ptr(), JPH::Vec3(offset_x, 0, offset_y), JPH::Vec3::sOne(), (JPH::uint32)width);

	shape_settings.mBitsPerSample = shape_settings.CalculateBitsPerSampleForError(0.0f);
	shape_settings.mActiveEdgeCosThresholdAngle = JoltProjectSettings::active_edge_threshold_cos;

	const JPH::ShapeSettings::ShapeResult shape_result = shape_settings.Create();
	ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to build Jolt Physics height map shape with %s. It returned the following error: '%s'. This shape belongs to %s.", to_string(), to_godot(shape_result.GetError()), _owners_to_string()));

	return with_scale(shape_result.Get(), Vector3(1, 1, -1));
}

JPH::ShapeRefC JoltHeightMapShape3D::_build_mesh() const {
	const int height_count = (int)heights.size();

	const int quad_count_x = width - 1;
	const int quad_count_z = depth - 1;

	const int quad_count = quad_count_x * quad_count_z;
	const int triangle_count = quad_count * 2;

	JPH::VertexList vertices;
	vertices.reserve((size_t)height_count);

	JPH::IndexedTriangleList indices;
	indices.reserve((size_t)triangle_count);

	const float offset_x = (float)-quad_count_x / 2.0f;
	const float offset_z = (float)-quad_count_z / 2.0f;

	for (int z = 0; z < depth; ++z) {
		for (int x = 0; x < width; ++x) {
			const float vertex_x = offset_x + (float)x;
			const float vertex_y = (float)heights[z * width + x];
			const float vertex_z = offset_z + (float)z;

			vertices.emplace_back(vertex_x, vertex_y, vertex_z);
		}
	}

	for (int z = 0; z < quad_count_z; ++z) {
		for (int x = 0; x < quad_count_x; ++x) {
			const int index_lower_right = z * width + x;
			const int index_lower_left = z * width + (x + 1);
			const int index_upper_right = (z + 1) * width + x;
			const int index_upper_left = (z + 1) * width + (x + 1);

			if (!_is_triangle_hole(vertices, index_lower_right, index_upper_right, index_lower_left)) {
				indices.emplace_back(index_lower_right, index_upper_right, index_lower_left);
			}

			if (!_is_triangle_hole(vertices, index_lower_left, index_upper_right, index_upper_left)) {
				indices.emplace_back(index_lower_left, index_upper_right, index_upper_left);
			}
		}
	}

	JPH::MeshShapeSettings shape_settings(std::move(vertices), std::move(indices));
	shape_settings.mActiveEdgeCosThresholdAngle = JoltProjectSettings::active_edge_threshold_cos;

	const JPH::ShapeSettings::ShapeResult shape_result = shape_settings.Create();
	ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to build Jolt Physics height map shape (as polygon) with %s. It returned the following error: '%s'. This shape belongs to %s.", to_string(), to_godot(shape_result.GetError()), _owners_to_string()));

	return shape_result.Get();
}

AABB JoltHeightMapShape3D::_calculate_aabb() const {
	AABB result;

	const int quad_count_x = width - 1;
	const int quad_count_z = depth - 1;

	const float offset_x = (float)-quad_count_x / 2.0f;
	const float offset_z = (float)-quad_count_z / 2.0f;

	for (int z = 0; z < depth; ++z) {
		for (int x = 0; x < width; ++x) {
			const Vector3 vertex(offset_x + (float)x, (float)heights[z * width + x], offset_z + (float)z);

			if (x == 0 && z == 0) {
				result.position = vertex;
			} else {
				result.expand_to(vertex);
			}
		}
	}

	return result;
}

Variant JoltHeightMapShape3D::get_data() const {
	Dictionary data;
	data["width"] = width;
	data["depth"] = depth;
	data["heights"] = heights;
	return data;
}

void JoltHeightMapShape3D::set_data(const Variant &p_data) {
	ERR_FAIL_COND(p_data.get_type() != Variant::DICTIONARY);

	const Dictionary data = p_data;

	const Variant maybe_heights = data.get("heights", Variant());

#ifdef REAL_T_IS_DOUBLE
	ERR_FAIL_COND(maybe_heights.get_type() != Variant::PACKED_FLOAT64_ARRAY);
#else
	ERR_FAIL_COND(maybe_heights.get_type() != Variant::PACKED_FLOAT32_ARRAY);
#endif

	const Variant maybe_width = data.get("width", Variant());
	ERR_FAIL_COND(maybe_width.get_type() != Variant::INT);

	const Variant maybe_depth = data.get("depth", Variant());
	ERR_FAIL_COND(maybe_depth.get_type() != Variant::INT);

	heights = maybe_heights;
	width = maybe_width;
	depth = maybe_depth;

	aabb = _calculate_aabb();

	destroy();
}

String JoltHeightMapShape3D::to_string() const {
	return vformat("{height_count=%d width=%d depth=%d}", heights.size(), width, depth);
}