5 files changed, 55 insertions, 24 deletions
diff --git a/doc/classes/HeightMapShape.xml b/doc/classes/HeightMapShape.xml
index 5ffeda1c10..22896535b9 100644
--- a/doc/classes/HeightMapShape.xml
+++ b/doc/classes/HeightMapShape.xml
@@ -1,7 +1,7 @@
 <?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].
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 32e9c527ef..8cd271dab0 100644
--- a/scene/resources/height_map_shape.cpp
+++ b/scene/resources/height_map_shape.cpp
@@ -34,35 +34,43 @@
 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++];
+		// reserve some memory for our points..
+		points.resize(((map_width - 1) * map_depth * 2) + (map_width * (map_depth - 1) * 2));
 
-			if (w != map_width - 1) {
-				points.push_back(height);
-				points.push_back(Vector3(height.x + 1.0, r[offset], height.z));
-			}
+		// 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++];
+
+				if (w != map_width - 1) {
+					points.write[w_offset++] = height;
+					points.write[w_offset++] = Vector3(height.x + 1.0, r[r_offset], height.z);
+				}
 
-			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 (d != map_depth - 1) {
+					points.write[w_offset++] = height;
+					points.write[w_offset++] = Vector3(height.x, r[r_offset + map_width - 1], height.z + 1.0);
+				}
+
+				height.x += 1.0;
 			}
 
-			height.x += 1.0;
+			start.y += 1.0;
 		}
-
-		start.y += 1.0;
 	}
 
 	return points;