Add transform support to deal with Bullets centering of shapes

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>

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);
 	}

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);
 	};

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");

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;