5 files changed, 30 insertions, 17 deletions

diff --git a/doc/classes/HeightMapShape.xml b/doc/classes/HeightMapShape.xml
index 9968551b7a..22896535b9 100644
--- a/doc/classes/HeightMapShape.xml
+++ b/doc/classes/HeightMapShape.xml
@@ -4,7 +4,7 @@
 		Height map shape for 3D physics (Bullet only).
 	</brief_description>
 	<description>
-		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.
+		Height map shape resource, which can be added to a [PhysicsBody] or [Area].
 	</description>
 	<tutorials>
 	</tutorials>
diff --git a/modules/bullet/collision_object_bullet.cpp b/modules/bullet/collision_object_bullet.cpp
index eb87901c24..166d7e6158 100644
--- a/modules/bullet/collision_object_bullet.cpp
+++ b/modules/bullet/collision_object_bullet.cpp
@@ -59,6 +59,25 @@ void CollisionObjectBullet::ShapeWrapper::set_transform(const btTransform &p_tra
 	transform = p_transform;
 }
 
+btTransform CollisionObjectBullet::ShapeWrapper::get_adjusted_transform() const {
+	if (shape->get_type() == PhysicsServer::SHAPE_HEIGHTMAP) {
+		const HeightMapShapeBullet *hm_shape = (const HeightMapShapeBullet *)shape; // should be safe to cast now
+		btTransform adjusted_transform;
+
+		// Bullet centers our heightmap:
+		// https://github.com/bulletphysics/bullet3/blob/master/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h#L33
+		// This is really counter intuitive so we're adjusting for it
+
+		adjusted_transform.setIdentity();
+		adjusted_transform.setOrigin(btVector3(0.0, hm_shape->min_height + ((hm_shape->max_height - hm_shape->min_height) * 0.5), 0.0));
+		adjusted_transform *= transform;
+
+		return adjusted_transform;
+	} else {
+		return transform;
+	}
+}
+
 void CollisionObjectBullet::ShapeWrapper::claim_bt_shape(const btVector3 &body_scale) {
 	if (!bt_shape) {
 		if (active)
@@ -345,7 +364,8 @@ void RigidCollisionObjectBullet::reload_shapes() {
 	// Try to optimize by not using compound
 	if (1 == shape_count) {
 		shpWrapper = &shapes.write[0];
-		if (shpWrapper->transform.getOrigin().isZero() && shpWrapper->transform.getBasis() == shpWrapper->transform.getBasis().getIdentity()) {
+		btTransform transform = shpWrapper->get_adjusted_transform();
+		if (transform.getOrigin().isZero() && transform.getBasis() == transform.getBasis().getIdentity()) {
 			shpWrapper->claim_bt_shape(body_scale);
 			mainShape = shpWrapper->bt_shape;
 			main_shape_changed();
@@ -359,7 +379,7 @@ void RigidCollisionObjectBullet::reload_shapes() {
 	for (int i(0); i < shape_count; ++i) {
 		shpWrapper = &shapes.write[i];
 		shpWrapper->claim_bt_shape(body_scale);
-		btTransform scaled_shape_transform(shpWrapper->transform);
+		btTransform scaled_shape_transform(shpWrapper->get_adjusted_transform());
 		scaled_shape_transform.getOrigin() *= body_scale;
 		compoundShape->addChildShape(scaled_shape_transform, shpWrapper->bt_shape);
 	}
diff --git a/modules/bullet/collision_object_bullet.h b/modules/bullet/collision_object_bullet.h
index e65bc52caf..c9430bec18 100644
--- a/modules/bullet/collision_object_bullet.h
+++ b/modules/bullet/collision_object_bullet.h
@@ -109,6 +109,7 @@ public:
 
 		void set_transform(const Transform &p_transform);
 		void set_transform(const btTransform &p_transform);
+		btTransform get_adjusted_transform() const;
 
 		void claim_bt_shape(const btVector3 &body_scale);
 	};
diff --git a/scene/3d/collision_shape.cpp b/scene/3d/collision_shape.cpp
index f32b1398fd..3190e1e0b2 100644
--- a/scene/3d/collision_shape.cpp
+++ b/scene/3d/collision_shape.cpp
@@ -65,7 +65,6 @@ void CollisionShape::make_convex_from_brothers() {
 }
 
 void CollisionShape::_update_in_shape_owner(bool p_xform_only) {
-
 	parent->shape_owner_set_transform(owner_id, get_transform());
 	if (p_xform_only)
 		return;
@@ -228,6 +227,9 @@ void CollisionShape::_update_debug_shape() {
 }
 
 void CollisionShape::_shape_changed() {
+	// If this is a heightfield shape our center may have changed
+	_update_in_shape_owner(true);
+
 	if (is_inside_tree() && get_tree()->is_debugging_collisions_hint() && !debug_shape_dirty) {
 		debug_shape_dirty = true;
 		call_deferred("_update_debug_shape");
diff --git a/scene/resources/height_map_shape.cpp b/scene/resources/height_map_shape.cpp
index 98b597ac0a..8cd271dab0 100644
--- a/scene/resources/height_map_shape.cpp
+++ b/scene/resources/height_map_shape.cpp
@@ -44,16 +44,6 @@ Vector<Vector3> HeightMapShape::_gen_debug_mesh_lines() {
 
 		PoolRealArray::Read r = map_data.read();
 
-		// 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);
-
 		// reserve some memory for our points..
 		points.resize(((map_width - 1) * map_depth * 2) + (map_width * (map_depth - 1) * 2));
 
@@ -64,16 +54,16 @@ Vector<Vector3> HeightMapShape::_gen_debug_mesh_lines() {
 			Vector3 height(start.x, 0.0, start.y);
 
 			for (int w = 0; w < map_width; w++) {
-				height.y = r[r_offset++] - center;
+				height.y = r[r_offset++];
 
 				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);
+					points.write[w_offset++] = Vector3(height.x + 1.0, r[r_offset], 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);
+					points.write[w_offset++] = Vector3(height.x, r[r_offset + map_width - 1], height.z + 1.0);
 				}
 
 				height.x += 1.0;