Merge pull request #22759 from AndreaCatania/pyfix

Some physics fixes for 3.1

14 files changed, 180 insertions, 191 deletions

diff --git a/modules/bullet/area_bullet.cpp b/modules/bullet/area_bullet.cpp
index a3ba3aa0bf..a0486443c2 100644
--- a/modules/bullet/area_bullet.cpp
+++ b/modules/bullet/area_bullet.cpp
@@ -58,7 +58,7 @@ AreaBullet::AreaBullet() :
 		isScratched(false) {
 
 	btGhost = bulletnew(btGhostObject);
-	btGhost->setCollisionShape(BulletPhysicsServer::get_empty_shape());
+	reload_shapes();
 	setupBulletCollisionObject(btGhost);
 	/// Collision objects with a callback still have collision response with dynamic rigid bodies.
 	/// In order to use collision objects as trigger, you have to disable the collision response.
@@ -166,11 +166,9 @@ bool AreaBullet::is_monitoring() const {
 	return get_godot_object_flags() & GOF_IS_MONITORING_AREA;
 }
 
-void AreaBullet::main_shape_resetted() {
-	if (get_main_shape())
-		btGhost->setCollisionShape(get_main_shape());
-	else
-		btGhost->setCollisionShape(BulletPhysicsServer::get_empty_shape());
+void AreaBullet::main_shape_changed() {
+	CRASH_COND(!get_main_shape())
+	btGhost->setCollisionShape(get_main_shape());
 }
 
 void AreaBullet::reload_body() {
diff --git a/modules/bullet/area_bullet.h b/modules/bullet/area_bullet.h
index a6bf73906c..1c5962cfe3 100644
--- a/modules/bullet/area_bullet.h
+++ b/modules/bullet/area_bullet.h
@@ -142,7 +142,7 @@ public:
 	_FORCE_INLINE_ void set_spOv_priority(int p_priority) { spOv_priority = p_priority; }
 	_FORCE_INLINE_ int get_spOv_priority() { return spOv_priority; }
 
-	virtual void main_shape_resetted();
+	virtual void main_shape_changed();
 	virtual void reload_body();
 	virtual void set_space(SpaceBullet *p_space);
 
@@ -157,6 +157,7 @@ public:
 
 	virtual void on_collision_filters_change();
 	virtual void on_collision_checker_start() {}
+	virtual void on_collision_checker_end() { isTransformChanged = false; }
 
 	void add_overlap(CollisionObjectBullet *p_otherObject);
 	void put_overlap_as_exit(int p_index);
diff --git a/modules/bullet/bullet_physics_server.cpp b/modules/bullet/bullet_physics_server.cpp
index 53a38967c3..315afe3d72 100644
--- a/modules/bullet/bullet_physics_server.cpp
+++ b/modules/bullet/bullet_physics_server.cpp
@@ -74,12 +74,6 @@
 	body->get_space()->add_constraint(joint, joint->is_disabled_collisions_between_bodies());
 // <--------------- Joint creation asserts
 
-btEmptyShape *BulletPhysicsServer::emptyShape(ShapeBullet::create_shape_empty());
-
-btEmptyShape *BulletPhysicsServer::get_empty_shape() {
-	return emptyShape;
-}
-
 void BulletPhysicsServer::_bind_methods() {
 	//ClassDB::bind_method(D_METHOD("DoTest"), &BulletPhysicsServer::DoTest);
 }
@@ -89,9 +83,7 @@ BulletPhysicsServer::BulletPhysicsServer() :
 		active(true),
 		active_spaces_count(0) {}
 
-BulletPhysicsServer::~BulletPhysicsServer() {
-	bulletdelete(emptyShape);
-}
+BulletPhysicsServer::~BulletPhysicsServer() {}
 
 RID BulletPhysicsServer::shape_create(ShapeType p_shape) {
 	ShapeBullet *shape = NULL;
@@ -338,7 +330,7 @@ Transform BulletPhysicsServer::area_get_shape_transform(RID p_area, int p_shape_
 void BulletPhysicsServer::area_remove_shape(RID p_area, int p_shape_idx) {
 	AreaBullet *area = area_owner.get(p_area);
 	ERR_FAIL_COND(!area);
-	return area->remove_shape(p_shape_idx);
+	return area->remove_shape_full(p_shape_idx);
 }
 
 void BulletPhysicsServer::area_clear_shapes(RID p_area) {
@@ -346,7 +338,7 @@ void BulletPhysicsServer::area_clear_shapes(RID p_area) {
 	ERR_FAIL_COND(!area);
 
 	for (int i = area->get_shape_count(); 0 < i; --i)
-		area->remove_shape(0);
+		area->remove_shape_full(0);
 }
 
 void BulletPhysicsServer::area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled) {
@@ -567,7 +559,7 @@ void BulletPhysicsServer::body_remove_shape(RID p_body, int p_shape_idx) {
 	RigidBodyBullet *body = rigid_body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 
-	body->remove_shape(p_shape_idx);
+	body->remove_shape_full(p_shape_idx);
 }
 
 void BulletPhysicsServer::body_clear_shapes(RID p_body) {
@@ -1486,7 +1478,7 @@ void BulletPhysicsServer::free(RID p_rid) {
 
 		// Notify the shape is configured
 		for (Map<ShapeOwnerBullet *, int>::Element *element = shape->get_owners().front(); element; element = element->next()) {
-			static_cast<ShapeOwnerBullet *>(element->key())->remove_shape(shape);
+			static_cast<ShapeOwnerBullet *>(element->key())->remove_shape_full(shape);
 		}
 
 		shape_owner.free(p_rid);
@@ -1497,7 +1489,7 @@ void BulletPhysicsServer::free(RID p_rid) {
 
 		body->set_space(NULL);
 
-		body->remove_all_shapes(true);
+		body->remove_all_shapes(true, true);
 
 		rigid_body_owner.free(p_rid);
 		bulletdelete(body);
@@ -1517,7 +1509,7 @@ void BulletPhysicsServer::free(RID p_rid) {
 
 		area->set_space(NULL);
 
-		area->remove_all_shapes(true);
+		area->remove_all_shapes(true, true);
 
 		area_owner.free(p_rid);
 		bulletdelete(area);
diff --git a/modules/bullet/bullet_physics_server.h b/modules/bullet/bullet_physics_server.h
index 4c52cace67..6b7dcd86e6 100644
--- a/modules/bullet/bullet_physics_server.h
+++ b/modules/bullet/bullet_physics_server.h
@@ -60,13 +60,6 @@ class BulletPhysicsServer : public PhysicsServer {
 	mutable RID_Owner<SoftBodyBullet> soft_body_owner;
 	mutable RID_Owner<JointBullet> joint_owner;
 
-private:
-	/// This is used as replacement of collision shape inside a compound or main shape
-	static btEmptyShape *emptyShape;
-
-public:
-	static btEmptyShape *get_empty_shape();
-
 protected:
 	static void _bind_methods();
 
diff --git a/modules/bullet/collision_object_bullet.cpp b/modules/bullet/collision_object_bullet.cpp
index 61834b8e3f..a3944b4f99 100644
--- a/modules/bullet/collision_object_bullet.cpp
+++ b/modules/bullet/collision_object_bullet.cpp
@@ -43,8 +43,7 @@
 	@author AndreaCatania
 */
 
-#define enableDynamicAabbTree true
-#define initialChildCapacity 1
+#define enableDynamicAabbTree false
 
 CollisionObjectBullet::ShapeWrapper::~ShapeWrapper() {}
 
@@ -60,7 +59,10 @@ void CollisionObjectBullet::ShapeWrapper::set_transform(const btTransform &p_tra
 
 void CollisionObjectBullet::ShapeWrapper::claim_bt_shape(const btVector3 &body_scale) {
 	if (!bt_shape) {
-		bt_shape = shape->create_bt_shape(scale * body_scale);
+		if (active)
+			bt_shape = shape->create_bt_shape(scale * body_scale);
+		else
+			bt_shape = ShapeBullet::create_shape_empty();
 	}
 }
 
@@ -72,7 +74,8 @@ CollisionObjectBullet::CollisionObjectBullet(Type p_type) :
 		bt_collision_object(NULL),
 		body_scale(1., 1., 1.),
 		force_shape_reset(false),
-		space(NULL) {}
+		space(NULL),
+		isTransformChanged(false) {}
 
 CollisionObjectBullet::~CollisionObjectBullet() {
 	// Remove all overlapping, notify is not required since godot take care of it
@@ -90,7 +93,7 @@ bool equal(real_t first, real_t second) {
 void CollisionObjectBullet::set_body_scale(const Vector3 &p_new_scale) {
 	if (!equal(p_new_scale[0], body_scale[0]) || !equal(p_new_scale[1], body_scale[1]) || !equal(p_new_scale[2], body_scale[2])) {
 		body_scale = p_new_scale;
-		on_body_scale_changed();
+		body_scale_changed();
 	}
 }
 
@@ -100,7 +103,7 @@ btVector3 CollisionObjectBullet::get_bt_body_scale() const {
 	return s;
 }
 
-void CollisionObjectBullet::on_body_scale_changed() {
+void CollisionObjectBullet::body_scale_changed() {
 	force_shape_reset = true;
 }
 
@@ -186,49 +189,33 @@ Transform CollisionObjectBullet::get_transform() const {
 
 void CollisionObjectBullet::set_transform__bullet(const btTransform &p_global_transform) {
 	bt_collision_object->setWorldTransform(p_global_transform);
+	notify_transform_changed();
 }
 
 const btTransform &CollisionObjectBullet::get_transform__bullet() const {
 	return bt_collision_object->getWorldTransform();
 }
 
+void CollisionObjectBullet::notify_transform_changed() {
+	isTransformChanged = true;
+}
+
 RigidCollisionObjectBullet::RigidCollisionObjectBullet(Type p_type) :
 		CollisionObjectBullet(p_type),
 		mainShape(NULL) {
 }
 
 RigidCollisionObjectBullet::~RigidCollisionObjectBullet() {
-	remove_all_shapes(true);
+	remove_all_shapes(true, true);
 	if (mainShape && mainShape->isCompound()) {
 		bulletdelete(mainShape);
 	}
 }
 
-/* Not used
-void RigidCollisionObjectBullet::_internal_replaceShape(btCollisionShape *p_old_shape, btCollisionShape *p_new_shape) {
-	bool at_least_one_was_changed = false;
-	btTransform old_transf;
-	// Inverse because I need remove the shapes
-	// Fetch all shapes to be sure to remove all shapes
-	for (int i = compoundShape->getNumChildShapes() - 1; 0 <= i; --i) {
-		if (compoundShape->getChildShape(i) == p_old_shape) {
-
-			old_transf = compoundShape->getChildTransform(i);
-			compoundShape->removeChildShapeByIndex(i);
-			compoundShape->addChildShape(old_transf, p_new_shape);
-			at_least_one_was_changed = true;
-		}
-	}
-
-	if (at_least_one_was_changed) {
-		on_shapes_changed();
-	}
-}*/
-
 void RigidCollisionObjectBullet::add_shape(ShapeBullet *p_shape, const Transform &p_transform) {
 	shapes.push_back(ShapeWrapper(p_shape, p_transform, true));
 	p_shape->add_owner(this);
-	on_shapes_changed();
+	reload_shapes();
 }
 
 void RigidCollisionObjectBullet::set_shape(int p_index, ShapeBullet *p_shape) {
@@ -236,17 +223,31 @@ void RigidCollisionObjectBullet::set_shape(int p_index, ShapeBullet *p_shape) {
 	shp.shape->remove_owner(this);
 	p_shape->add_owner(this);
 	shp.shape = p_shape;
-	on_shapes_changed();
+	reload_shapes();
 }
 
-void RigidCollisionObjectBullet::set_shape_transform(int p_index, const Transform &p_transform) {
-	ERR_FAIL_INDEX(p_index, get_shape_count());
+int RigidCollisionObjectBullet::get_shape_count() const {
+	return shapes.size();
+}
 
-	shapes.write[p_index].set_transform(p_transform);
-	on_shape_changed(shapes.write[p_index].shape);
+ShapeBullet *RigidCollisionObjectBullet::get_shape(int p_index) const {
+	return shapes[p_index].shape;
 }
 
-void RigidCollisionObjectBullet::remove_shape(ShapeBullet *p_shape) {
+btCollisionShape *RigidCollisionObjectBullet::get_bt_shape(int p_index) const {
+	return shapes[p_index].bt_shape;
+}
+
+int RigidCollisionObjectBullet::find_shape(ShapeBullet *p_shape) const {
+	const int size = shapes.size();
+	for (int i = 0; i < size; ++i) {
+		if (shapes[i].shape == p_shape)
+			return i;
+	}
+	return -1;
+}
+
+void RigidCollisionObjectBullet::remove_shape_full(ShapeBullet *p_shape) {
 	// Remove the shape, all the times it appears
 	// Reverse order required for delete.
 	for (int i = shapes.size() - 1; 0 <= i; --i) {
@@ -255,35 +256,32 @@ void RigidCollisionObjectBullet::remove_shape(ShapeBullet *p_shape) {
 			shapes.remove(i);
 		}
 	}
-	on_shapes_changed();
+	reload_shapes();
 }
 
-void RigidCollisionObjectBullet::remove_shape(int p_index) {
+void RigidCollisionObjectBullet::remove_shape_full(int p_index) {
 	ERR_FAIL_INDEX(p_index, get_shape_count());
 	internal_shape_destroy(p_index);
 	shapes.remove(p_index);
-	on_shapes_changed();
+	reload_shapes();
 }
 
-void RigidCollisionObjectBullet::remove_all_shapes(bool p_permanentlyFromThisBody) {
+void RigidCollisionObjectBullet::remove_all_shapes(bool p_permanentlyFromThisBody, bool p_force_not_reload) {
 	// Reverse order required for delete.
 	for (int i = shapes.size() - 1; 0 <= i; --i) {
 		internal_shape_destroy(i, p_permanentlyFromThisBody);
 	}
 	shapes.clear();
-	on_shapes_changed();
+	if (!p_force_not_reload)
+		reload_shapes();
 }
 
-int RigidCollisionObjectBullet::get_shape_count() const {
-	return shapes.size();
-}
-
-ShapeBullet *RigidCollisionObjectBullet::get_shape(int p_index) const {
-	return shapes[p_index].shape;
-}
+void RigidCollisionObjectBullet::set_shape_transform(int p_index, const Transform &p_transform) {
+	ERR_FAIL_INDEX(p_index, get_shape_count());
 
-btCollisionShape *RigidCollisionObjectBullet::get_bt_shape(int p_index) const {
-	return shapes[p_index].bt_shape;
+	shapes.write[p_index].set_transform(p_transform);
+	// Note, enableDynamicAabbTree is false because on transform change compound is destroyed
+	reload_shapes();
 }
 
 const btTransform &RigidCollisionObjectBullet::get_bt_shape_transform(int p_index) const {
@@ -296,21 +294,26 @@ Transform RigidCollisionObjectBullet::get_shape_transform(int p_index) const {
 	return trs;
 }
 
-void RigidCollisionObjectBullet::on_shape_changed(const ShapeBullet *const p_shape) {
-	const int size = shapes.size();
-	for (int i = 0; i < size; ++i) {
-		if (shapes[i].shape == p_shape) {
-			bulletdelete(shapes.write[i].bt_shape);
-		}
-	}
-	on_shapes_changed();
+void RigidCollisionObjectBullet::set_shape_disabled(int p_index, bool p_disabled) {
+	shapes.write[p_index].active = !p_disabled;
+	shape_changed(p_index);
+}
+
+bool RigidCollisionObjectBullet::is_shape_disabled(int p_index) {
+	return !shapes[p_index].active;
 }
 
-void RigidCollisionObjectBullet::on_shapes_changed() {
+void RigidCollisionObjectBullet::shape_changed(int p_shape_index) {
+	bulletdelete(shapes.write[p_shape_index].bt_shape);
+	reload_shapes();
+}
 
-	if (mainShape && mainShape->isCompound()) {
+void RigidCollisionObjectBullet::reload_shapes() {
+
+	if (mainShape && mainShape->isCompound())
+		// Destroy compound
 		bulletdelete(mainShape);
-	}
+
 	mainShape = NULL;
 
 	ShapeWrapper *shpWrapper;
@@ -325,55 +328,38 @@ void RigidCollisionObjectBullet::on_shapes_changed() {
 		force_shape_reset = false;
 	}
 
-	btVector3 body_scale(get_bt_body_scale());
-
-	if (!shape_count)
-		return;
+	const btVector3 body_scale(get_bt_body_scale());
 
 	// Try to optimize by not using compound
 	if (1 == shape_count) {
 		shpWrapper = &shapes.write[0];
-		if (shpWrapper->active && shpWrapper->transform.getOrigin().isZero() && shpWrapper->transform.getBasis() == shpWrapper->transform.getBasis().getIdentity()) {
+		if (shpWrapper->transform.getOrigin().isZero() && shpWrapper->transform.getBasis() == shpWrapper->transform.getBasis().getIdentity()) {
 			shpWrapper->claim_bt_shape(body_scale);
 			mainShape = shpWrapper->bt_shape;
-			main_shape_resetted();
+			main_shape_changed();
 			return;
 		}
 	}
 
-	btCompoundShape *compoundShape = bulletnew(btCompoundShape(enableDynamicAabbTree, initialChildCapacity));
+	// Optimization not possible use a compound shape
+	btCompoundShape *compoundShape = bulletnew(btCompoundShape(enableDynamicAabbTree, shape_count));
 
-	// Insert all shapes into compound
 	for (int i(0); i < shape_count; ++i) {
 		shpWrapper = &shapes.write[i];
-		if (shpWrapper->active) {
-			shpWrapper->claim_bt_shape(body_scale);
-
-			btTransform scaled_shape_transform(shpWrapper->transform);
-			scaled_shape_transform.getOrigin() *= body_scale;
-			compoundShape->addChildShape(scaled_shape_transform, shpWrapper->bt_shape);
-		} else {
-			compoundShape->addChildShape(btTransform(), BulletPhysicsServer::get_empty_shape());
-		}
+		shpWrapper->claim_bt_shape(body_scale);
+		btTransform scaled_shape_transform(shpWrapper->transform);
+		scaled_shape_transform.getOrigin() *= body_scale;
+		compoundShape->addChildShape(scaled_shape_transform, shpWrapper->bt_shape);
 	}
 
 	compoundShape->recalculateLocalAabb();
 	mainShape = compoundShape;
-	main_shape_resetted();
-}
-
-void RigidCollisionObjectBullet::set_shape_disabled(int p_index, bool p_disabled) {
-	shapes.write[p_index].active = !p_disabled;
-	on_shapes_changed();
-}
-
-bool RigidCollisionObjectBullet::is_shape_disabled(int p_index) {
-	return !shapes[p_index].active;
+	main_shape_changed();
 }
 
-void RigidCollisionObjectBullet::on_body_scale_changed() {
-	CollisionObjectBullet::on_body_scale_changed();
-	on_shapes_changed();
+void RigidCollisionObjectBullet::body_scale_changed() {
+	CollisionObjectBullet::body_scale_changed();
+	reload_shapes();
 }
 
 void RigidCollisionObjectBullet::internal_shape_destroy(int p_index, bool p_permanentlyFromThisBody) {
diff --git a/modules/bullet/collision_object_bullet.h b/modules/bullet/collision_object_bullet.h
index ea06cecb17..4a0c805ce5 100644
--- a/modules/bullet/collision_object_bullet.h
+++ b/modules/bullet/collision_object_bullet.h
@@ -132,6 +132,7 @@ protected:
 	/// New area is added when overlap with new area (AreaBullet::addOverlap), then is removed when it exit (CollisionObjectBullet::onExitArea)
 	/// This array is used mainly to know which area hold the pointer of this object
 	Vector<AreaBullet *> areasOverlapped;
+	bool isTransformChanged;
 
 public:
 	CollisionObjectBullet(Type p_type);
@@ -157,7 +158,7 @@ public:
 	void set_body_scale(const Vector3 &p_new_scale);
 	const Vector3 &get_body_scale() const { return body_scale; }
 	btVector3 get_bt_body_scale() const;
-	virtual void on_body_scale_changed();
+	virtual void body_scale_changed();
 
 	void add_collision_exception(const CollisionObjectBullet *p_ignoreCollisionObject);
 	void remove_collision_exception(const CollisionObjectBullet *p_ignoreCollisionObject);
@@ -185,8 +186,9 @@ public:
 	virtual void reload_body() = 0;
 	virtual void set_space(SpaceBullet *p_space) = 0;
 	_FORCE_INLINE_ SpaceBullet *get_space() const { return space; }
-	/// This is an event that is called when a collision checker starts
+
 	virtual void on_collision_checker_start() = 0;
+	virtual void on_collision_checker_end() = 0;
 
 	virtual void dispatch_callbacks() = 0;
 
@@ -197,7 +199,6 @@ public:
 	virtual void on_enter_area(AreaBullet *p_area) = 0;
 	virtual void on_exit_area(AreaBullet *p_area);
 
-	/// GodotObjectFlags
 	void set_godot_object_flags(int flags);
 	int get_godot_object_flags() const;
 
@@ -205,11 +206,13 @@ public:
 	Transform get_transform() const;
 	virtual void set_transform__bullet(const btTransform &p_global_transform);
 	virtual const btTransform &get_transform__bullet() const;
+
+	bool is_transform_changed() const { return isTransformChanged; }
+	virtual void notify_transform_changed();
 };
 
 class RigidCollisionObjectBullet : public CollisionObjectBullet, public ShapeOwnerBullet {
 protected:
-	/// This could be a compound shape in case multi please collision are found
 	btCollisionShape *mainShape;
 	Vector<ShapeWrapper> shapes;
 
@@ -219,31 +222,34 @@ public:
 
 	_FORCE_INLINE_ const Vector<ShapeWrapper> &get_shapes_wrappers() const { return shapes; }
 
-	/// This is used to set new shape or replace existing
-	//virtual void _internal_replaceShape(btCollisionShape *p_old_shape, btCollisionShape *p_new_shape) = 0;
+	_FORCE_INLINE_ btCollisionShape *get_main_shape() const { return mainShape; }
+
 	void add_shape(ShapeBullet *p_shape, const Transform &p_transform = Transform());
 	void set_shape(int p_index, ShapeBullet *p_shape);
-	void set_shape_transform(int p_index, const Transform &p_transform);
-	virtual void remove_shape(ShapeBullet *p_shape);
-	void remove_shape(int p_index);
-	void remove_all_shapes(bool p_permanentlyFromThisBody = false);
-
-	virtual void on_shape_changed(const ShapeBullet *const p_shape);
-	virtual void on_shapes_changed();
-
-	_FORCE_INLINE_ btCollisionShape *get_main_shape() const { return mainShape; }
 
 	int get_shape_count() const;
 	ShapeBullet *get_shape(int p_index) const;
 	btCollisionShape *get_bt_shape(int p_index) const;
+
+	int find_shape(ShapeBullet *p_shape) const;
+
+	virtual void remove_shape_full(ShapeBullet *p_shape);
+	void remove_shape_full(int p_index);
+	void remove_all_shapes(bool p_permanentlyFromThisBody = false, bool p_force_not_reload = false);
+
+	void set_shape_transform(int p_index, const Transform &p_transform);
+
 	const btTransform &get_bt_shape_transform(int p_index) const;
 	Transform get_shape_transform(int p_index) const;
 
 	void set_shape_disabled(int p_index, bool p_disabled);
 	bool is_shape_disabled(int p_index);
 
-	virtual void main_shape_resetted() = 0;
-	virtual void on_body_scale_changed();
+	virtual void shape_changed(int p_shape_index);
+	virtual void reload_shapes();
+
+	virtual void main_shape_changed() = 0;
+	virtual void body_scale_changed();
 
 private:
 	void internal_shape_destroy(int p_index, bool p_permanentlyFromThisBody = false);
diff --git a/modules/bullet/godot_motion_state.h b/modules/bullet/godot_motion_state.h
index fa58e86589..5844ef8bf3 100644
--- a/modules/bullet/godot_motion_state.h
+++ b/modules/bullet/godot_motion_state.h
@@ -82,7 +82,7 @@ public:
 	virtual void setWorldTransform(const btTransform &worldTrans) {
 		bodyCurrentWorldTransform = worldTrans;
 
-		owner->scratch();
+		owner->notify_transform_changed();
 	}
 
 public:
diff --git a/modules/bullet/rigid_body_bullet.cpp b/modules/bullet/rigid_body_bullet.cpp
index d9a77885b3..37e7718969 100644
--- a/modules/bullet/rigid_body_bullet.cpp
+++ b/modules/bullet/rigid_body_bullet.cpp
@@ -265,13 +265,13 @@ RigidBodyBullet::RigidBodyBullet() :
 		angularDamp(0),
 		can_sleep(true),
 		omit_forces_integration(false),
+		can_integrate_forces(false),
 		maxCollisionsDetection(0),
 		collisionsCount(0),
 		maxAreasWhereIam(10),
 		areaWhereIamCount(0),
 		countGravityPointSpaces(0),
 		isScratchedSpaceOverrideModificator(false),
-		isTransformChanged(false),
 		previousActiveState(true),
 		force_integration_callback(NULL) {
 
@@ -279,9 +279,10 @@ RigidBodyBullet::RigidBodyBullet() :
 
 	// Initial properties
 	const btVector3 localInertia(0, 0, 0);
-	btRigidBody::btRigidBodyConstructionInfo cInfo(mass, godotMotionState, BulletPhysicsServer::get_empty_shape(), localInertia);
+	btRigidBody::btRigidBodyConstructionInfo cInfo(mass, godotMotionState, NULL, localInertia);
 
 	btBody = bulletnew(btRigidBody(cInfo));
+	reload_shapes();
 	setupBulletCollisionObject(btBody);
 
 	set_mode(PhysicsServer::BODY_MODE_RIGID);
@@ -314,11 +315,9 @@ void RigidBodyBullet::destroy_kinematic_utilities() {
 	}
 }
 
-void RigidBodyBullet::main_shape_resetted() {
-	if (get_main_shape())
-		btBody->setCollisionShape(get_main_shape());
-	else
-		btBody->setCollisionShape(BulletPhysicsServer::get_empty_shape());
+void RigidBodyBullet::main_shape_changed() {
+	CRASH_COND(!get_main_shape())
+	btBody->setCollisionShape(get_main_shape());
 	set_continuous_collision_detection(is_continuous_collision_detection_enabled()); // Reset
 }
 
@@ -333,7 +332,7 @@ void RigidBodyBullet::reload_body() {
 void RigidBodyBullet::set_space(SpaceBullet *p_space) {
 	// Clear the old space if there is one
 	if (space) {
-		isTransformChanged = false;
+		can_integrate_forces = false;
 
 		// Remove all eventual constraints
 		assert_no_constraints();
@@ -350,8 +349,8 @@ void RigidBodyBullet::set_space(SpaceBullet *p_space) {
 }
 
 void RigidBodyBullet::dispatch_callbacks() {
-	/// The check isTransformChanged is necessary in order to call integrated forces only when the first transform is sent
-	if ((btBody->isKinematicObject() || btBody->isActive() || previousActiveState != btBody->isActive()) && force_integration_callback && isTransformChanged) {
+	/// The check isFirstTransformChanged is necessary in order to call integrated forces only when the first transform is sent
+	if ((btBody->isKinematicObject() || btBody->isActive() || previousActiveState != btBody->isActive()) && force_integration_callback && can_integrate_forces) {
 
 		if (omit_forces_integration)
 			btBody->clearForces();
@@ -400,10 +399,6 @@ void RigidBodyBullet::set_force_integration_callback(ObjectID p_id, const String
 	}
 }
 
-void RigidBodyBullet::scratch() {
-	isTransformChanged = true;
-}
-
 void RigidBodyBullet::scratch_space_override_modificator() {
 	isScratchedSpaceOverrideModificator = true;
 }
@@ -418,6 +413,11 @@ void RigidBodyBullet::on_collision_checker_start() {
 	collisionsCount = 0;
 }
 
+void RigidBodyBullet::on_collision_checker_end() {
+	// Always true if active and not a static or kinematic body
+	isTransformChanged = btBody->isActive() && !btBody->isStaticOrKinematicObject();
+}
+
 bool RigidBodyBullet::add_collision_object(RigidBodyBullet *p_otherObject, const Vector3 &p_hitWorldLocation, const Vector3 &p_hitLocalLocation, const Vector3 &p_hitNormal, const float &p_appliedImpulse, int p_other_shape_index, int p_local_shape_index) {
 
 	if (collisionsCount >= maxCollisionsDetection) {
@@ -520,7 +520,7 @@ real_t RigidBodyBullet::get_param(PhysicsServer::BodyParameter p_param) const {
 
 void RigidBodyBullet::set_mode(PhysicsServer::BodyMode p_mode) {
 	// This is necessary to block force_integration untile next move
-	isTransformChanged = false;
+	can_integrate_forces = false;
 	destroy_kinematic_utilities();
 	// The mode change is relevant to its mass
 	switch (p_mode) {
@@ -777,8 +777,7 @@ void RigidBodyBullet::set_transform__bullet(const btTransform &p_global_transfor
 		// The kinematic use MotionState class
 		godotMotionState->moveBody(p_global_transform);
 	}
-	btBody->setWorldTransform(p_global_transform);
-	scratch();
+	CollisionObjectBullet::set_transform__bullet(p_global_transform);
 }
 
 const btTransform &RigidBodyBullet::get_transform__bullet() const {
@@ -791,8 +790,8 @@ const btTransform &RigidBodyBullet::get_transform__bullet() const {
 	}
 }
 
-void RigidBodyBullet::on_shapes_changed() {
-	RigidCollisionObjectBullet::on_shapes_changed();
+void RigidBodyBullet::reload_shapes() {
+	RigidCollisionObjectBullet::reload_shapes();
 
 	const btScalar invMass = btBody->getInvMass();
 	const btScalar mass = invMass == 0 ? 0 : 1 / invMass;
@@ -987,6 +986,11 @@ void RigidBodyBullet::reload_kinematic_shapes() {
 	kinematic_utilities->copyAllOwnerShapes();
 }
 
+void RigidBodyBullet::notify_transform_changed() {
+	RigidCollisionObjectBullet::notify_transform_changed();
+	can_integrate_forces = true;
+}
+
 void RigidBodyBullet::_internal_set_mass(real_t p_mass) {
 
 	btVector3 localInertia(0, 0, 0);
diff --git a/modules/bullet/rigid_body_bullet.h b/modules/bullet/rigid_body_bullet.h
index 25dac30951..26e5018c87 100644
--- a/modules/bullet/rigid_body_bullet.h
+++ b/modules/bullet/rigid_body_bullet.h
@@ -202,6 +202,7 @@ private:
 	real_t angularDamp;
 	bool can_sleep;
 	bool omit_forces_integration;
+	bool can_integrate_forces;
 
 	Vector<CollisionData> collisions;
 	// these parameters are used to avoid vector resize
@@ -216,7 +217,6 @@ private:
 	int countGravityPointSpaces;
 	bool isScratchedSpaceOverrideModificator;
 
-	bool isTransformChanged;
 	bool previousActiveState; // Last check state
 
 	ForceIntegrationCallback *force_integration_callback;
@@ -231,17 +231,18 @@ public:
 
 	_FORCE_INLINE_ btRigidBody *get_bt_rigid_body() { return btBody; }
 
-	virtual void main_shape_resetted();
+	virtual void main_shape_changed();
 	virtual void reload_body();
 	virtual void set_space(SpaceBullet *p_space);
 
 	virtual void dispatch_callbacks();
 	void set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata = Variant());
-	void scratch();
 	void scratch_space_override_modificator();
 
 	virtual void on_collision_filters_change();
 	virtual void on_collision_checker_start();
+	virtual void on_collision_checker_end();
+
 	void set_max_collisions_detection(int p_maxCollisionsDetection) {
 		maxCollisionsDetection = p_maxCollisionsDetection;
 		collisions.resize(p_maxCollisionsDetection);
@@ -302,7 +303,7 @@ public:
 	virtual void set_transform__bullet(const btTransform &p_global_transform);
 	virtual const btTransform &get_transform__bullet() const;
 
-	virtual void on_shapes_changed();
+	virtual void reload_shapes();
 
 	virtual void on_enter_area(AreaBullet *p_area);
 	virtual void on_exit_area(AreaBullet *p_area);
@@ -311,6 +312,8 @@ public:
 	/// Kinematic
 	void reload_kinematic_shapes();
 
+	virtual void notify_transform_changed();
+
 private:
 	void _internal_set_mass(real_t p_mass);
 };
diff --git a/modules/bullet/shape_bullet.cpp b/modules/bullet/shape_bullet.cpp
index 4a2263407c..8bb621a863 100644
--- a/modules/bullet/shape_bullet.cpp
+++ b/modules/bullet/shape_bullet.cpp
@@ -34,6 +34,7 @@
 #include "bullet_physics_server.h"
 #include "bullet_types_converter.h"
 #include "bullet_utilities.h"
+#include "core/project_settings.h"
 #include "shape_owner_bullet.h"
 
 #include <BulletCollision/CollisionDispatch/btInternalEdgeUtility.h>
@@ -64,7 +65,8 @@ btCollisionShape *ShapeBullet::prepare(btCollisionShape *p_btShape) const {
 
 void ShapeBullet::notifyShapeChanged() {
 	for (Map<ShapeOwnerBullet *, int>::Element *E = owners.front(); E; E = E->next()) {
-		static_cast<ShapeOwnerBullet *>(E->key())->on_shape_changed(this);
+		ShapeOwnerBullet *owner = static_cast<ShapeOwnerBullet *>(E->key());
+		owner->shape_changed(owner->find_shape(this));
 	}
 }
 
@@ -400,18 +402,22 @@ void ConcavePolygonShapeBullet::setup(PoolVector<Vector3> p_faces) {
 		btVector3 supVec_1;
 		btVector3 supVec_2;
 		for (int i = 0; i < src_face_count; ++i) {
-			G_TO_B(facesr[i * 3], supVec_0);
+			G_TO_B(facesr[i * 3 + 0], supVec_0);
 			G_TO_B(facesr[i * 3 + 1], supVec_1);
 			G_TO_B(facesr[i * 3 + 2], supVec_2);
 
-			shapeInterface->addTriangle(supVec_0, supVec_1, supVec_2);
+			// Inverted from standard godot otherwise btGenerateInternalEdgeInfo generates wrong edge info
+			shapeInterface->addTriangle(supVec_2, supVec_1, supVec_0);
 		}
 
 		const bool useQuantizedAabbCompression = true;
 
 		meshShape = bulletnew(btBvhTriangleMeshShape(shapeInterface, useQuantizedAabbCompression));
-		btTriangleInfoMap *triangleInfoMap = new btTriangleInfoMap();
-		btGenerateInternalEdgeInfo(meshShape, triangleInfoMap);
+
+		if (GLOBAL_DEF("physics/3d/smooth_trimesh_collision", false)) {
+			btTriangleInfoMap *triangleInfoMap = new btTriangleInfoMap();
+			btGenerateInternalEdgeInfo(meshShape, triangleInfoMap);
+		}
 	} else {
 		meshShape = NULL;
 		ERR_PRINT("The faces count are 0, the mesh shape cannot be created");
@@ -426,6 +432,7 @@ btCollisionShape *ConcavePolygonShapeBullet::create_bt_shape(const btVector3 &p_
 		cs = ShapeBullet::create_shape_empty();
 	cs->setLocalScaling(p_implicit_scale);
 	prepare(cs);
+	cs->setMargin(0);
 	return cs;
 }
 
diff --git a/modules/bullet/shape_owner_bullet.h b/modules/bullet/shape_owner_bullet.h
index 29d42d12f2..72ddbc482c 100644
--- a/modules/bullet/shape_owner_bullet.h
+++ b/modules/bullet/shape_owner_bullet.h
@@ -45,11 +45,10 @@ class CollisionObjectBullet;
 /// E.G. BodyShape is a child of this
 class ShapeOwnerBullet {
 public:
-	/// This is used to set new shape or replace existing
-	//virtual void _internal_replaceShape(btCollisionShape *p_old_shape, btCollisionShape *p_new_shape) = 0;
-	virtual void on_shape_changed(const ShapeBullet *const p_shape) = 0;
-	virtual void on_shapes_changed() = 0;
-	virtual void remove_shape(class ShapeBullet *p_shape) = 0;
+	virtual int find_shape(ShapeBullet *p_shape) const = 0;
+	virtual void shape_changed(int p_shape_index) = 0;
+	virtual void reload_shapes() = 0;
+	virtual void remove_shape_full(class ShapeBullet *p_shape) = 0;
 	virtual ~ShapeOwnerBullet() {}
 };
 #endif
diff --git a/modules/bullet/soft_body_bullet.cpp b/modules/bullet/soft_body_bullet.cpp
index f373ce5db4..94f350210f 100644
--- a/modules/bullet/soft_body_bullet.cpp
+++ b/modules/bullet/soft_body_bullet.cpp
@@ -72,12 +72,6 @@ void SoftBodyBullet::set_space(SpaceBullet *p_space) {
 	}
 }
 
-void SoftBodyBullet::dispatch_callbacks() {}
-
-void SoftBodyBullet::on_collision_filters_change() {}
-
-void SoftBodyBullet::on_collision_checker_start() {}
-
 void SoftBodyBullet::on_enter_area(AreaBullet *p_area) {}
 
 void SoftBodyBullet::on_exit_area(AreaBullet *p_area) {}
diff --git a/modules/bullet/soft_body_bullet.h b/modules/bullet/soft_body_bullet.h
index c775193584..d04bfca046 100644
--- a/modules/bullet/soft_body_bullet.h
+++ b/modules/bullet/soft_body_bullet.h
@@ -91,9 +91,10 @@ public:
 	virtual void reload_body();
 	virtual void set_space(SpaceBullet *p_space);
 
-	virtual void dispatch_callbacks();
-	virtual void on_collision_filters_change();
-	virtual void on_collision_checker_start();
+	virtual void dispatch_callbacks() {}
+	virtual void on_collision_filters_change() {}
+	virtual void on_collision_checker_start() {}
+	virtual void on_collision_checker_end() {}
 	virtual void on_enter_area(AreaBullet *p_area);
 	virtual void on_exit_area(AreaBullet *p_area);
 
diff --git a/modules/bullet/space_bullet.cpp b/modules/bullet/space_bullet.cpp
index ba4c72f4c7..ab2d1781ad 100644
--- a/modules/bullet/space_bullet.cpp
+++ b/modules/bullet/space_bullet.cpp
@@ -175,7 +175,7 @@ bool BulletPhysicsDirectSpaceState::cast_motion(const RID &p_shape, const Transf
 	btResult.m_collisionFilterGroup = 0;
 	btResult.m_collisionFilterMask = p_collision_mask;
 
-	space->dynamicsWorld->convexSweepTest(bt_convex_shape, bt_xform_from, bt_xform_to, btResult, 0.002);
+	space->dynamicsWorld->convexSweepTest(bt_convex_shape, bt_xform_from, bt_xform_to, btResult, space->dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration);
 
 	r_closest_unsafe = 1.0;
 	r_closest_safe = 1.0;
@@ -540,17 +540,20 @@ void onBulletPreTickCallback(btDynamicsWorld *p_dynamicsWorld, btScalar timeStep
 
 void onBulletTickCallback(btDynamicsWorld *p_dynamicsWorld, btScalar timeStep) {
 
-	// Notify all Collision objects the collision checker is started
 	const btCollisionObjectArray &colObjArray = p_dynamicsWorld->getCollisionObjectArray();
+
+	// Notify all Collision objects the collision checker is started
 	for (int i = colObjArray.size() - 1; 0 <= i; --i) {
-		CollisionObjectBullet *colObj = static_cast<CollisionObjectBullet *>(colObjArray[i]->getUserPointer());
-		assert(NULL != colObj);
-		colObj->on_collision_checker_start();
+		static_cast<CollisionObjectBullet *>(colObjArray[i]->getUserPointer())->on_collision_checker_start();
 	}
 
 	SpaceBullet *sb = static_cast<SpaceBullet *>(p_dynamicsWorld->getWorldUserInfo());
 	sb->check_ghost_overlaps();
 	sb->check_body_collision();
+
+	for (int i = colObjArray.size() - 1; 0 <= i; --i) {
+		static_cast<CollisionObjectBullet *>(colObjArray[i]->getUserPointer())->on_collision_checker_end();
+	}
 }
 
 BulletPhysicsDirectSpaceState *SpaceBullet::get_direct_state() {
@@ -571,7 +574,6 @@ void SpaceBullet::create_empty_world(bool p_create_soft_world) {
 
 	gjk_epa_pen_solver = bulletnew(btGjkEpaPenetrationDepthSolver);
 	gjk_simplex_solver = bulletnew(btVoronoiSimplexSolver);
-	gjk_simplex_solver->setEqualVertexThreshold(0.f);
 
 	void *world_mem;
 	if (p_create_soft_world) {
@@ -650,7 +652,6 @@ void SpaceBullet::check_ghost_overlaps() {
 	btConvexShape *area_shape;
 	btGjkPairDetector::ClosestPointInput gjk_input;
 	AreaBullet *area;
-	RigidCollisionObjectBullet *otherObject;
 	int x(-1), i(-1), y(-1), z(-1), indexOverlap(-1);
 
 	/// For each areas
@@ -677,13 +678,17 @@ void SpaceBullet::check_ghost_overlaps() {
 		// For each overlapping
 		for (i = ghostOverlaps.size() - 1; 0 <= i; --i) {
 
-			if (ghostOverlaps[i]->getUserIndex() == CollisionObjectBullet::TYPE_AREA) {
-				if (!static_cast<AreaBullet *>(ghostOverlaps[i]->getUserPointer())->is_monitorable())
-					continue;
-			} else if (ghostOverlaps[i]->getUserIndex() != CollisionObjectBullet::TYPE_RIGID_BODY)
+			btCollisionObject *overlapped_bt_co = ghostOverlaps[i];
+			RigidCollisionObjectBullet *otherObject = static_cast<RigidCollisionObjectBullet *>(overlapped_bt_co->getUserPointer());
+
+			if (!area->is_transform_changed() && !otherObject->is_transform_changed())
 				continue;
 
-			otherObject = static_cast<RigidCollisionObjectBullet *>(ghostOverlaps[i]->getUserPointer());
+			if (overlapped_bt_co->getUserIndex() == CollisionObjectBullet::TYPE_AREA) {
+				if (!static_cast<AreaBullet *>(overlapped_bt_co->getUserPointer())->is_monitorable())
+					continue;
+			} else if (overlapped_bt_co->getUserIndex() != CollisionObjectBullet::TYPE_RIGID_BODY)
+				continue;
 
 			bool hasOverlap = false;