From 4bb0df7060329390890d74ede7ec848eef552b16 Mon Sep 17 00:00:00 2001
From: Bastiaan Olij <mux213@gmail.com>
Date: Tue, 23 Apr 2019 21:51:56 +1000
Subject: [PATCH] Center shape according to logic Bullet applies

---
 doc/classes/HeightMapShape.xml       |  4 +-
 scene/resources/height_map_shape.cpp | 60 ++++++++++++++++++----------
 2 files changed, 41 insertions(+), 23 deletions(-)

diff --git a/doc/classes/HeightMapShape.xml b/doc/classes/HeightMapShape.xml
index 5ffeda1c109..9968551b7ab 100644
--- a/doc/classes/HeightMapShape.xml
+++ b/doc/classes/HeightMapShape.xml
@@ -1,10 +1,10 @@
 <?xml version="1.0" encoding="UTF-8" ?>
 <class name="HeightMapShape" inherits="Shape" category="Core" version="3.2">
 	<brief_description>
-		Height map shape for 3D physics (bullet only)
+		Height map shape for 3D physics (Bullet only).
 	</brief_description>
 	<description>
-		Height map shape resource, which can be added to a [PhysicsBody] or [Area].
+		Height map shape resource, which can be added to a [PhysicsBody] or [Area]. Note that bullet will always center the collision shape. If you minimum height is 0, and you maximum height is 10, bullet will adjust your collision shape down so the minimum height is -5 and the maximum height is 5.
 	</description>
 	<tutorials>
 	</tutorials>
diff --git a/scene/resources/height_map_shape.cpp b/scene/resources/height_map_shape.cpp
index 32e9c527ef5..98b597ac0a5 100644
--- a/scene/resources/height_map_shape.cpp
+++ b/scene/resources/height_map_shape.cpp
@@ -34,35 +34,53 @@
 Vector<Vector3> HeightMapShape::_gen_debug_mesh_lines() {
 	Vector<Vector3> points;
 
-	// This will be slow for large maps...
-	// also we'll have to figure out how well bullet centers this shape...
+	if ((map_width != 0) && (map_depth != 0)) {
 
-	Vector2 size(map_width - 1, map_depth - 1);
-	Vector2 start = size * -0.5;
-	int offset = 0;
+		// This will be slow for large maps...
+		// also we'll have to figure out how well bullet centers this shape...
 
-	PoolRealArray::Read r = map_data.read();
+		Vector2 size(map_width - 1, map_depth - 1);
+		Vector2 start = size * -0.5;
 
-	for (int d = 0; d < map_depth; d++) {
-		Vector3 height(start.x, 0.0, start.y);
+		PoolRealArray::Read r = map_data.read();
 
-		for (int w = 0; w < map_width; w++) {
-			height.y = r[offset++];
+		// Bullet centers our heightmap, this is really counter intuitive but for now we'll adjust our debug shape accordingly:
+		// https://github.com/bulletphysics/bullet3/blob/master/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h#L33
+		float min = r[0];
+		float max = r[0];
+		for (int i = 0; i < map_data.size(); i++) {
+			if (min > r[i]) min = r[i];
+			if (max < r[i]) max = r[i];
+		};
+		float center = min + ((max - min) * 0.5);
 
-			if (w != map_width - 1) {
-				points.push_back(height);
-				points.push_back(Vector3(height.x + 1.0, r[offset], height.z));
+		// reserve some memory for our points..
+		points.resize(((map_width - 1) * map_depth * 2) + (map_width * (map_depth - 1) * 2));
+
+		// now set our points
+		int r_offset = 0;
+		int w_offset = 0;
+		for (int d = 0; d < map_depth; d++) {
+			Vector3 height(start.x, 0.0, start.y);
+
+			for (int w = 0; w < map_width; w++) {
+				height.y = r[r_offset++] - center;
+
+				if (w != map_width - 1) {
+					points.write[w_offset++] = height;
+					points.write[w_offset++] = Vector3(height.x + 1.0, r[r_offset] - center, height.z);
+				}
+
+				if (d != map_depth - 1) {
+					points.write[w_offset++] = height;
+					points.write[w_offset++] = Vector3(height.x, r[r_offset + map_width - 1] - center, height.z + 1.0);
+				}
+
+				height.x += 1.0;
 			}
 
-			if (d != map_depth - 1) {
-				points.push_back(height);
-				points.push_back(Vector3(height.x, r[offset + map_width - 1], height.z + 1.0));
-			}
-
-			height.x += 1.0;
+			start.y += 1.0;
 		}
-
-		start.y += 1.0;
 	}
 
 	return points;