1 files changed, 42 insertions, 24 deletions

diff --git a/modules/bullet/collision_object_bullet.cpp b/modules/bullet/collision_object_bullet.cpp
index 70d604fa70..57e4db708e 100644
--- a/modules/bullet/collision_object_bullet.cpp
+++ b/modules/bullet/collision_object_bullet.cpp
@@ -1,10 +1,9 @@
 /*************************************************************************/
 /*  collision_object_bullet.cpp                                          */
-/*  Author: AndreaCatania                                                */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
-/*                    http://www.godotengine.org                         */
+/*                      https://godotengine.org                          */
 /*************************************************************************/
 /* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur.                 */
 /* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md)    */
@@ -30,21 +29,29 @@
 /*************************************************************************/
 
 #include "collision_object_bullet.h"
+
 #include "area_bullet.h"
-#include "btBulletCollisionCommon.h"
 #include "bullet_physics_server.h"
 #include "bullet_types_converter.h"
 #include "bullet_utilities.h"
 #include "shape_bullet.h"
 #include "space_bullet.h"
 
+#include <btBulletCollisionCommon.h>
+
+/**
+	@author AndreaCatania
+*/
+
 #define enableDynamicAabbTree true
 #define initialChildCapacity 1
 
 CollisionObjectBullet::ShapeWrapper::~ShapeWrapper() {}
 
 void CollisionObjectBullet::ShapeWrapper::set_transform(const Transform &p_transform) {
+	G_TO_B(p_transform.get_basis().get_scale_abs(), scale);
 	G_TO_B(p_transform, transform);
+	UNSCALE_BT_BASIS(transform);
 }
 void CollisionObjectBullet::ShapeWrapper::set_transform(const btTransform &p_transform) {
 	transform = p_transform;
@@ -57,15 +64,14 @@ CollisionObjectBullet::CollisionObjectBullet(Type p_type) :
 		collisionsEnabled(true),
 		m_isStatic(false),
 		bt_collision_object(NULL),
-		body_scale(1., 1., 1.) {}
+		body_scale(1., 1., 1.),
+		force_shape_reset(false) {}
 
 CollisionObjectBullet::~CollisionObjectBullet() {
-	// Remove all overlapping
+	// Remove all overlapping, notify is not required since godot take care of it
 	for (int i = areasOverlapped.size() - 1; 0 <= i; --i) {
-		areasOverlapped[i]->remove_overlapping_instantly(this);
+		areasOverlapped[i]->remove_overlap(this, /*Notify*/ false);
 	}
-	// not required
-	// areasOverlapped.clear();
 
 	destroyBulletCollisionObject();
 }
@@ -88,6 +94,7 @@ btVector3 CollisionObjectBullet::get_bt_body_scale() const {
 }
 
 void CollisionObjectBullet::on_body_scale_changed() {
+	force_shape_reset = true;
 }
 
 void CollisionObjectBullet::destroyBulletCollisionObject() {
@@ -151,16 +158,13 @@ int CollisionObjectBullet::get_godot_object_flags() const {
 
 void CollisionObjectBullet::set_transform(const Transform &p_global_transform) {
 
-	btTransform btTrans;
-	Basis decomposed_basis;
-
-	Vector3 decomposed_scale = p_global_transform.get_basis().rotref_posscale_decomposition(decomposed_basis);
+	set_body_scale(p_global_transform.basis.get_scale_abs());
 
-	G_TO_B(p_global_transform.get_origin(), btTrans.getOrigin());
-	G_TO_B(decomposed_basis, btTrans.getBasis());
+	btTransform bt_transform;
+	G_TO_B(p_global_transform, bt_transform);
+	UNSCALE_BT_BASIS(bt_transform);
 
-	set_body_scale(decomposed_scale);
-	set_transform__bullet(btTrans);
+	set_transform__bullet(bt_transform);
 }
 
 Transform CollisionObjectBullet::get_transform() const {
@@ -228,7 +232,7 @@ void RigidCollisionObjectBullet::set_shape_transform(int p_index, const Transfor
 	ERR_FAIL_INDEX(p_index, get_shape_count());
 
 	shapes[p_index].set_transform(p_transform);
-	on_shapes_changed();
+	on_shape_changed(shapes[p_index].shape);
 }
 
 void RigidCollisionObjectBullet::remove_shape(ShapeBullet *p_shape) {
@@ -289,27 +293,41 @@ void RigidCollisionObjectBullet::on_shape_changed(const ShapeBullet *const p_sha
 
 void RigidCollisionObjectBullet::on_shapes_changed() {
 	int i;
+
 	// Remove all shapes, reverse order for performance reason (Array resize)
 	for (i = compoundShape->getNumChildShapes() - 1; 0 <= i; --i) {
 		compoundShape->removeChildShapeByIndex(i);
 	}
 
-	// Insert all shapes
 	ShapeWrapper *shpWrapper;
-	const int size = shapes.size();
-	for (i = 0; i < size; ++i) {
+	const int shapes_size = shapes.size();
+
+	// Reset shape if required
+	if (force_shape_reset) {
+		for (i = 0; i < shapes_size; ++i) {
+			shpWrapper = &shapes[i];
+			bulletdelete(shpWrapper->bt_shape);
+		}
+		force_shape_reset = false;
+	}
+
+	// Insert all shapes
+	btVector3 body_scale(get_bt_body_scale());
+	for (i = 0; i < shapes_size; ++i) {
 		shpWrapper = &shapes[i];
 		if (shpWrapper->active) {
 			if (!shpWrapper->bt_shape) {
-				shpWrapper->bt_shape = shpWrapper->shape->create_bt_shape();
+				shpWrapper->bt_shape = shpWrapper->shape->create_bt_shape(shpWrapper->scale * body_scale);
 			}
-			compoundShape->addChildShape(shpWrapper->transform, shpWrapper->bt_shape);
+
+			btTransform scaled_shape_transform(shpWrapper->transform);
+			scaled_shape_transform.getOrigin() *= body_scale;
+			compoundShape->addChildShape(scaled_shape_transform, shpWrapper->bt_shape);
 		} else {
-			compoundShape->addChildShape(shpWrapper->transform, BulletPhysicsServer::get_empty_shape());
+			compoundShape->addChildShape(btTransform(), BulletPhysicsServer::get_empty_shape());
 		}
 	}
 
-	compoundShape->setLocalScaling(get_bt_body_scale());
 	compoundShape->recalculateLocalAabb();
 }