Draw triangles for HeightMapShape debug collision

Helps with ambiguous cases where it's not possible to tell which diagonal is used for collision in quads.

1 files changed, 6 insertions, 1 deletions

diff --git a/scene/resources/height_map_shape_3d.cpp b/scene/resources/height_map_shape_3d.cpp
index 5593bb766f..447284c5be 100644
--- a/scene/resources/height_map_shape_3d.cpp
+++ b/scene/resources/height_map_shape_3d.cpp
@@ -44,7 +44,7 @@ Vector<Vector3> HeightMapShape3D::get_debug_mesh_lines() const {
 		const real_t *r = map_data.ptr();
 
 		// reserve some memory for our points..
-		points.resize(((map_width - 1) * map_depth * 2) + (map_width * (map_depth - 1) * 2));
+		points.resize(((map_width - 1) * map_depth * 2) + (map_width * (map_depth - 1) * 2) + ((map_width - 1) * (map_depth - 1) * 2));
 
 		// now set our points
 		int r_offset = 0;
@@ -65,6 +65,11 @@ Vector<Vector3> HeightMapShape3D::get_debug_mesh_lines() const {
 					points.write[w_offset++] = Vector3(height.x, r[r_offset + map_width - 1], height.z + 1.0);
 				}
 
+				if ((w != map_width - 1) && (d != map_depth - 1)) {
+					points.write[w_offset++] = Vector3(height.x + 1.0, r[r_offset], height.z);
+					points.write[w_offset++] = Vector3(height.x, r[r_offset + map_width - 1], height.z + 1.0);
+				}
+
 				height.x += 1.0;
 			}