Center shape according to logic Bullet applies

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>

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;