Center shape according to logic Bullet applies

2 files changed, 40 insertions, 22 deletions

diff --git a/doc/classes/HeightMapShape.xml b/doc/classes/HeightMapShape.xml
index 5ffeda1c10..9968551b7a 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 32e9c527ef..98b597ac0a 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 (d != map_depth - 1) {
-				points.push_back(height);
-				points.push_back(Vector3(height.x, r[offset + map_width - 1], height.z + 1.0));
+				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;
 			}
 
-			height.x += 1.0;
+			start.y += 1.0;
 		}
-
-		start.y += 1.0;
 	}
 
 	return points;