Fixed kinematic movement stuck, Changed how shape scale works, Optimized

9 files changed, 223 insertions, 174 deletions

diff --git a/modules/bullet/bullet_types_converter.cpp b/modules/bullet/bullet_types_converter.cpp
index 5010197a78..f71a2aae65 100644
--- a/modules/bullet/bullet_types_converter.cpp
+++ b/modules/bullet/bullet_types_converter.cpp
@@ -92,3 +92,14 @@ void G_TO_B(Transform const &inVal, btTransform &outVal) {
 	G_TO_B(inVal.basis, outVal.getBasis());
 	G_TO_B(inVal.origin, outVal.getOrigin());
 }
+
+void UNSCALE_BT_BASIS(btTransform &scaledBasis) {
+	btMatrix3x3 &m(scaledBasis.getBasis());
+	btVector3 column0(m[0][0], m[1][0], m[2][0]);
+	btVector3 column1(m[0][1], m[1][1], m[2][1]);
+	btVector3 column2(m[0][2], m[1][2], m[2][2]);
+	column0.normalize();
+	column1.normalize();
+	column2.normalize();
+	m.setValue(column0[0], column1[0], column2[0], column0[1], column1[1], column2[1], column0[2], column1[2], column2[2]);
+}
diff --git a/modules/bullet/bullet_types_converter.h b/modules/bullet/bullet_types_converter.h
index ed6a349382..b7fce496f9 100644
--- a/modules/bullet/bullet_types_converter.h
+++ b/modules/bullet/bullet_types_converter.h
@@ -54,4 +54,5 @@ extern void G_TO_B(Basis const &inVal, btMatrix3x3 &outVal);
 extern void INVERT_G_TO_B(Basis const &inVal, btMatrix3x3 &outVal);
 extern void G_TO_B(Transform const &inVal, btTransform &outVal);
 
+extern void UNSCALE_BT_BASIS(btTransform &scaledBasis);
 #endif
diff --git a/modules/bullet/collision_object_bullet.cpp b/modules/bullet/collision_object_bullet.cpp
index 88d4108f82..55fcc7ceac 100644
--- a/modules/bullet/collision_object_bullet.cpp
+++ b/modules/bullet/collision_object_bullet.cpp
@@ -57,7 +57,8 @@ 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
@@ -88,6 +89,7 @@ btVector3 CollisionObjectBullet::get_bt_body_scale() const {
 }
 
 void CollisionObjectBullet::on_body_scale_changed() {
+	force_shape_reset = true;
 }
 
 void CollisionObjectBullet::destroyBulletCollisionObject() {
@@ -289,15 +291,27 @@ 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
+
+	for (i = 0; i < shapes_size; ++i) {
 		shpWrapper = &shapes[i];
 		if (shpWrapper->active) {
 			if (!shpWrapper->bt_shape) {
diff --git a/modules/bullet/collision_object_bullet.h b/modules/bullet/collision_object_bullet.h
index 7d4659b64e..4004354a0f 100644
--- a/modules/bullet/collision_object_bullet.h
+++ b/modules/bullet/collision_object_bullet.h
@@ -115,6 +115,7 @@ protected:
 	bool ray_pickable;
 	btCollisionObject *bt_collision_object;
 	Vector3 body_scale;
+	bool force_shape_reset;
 	SpaceBullet *space;
 
 	VSet<RID> exceptions;
diff --git a/modules/bullet/rigid_body_bullet.cpp b/modules/bullet/rigid_body_bullet.cpp
index 669b2c3f0c..dff4af824e 100644
--- a/modules/bullet/rigid_body_bullet.cpp
+++ b/modules/bullet/rigid_body_bullet.cpp
@@ -205,41 +205,20 @@ void RigidBodyBullet::KinematicUtilities::copyAllOwnerShapes() {
 		if (!shape_wrapper->active) {
 			continue;
 		}
-		shapes[i].transform = shape_wrapper->transform;
-
-		btConvexShape *&kin_shape_ref = shapes[i].shape;
 
+		shapes[i].transform = shape_wrapper->transform;
+		shapes[i].transform.getOrigin() *= owner_body_scale;
 		switch (shape_wrapper->shape->get_type()) {
-			case PhysicsServer::SHAPE_SPHERE: {
-				SphereShapeBullet *sphere = static_cast<SphereShapeBullet *>(shape_wrapper->shape);
-				kin_shape_ref = ShapeBullet::create_shape_sphere(sphere->get_radius() * owner_body_scale[0] + safe_margin);
-				break;
-			}
-			case PhysicsServer::SHAPE_BOX: {
-				BoxShapeBullet *box = static_cast<BoxShapeBullet *>(shape_wrapper->shape);
-				kin_shape_ref = ShapeBullet::create_shape_box((box->get_half_extents() * owner_body_scale) + btVector3(safe_margin, safe_margin, safe_margin));
-				break;
-			}
-			case PhysicsServer::SHAPE_CAPSULE: {
-				CapsuleShapeBullet *capsule = static_cast<CapsuleShapeBullet *>(shape_wrapper->shape);
-
-				kin_shape_ref = ShapeBullet::create_shape_capsule(capsule->get_radius() * owner_body_scale[0] + safe_margin, capsule->get_height() * owner_body_scale[1] + safe_margin);
-				break;
-			}
-			case PhysicsServer::SHAPE_CONVEX_POLYGON: {
-				ConvexPolygonShapeBullet *godot_convex = static_cast<ConvexPolygonShapeBullet *>(shape_wrapper->shape);
-				kin_shape_ref = ShapeBullet::create_shape_convex(godot_convex->vertices);
-				kin_shape_ref->setLocalScaling(owner_body_scale + btVector3(safe_margin, safe_margin, safe_margin));
-				break;
-			}
+			case PhysicsServer::SHAPE_SPHERE:
+			case PhysicsServer::SHAPE_BOX:
+			case PhysicsServer::SHAPE_CAPSULE:
+			case PhysicsServer::SHAPE_CONVEX_POLYGON:
 			case PhysicsServer::SHAPE_RAY: {
-				RayShapeBullet *godot_ray = static_cast<RayShapeBullet *>(shape_wrapper->shape);
-				kin_shape_ref = ShapeBullet::create_shape_ray(godot_ray->length * owner_body_scale[1] + safe_margin);
-				break;
-			}
+				shapes[i].shape = static_cast<btConvexShape *>(shape_wrapper->shape->create_bt_shape(owner_body_scale, safe_margin));
+			} break;
 			default:
 				WARN_PRINT("This shape is not supported to be kinematic!");
-				kin_shape_ref = NULL;
+				shapes[i].shape = NULL;
 		}
 	}
 }
diff --git a/modules/bullet/shape_bullet.cpp b/modules/bullet/shape_bullet.cpp
index 572a3b4476..52a5b039e3 100644
--- a/modules/bullet/shape_bullet.cpp
+++ b/modules/bullet/shape_bullet.cpp
@@ -43,6 +43,17 @@ ShapeBullet::ShapeBullet() {}
 
 ShapeBullet::~ShapeBullet() {}
 
+btCollisionShape *ShapeBullet::create_bt_shape() {
+	btVector3 s(1, 1, 1);
+	return create_bt_shape(s);
+}
+
+btCollisionShape *ShapeBullet::create_bt_shape(const Vector3 &p_implicit_scale, real_t p_margin) {
+	btVector3 s;
+	G_TO_B(p_implicit_scale, s);
+	return create_bt_shape(s, p_margin);
+}
+
 btCollisionShape *ShapeBullet::prepare(btCollisionShape *p_btShape) const {
 	p_btShape->setUserPointer(const_cast<ShapeBullet *>(this));
 	p_btShape->setMargin(0.);
@@ -150,7 +161,7 @@ void PlaneShapeBullet::setup(const Plane &p_plane) {
 	notifyShapeChanged();
 }
 
-btCollisionShape *PlaneShapeBullet::create_bt_shape() {
+btCollisionShape *PlaneShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_margin) {
 	btVector3 btPlaneNormal;
 	G_TO_B(plane.normal, btPlaneNormal);
 	return prepare(PlaneShapeBullet::create_shape_plane(btPlaneNormal, plane.d));
@@ -178,8 +189,8 @@ void SphereShapeBullet::setup(real_t p_radius) {
 	notifyShapeChanged();
 }
 
-btCollisionShape *SphereShapeBullet::create_bt_shape() {
-	return prepare(ShapeBullet::create_shape_sphere(radius));
+btCollisionShape *SphereShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_margin) {
+	return prepare(ShapeBullet::create_shape_sphere(radius * p_implicit_scale[0] + p_margin));
 }
 
 /* Box */
@@ -205,8 +216,8 @@ void BoxShapeBullet::setup(const Vector3 &p_half_extents) {
 	notifyShapeChanged();
 }
 
-btCollisionShape *BoxShapeBullet::create_bt_shape() {
-	return prepare(ShapeBullet::create_shape_box(half_extents));
+btCollisionShape *BoxShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_margin) {
+	return prepare(ShapeBullet::create_shape_box((half_extents * p_implicit_scale) + btVector3(p_margin, p_margin, p_margin)));
 }
 
 /* Capsule */
@@ -238,8 +249,8 @@ void CapsuleShapeBullet::setup(real_t p_height, real_t p_radius) {
 	notifyShapeChanged();
 }
 
-btCollisionShape *CapsuleShapeBullet::create_bt_shape() {
-	return prepare(ShapeBullet::create_shape_capsule(radius, height));
+btCollisionShape *CapsuleShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_margin) {
+	return prepare(ShapeBullet::create_shape_capsule(radius * p_implicit_scale[0] + p_margin, height * p_implicit_scale[1] + p_margin));
 }
 
 /* Convex polygon */
@@ -280,8 +291,12 @@ void ConvexPolygonShapeBullet::setup(const Vector<Vector3> &p_vertices) {
 	notifyShapeChanged();
 }
 
-btCollisionShape *ConvexPolygonShapeBullet::create_bt_shape() {
-	return prepare(ShapeBullet::create_shape_convex(vertices));
+btCollisionShape *ConvexPolygonShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_margin) {
+	btCollisionShape *cs(ShapeBullet::create_shape_convex(vertices));
+	cs->setLocalScaling(p_implicit_scale);
+	prepare(cs);
+	cs->setMargin(p_margin);
+	return cs;
 }
 
 /* Concave polygon */
@@ -349,13 +364,15 @@ void ConcavePolygonShapeBullet::setup(PoolVector<Vector3> p_faces) {
 	notifyShapeChanged();
 }
 
-btCollisionShape *ConcavePolygonShapeBullet::create_bt_shape() {
+btCollisionShape *ConcavePolygonShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_margin) {
 	btCollisionShape *cs = ShapeBullet::create_shape_concave(meshShape);
-	if (!cs) {
+	if (!cs)
 		// This is necessary since if 0 faces the creation of concave return NULL
 		cs = ShapeBullet::create_shape_empty();
-	}
-	return prepare(cs);
+	cs->setLocalScaling(p_implicit_scale);
+	prepare(cs);
+	cs->setMargin(p_margin);
+	return cs;
 }
 
 /* Height map shape */
@@ -407,8 +424,12 @@ void HeightMapShapeBullet::setup(PoolVector<real_t> &p_heights, int p_width, int
 	notifyShapeChanged();
 }
 
-btCollisionShape *HeightMapShapeBullet::create_bt_shape() {
-	return prepare(ShapeBullet::create_shape_height_field(heights, width, depth, cell_size));
+btCollisionShape *HeightMapShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_margin) {
+	btCollisionShape *cs(ShapeBullet::create_shape_height_field(heights, width, depth, cell_size));
+	cs->setLocalScaling(p_implicit_scale);
+	prepare(cs);
+	cs->setMargin(p_margin);
+	return cs;
 }
 
 /* Ray shape */
@@ -433,6 +454,6 @@ void RayShapeBullet::setup(real_t p_length) {
 	notifyShapeChanged();
 }
 
-btCollisionShape *RayShapeBullet::create_bt_shape() {
-	return prepare(ShapeBullet::create_shape_ray(length));
+btCollisionShape *RayShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_margin) {
+	return prepare(ShapeBullet::create_shape_ray(length * p_implicit_scale[1] + p_margin));
 }
diff --git a/modules/bullet/shape_bullet.h b/modules/bullet/shape_bullet.h
index 0a56fa1709..d0249c63cc 100644
--- a/modules/bullet/shape_bullet.h
+++ b/modules/bullet/shape_bullet.h
@@ -58,7 +58,9 @@ public:
 	ShapeBullet();
 	virtual ~ShapeBullet();
 
-	virtual btCollisionShape *create_bt_shape() = 0;
+	btCollisionShape *create_bt_shape();
+	btCollisionShape *create_bt_shape(const Vector3 &p_implicit_scale, real_t p_margin = 0);
+	virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_margin = 0) = 0;
 
 	void add_owner(ShapeOwnerBullet *p_owner);
 	void remove_owner(ShapeOwnerBullet *p_owner, bool p_permanentlyFromThisBody = false);
@@ -94,7 +96,7 @@ public:
 	virtual void set_data(const Variant &p_data);
 	virtual Variant get_data() const;
 	virtual PhysicsServer::ShapeType get_type() const;
-	virtual btCollisionShape *create_bt_shape();
+	virtual btCollisionShape *create_bt_shape(const btVector3 &p_scale, real_t p_margin = 0);
 
 private:
 	void setup(const Plane &p_plane);
@@ -111,7 +113,7 @@ public:
 	virtual void set_data(const Variant &p_data);
 	virtual Variant get_data() const;
 	virtual PhysicsServer::ShapeType get_type() const;
-	virtual btCollisionShape *create_bt_shape();
+	virtual btCollisionShape *create_bt_shape(const btVector3 &p_scale, real_t p_margin = 0);
 
 private:
 	void setup(real_t p_radius);
@@ -128,7 +130,7 @@ public:
 	virtual void set_data(const Variant &p_data);
 	virtual Variant get_data() const;
 	virtual PhysicsServer::ShapeType get_type() const;
-	virtual btCollisionShape *create_bt_shape();
+	virtual btCollisionShape *create_bt_shape(const btVector3 &p_scale, real_t p_margin = 0);
 
 private:
 	void setup(const Vector3 &p_half_extents);
@@ -147,7 +149,7 @@ public:
 	virtual void set_data(const Variant &p_data);
 	virtual Variant get_data() const;
 	virtual PhysicsServer::ShapeType get_type() const;
-	virtual btCollisionShape *create_bt_shape();
+	virtual btCollisionShape *create_bt_shape(const btVector3 &p_scale, real_t p_margin = 0);
 
 private:
 	void setup(real_t p_height, real_t p_radius);
@@ -164,7 +166,7 @@ public:
 	void get_vertices(Vector<Vector3> &out_vertices);
 	virtual Variant get_data() const;
 	virtual PhysicsServer::ShapeType get_type() const;
-	virtual btCollisionShape *create_bt_shape();
+	virtual btCollisionShape *create_bt_shape(const btVector3 &p_scale, real_t p_margin = 0);
 
 private:
 	void setup(const Vector<Vector3> &p_vertices);
@@ -182,7 +184,7 @@ public:
 	virtual void set_data(const Variant &p_data);
 	virtual Variant get_data() const;
 	virtual PhysicsServer::ShapeType get_type() const;
-	virtual btCollisionShape *create_bt_shape();
+	virtual btCollisionShape *create_bt_shape(const btVector3 &p_scale, real_t p_margin = 0);
 
 private:
 	void setup(PoolVector<Vector3> p_faces);
@@ -201,7 +203,7 @@ public:
 	virtual void set_data(const Variant &p_data);
 	virtual Variant get_data() const;
 	virtual PhysicsServer::ShapeType get_type() const;
-	virtual btCollisionShape *create_bt_shape();
+	virtual btCollisionShape *create_bt_shape(const btVector3 &p_scale, real_t p_margin = 0);
 
 private:
 	void setup(PoolVector<real_t> &p_heights, int p_width, int p_depth, real_t p_cell_size);
@@ -217,7 +219,7 @@ public:
 	virtual void set_data(const Variant &p_data);
 	virtual Variant get_data() const;
 	virtual PhysicsServer::ShapeType get_type() const;
-	virtual btCollisionShape *create_bt_shape();
+	virtual btCollisionShape *create_bt_shape(const btVector3 &p_scale, real_t p_margin = 0);
 
 private:
 	void setup(real_t p_length);
diff --git a/modules/bullet/space_bullet.cpp b/modules/bullet/space_bullet.cpp
index 3ce4b294db..e285bbeaa6 100644
--- a/modules/bullet/space_bullet.cpp
+++ b/modules/bullet/space_bullet.cpp
@@ -116,7 +116,7 @@ int BulletPhysicsDirectSpaceState::intersect_shape(const RID &p_shape, const Tra
 
 	ShapeBullet *shape = space->get_physics_server()->get_shape_owner()->get(p_shape);
 
-	btCollisionShape *btShape = shape->create_bt_shape();
+	btCollisionShape *btShape = shape->create_bt_shape(p_xform.basis.get_scale(), p_margin);
 	if (!btShape->isConvex()) {
 		bulletdelete(btShape);
 		ERR_PRINTS("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type()));
@@ -124,12 +124,9 @@ int BulletPhysicsDirectSpaceState::intersect_shape(const RID &p_shape, const Tra
 	}
 	btConvexShape *btConvex = static_cast<btConvexShape *>(btShape);
 
-	btVector3 scale_with_margin;
-	G_TO_B(p_xform.basis.get_scale(), scale_with_margin);
-	btConvex->setLocalScaling(scale_with_margin);
-
 	btTransform bt_xform;
 	G_TO_B(p_xform, bt_xform);
+	UNSCALE_BT_BASIS(bt_xform);
 
 	btCollisionObject collision_object;
 	collision_object.setCollisionShape(btConvex);
@@ -138,7 +135,7 @@ int BulletPhysicsDirectSpaceState::intersect_shape(const RID &p_shape, const Tra
 	GodotAllContactResultCallback btQuery(&collision_object, p_results, p_result_max, &p_exclude);
 	btQuery.m_collisionFilterGroup = 0;
 	btQuery.m_collisionFilterMask = p_collision_mask;
-	btQuery.m_closestDistanceThreshold = p_margin;
+	btQuery.m_closestDistanceThreshold = 0;
 	space->dynamicsWorld->contactTest(&collision_object, btQuery);
 
 	bulletdelete(btConvex);
@@ -149,7 +146,7 @@ int BulletPhysicsDirectSpaceState::intersect_shape(const RID &p_shape, const Tra
 bool BulletPhysicsDirectSpaceState::cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, float p_margin, float &p_closest_safe, float &p_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_collision_mask, ShapeRestInfo *r_info) {
 	ShapeBullet *shape = space->get_physics_server()->get_shape_owner()->get(p_shape);
 
-	btCollisionShape *btShape = shape->create_bt_shape();
+	btCollisionShape *btShape = shape->create_bt_shape(p_xform.basis.get_scale(), p_margin);
 	if (!btShape->isConvex()) {
 		bulletdelete(btShape);
 		ERR_PRINTS("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type()));
@@ -160,12 +157,9 @@ bool BulletPhysicsDirectSpaceState::cast_motion(const RID &p_shape, const Transf
 	btVector3 bt_motion;
 	G_TO_B(p_motion, bt_motion);
 
-	btVector3 scale_with_margin;
-	G_TO_B(p_xform.basis.get_scale() + Vector3(p_margin, p_margin, p_margin), scale_with_margin);
-	bt_convex_shape->setLocalScaling(scale_with_margin);
-
 	btTransform bt_xform_from;
 	G_TO_B(p_xform, bt_xform_from);
+	UNSCALE_BT_BASIS(bt_xform_from);
 
 	btTransform bt_xform_to(bt_xform_from);
 	bt_xform_to.getOrigin() += bt_motion;
@@ -202,7 +196,7 @@ bool BulletPhysicsDirectSpaceState::collide_shape(RID p_shape, const Transform &
 
 	ShapeBullet *shape = space->get_physics_server()->get_shape_owner()->get(p_shape);
 
-	btCollisionShape *btShape = shape->create_bt_shape();
+	btCollisionShape *btShape = shape->create_bt_shape(p_shape_xform.basis.get_scale(), p_margin);
 	if (!btShape->isConvex()) {
 		bulletdelete(btShape);
 		ERR_PRINTS("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type()));
@@ -210,12 +204,9 @@ bool BulletPhysicsDirectSpaceState::collide_shape(RID p_shape, const Transform &
 	}
 	btConvexShape *btConvex = static_cast<btConvexShape *>(btShape);
 
-	btVector3 scale_with_margin;
-	G_TO_B(p_shape_xform.basis.get_scale(), scale_with_margin);
-	btConvex->setLocalScaling(scale_with_margin);
-
 	btTransform bt_xform;
 	G_TO_B(p_shape_xform, bt_xform);
+	UNSCALE_BT_BASIS(bt_xform);
 
 	btCollisionObject collision_object;
 	collision_object.setCollisionShape(btConvex);
@@ -224,7 +215,7 @@ bool BulletPhysicsDirectSpaceState::collide_shape(RID p_shape, const Transform &
 	GodotContactPairContactResultCallback btQuery(&collision_object, r_results, p_result_max, &p_exclude);
 	btQuery.m_collisionFilterGroup = 0;
 	btQuery.m_collisionFilterMask = p_collision_mask;
-	btQuery.m_closestDistanceThreshold = p_margin;
+	btQuery.m_closestDistanceThreshold = 0;
 	space->dynamicsWorld->contactTest(&collision_object, btQuery);
 
 	r_result_count = btQuery.m_count;
@@ -237,7 +228,7 @@ bool BulletPhysicsDirectSpaceState::rest_info(RID p_shape, const Transform &p_sh
 
 	ShapeBullet *shape = space->get_physics_server()->get_shape_owner()->get(p_shape);
 
-	btCollisionShape *btShape = shape->create_bt_shape();
+	btCollisionShape *btShape = shape->create_bt_shape(p_shape_xform.basis.get_scale(), p_margin);
 	if (!btShape->isConvex()) {
 		bulletdelete(btShape);
 		ERR_PRINTS("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type()));
@@ -245,12 +236,9 @@ bool BulletPhysicsDirectSpaceState::rest_info(RID p_shape, const Transform &p_sh
 	}
 	btConvexShape *btConvex = static_cast<btConvexShape *>(btShape);
 
-	btVector3 scale_with_margin;
-	G_TO_B(p_shape_xform.basis.get_scale() + Vector3(p_margin, p_margin, p_margin), scale_with_margin);
-	btConvex->setLocalScaling(scale_with_margin);
-
 	btTransform bt_xform;
 	G_TO_B(p_shape_xform, bt_xform);
+	UNSCALE_BT_BASIS(bt_xform);
 
 	btCollisionObject collision_object;
 	collision_object.setCollisionShape(btConvex);
@@ -259,7 +247,7 @@ bool BulletPhysicsDirectSpaceState::rest_info(RID p_shape, const Transform &p_sh
 	GodotRestInfoContactResultCallback btQuery(&collision_object, r_info, &p_exclude);
 	btQuery.m_collisionFilterGroup = 0;
 	btQuery.m_collisionFilterMask = p_collision_mask;
-	btQuery.m_closestDistanceThreshold = p_margin;
+	btQuery.m_closestDistanceThreshold = 0;
 	space->dynamicsWorld->contactTest(&collision_object, btQuery);
 
 	bulletdelete(btConvex);
@@ -796,7 +784,9 @@ void SpaceBullet::update_gravity() {
 /// I'm leaving this here just for future tests.
 /// Debug motion and normal vector drawing
 #define debug_test_motion 0
-#define PERFORM_INITIAL_UNSTACK 1
+#define PERFORM_INITIAL_UNSTACK 0
+#define RECOVERING_MOVEMENT_SCALE 0.4
+#define RECOVERING_MOVEMENT_CYCLES 4
 
 #if debug_test_motion
 
@@ -820,6 +810,9 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
 		SceneTree::get_singleton()->get_current_scene()->add_child(motionVec);
 		SceneTree::get_singleton()->get_current_scene()->add_child(normalLine);
 
+		motionVec->set_as_toplevel(true);
+		normalLine->set_as_toplevel(true);
+
 		red_mat = Ref<SpatialMaterial>(memnew(SpatialMaterial));
 		red_mat->set_flag(SpatialMaterial::FLAG_UNSHADED, true);
 		red_mat->set_line_width(20.0);
@@ -850,20 +843,24 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
 	//    }
 	//}
 
-	btVector3 recover_initial_position(0, 0, 0);
-
 	btTransform body_safe_position;
 	G_TO_B(p_from, body_safe_position);
+	UNSCALE_BT_BASIS(body_safe_position);
 
-	{ /// Phase one - multi shapes depenetration using margin
 #if PERFORM_INITIAL_UNSTACK
-		if (recover_from_penetration(p_body, body_safe_position, recover_initial_position)) {
+	btVector3 recover_initial_position(0, 0, 0);
+	{ /// Phase one - multi shapes depenetration using margin
+		for (int t(RECOVERING_MOVEMENT_CYCLES); 0 < t; --t) {
+			if (recover_from_penetration(p_body, body_safe_position, RECOVERING_MOVEMENT_SCALE, recover_initial_position)) {
 
-			// Add recover position to "From" and "To" transforms
-			body_safe_position.getOrigin() += recover_initial_position;
+				// Add recover position to "From" and "To" transforms
+				body_safe_position.getOrigin() += recover_initial_position;
+			} else {
+				break;
+			}
 		}
-#endif
 	}
+#endif
 
 	btVector3 recovered_motion;
 	G_TO_B(p_motion, recovered_motion);
@@ -872,13 +869,13 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
 	{ /// phase two - sweep test, from a secure position without margin
 
 #if debug_test_motion
-		Vector3 sup_line;
-		B_TO_G(body_safe_position.getOrigin(), sup_line);
-		motionVec->clear();
-		motionVec->begin(Mesh::PRIMITIVE_LINES, NULL);
-		motionVec->add_vertex(sup_line);
-		motionVec->add_vertex(sup_line + p_motion * 10);
-		motionVec->end();
+//Vector3 sup_line;
+//B_TO_G(body_safe_position.getOrigin(), sup_line);
+//motionVec->clear();
+//motionVec->begin(Mesh::PRIMITIVE_LINES, NULL);
+//motionVec->add_vertex(sup_line);
+//motionVec->add_vertex(sup_line + p_motion * 10);
+//motionVec->end();
 #endif
 
 		for (int shIndex = 0; shIndex < shape_count; ++shIndex) {
@@ -892,11 +889,7 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
 			}
 			btConvexShape *convex_shape_test(static_cast<btConvexShape *>(p_body->get_bt_shape(shIndex)));
 
-			btTransform shape_world_from;
-			G_TO_B(p_body->get_shape_transform(shIndex), shape_world_from);
-
-			// Add local shape transform
-			shape_world_from = body_safe_position * shape_world_from;
+			btTransform shape_world_from = body_safe_position * p_body->get_kinematic_utilities()->shapes[shIndex].transform;
 
 			btTransform shape_world_to(shape_world_from);
 			shape_world_to.getOrigin() += recovered_motion;
@@ -915,59 +908,75 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
 		}
 	}
 
-	bool hasPenetration = false;
+	bool has_penetration = false;
 
 	{ /// Phase three - Recover + contact test with margin
 
 		RecoverResult r_recover_result;
+		bool l_has_penetration;
+		real_t l_penetration_distance = 1e20;
 
-		hasPenetration = recover_from_penetration(p_body, body_safe_position, recovered_motion, &r_recover_result);
+		for (int t(RECOVERING_MOVEMENT_CYCLES); 0 < t; --t) {
+			l_has_penetration = recover_from_penetration(p_body, body_safe_position, RECOVERING_MOVEMENT_SCALE, recovered_motion, &r_recover_result);
 
-		if (r_result) {
-
-			B_TO_G(recovered_motion + recover_initial_position, r_result->motion);
-
-			if (hasPenetration) {
-				const btRigidBody *btRigid = static_cast<const btRigidBody *>(r_recover_result.other_collision_object);
-				CollisionObjectBullet *collisionObject = static_cast<CollisionObjectBullet *>(btRigid->getUserPointer());
-
-				r_result->remainder = p_motion - r_result->motion; // is the remaining movements
-				B_TO_G(r_recover_result.pointWorld, r_result->collision_point);
-				B_TO_G(r_recover_result.pointNormalWorld, r_result->collision_normal);
-				B_TO_G(btRigid->getVelocityInLocalPoint(r_recover_result.pointWorld - btRigid->getWorldTransform().getOrigin()), r_result->collider_velocity); // It calculates velocity at point and assign it using special function Bullet_to_Godot
-				r_result->collider = collisionObject->get_self();
-				r_result->collider_id = collisionObject->get_instance_id();
-				r_result->collider_shape = r_recover_result.other_compound_shape_index;
-				r_result->collision_local_shape = r_recover_result.local_shape_most_recovered;
+			if (r_result) {
+#if PERFORM_INITIAL_UNSTACK
+				B_TO_G(recovered_motion + recover_initial_position, r_result->motion);
+#else
+				B_TO_G(recovered_motion, r_result->motion);
+#endif
+				if (l_has_penetration) {
+					has_penetration = true;
+					if (l_penetration_distance <= r_recover_result.penetration_distance) {
+						continue;
+					}
 
-				//{ /// Add manifold point to manage collisions
-				//    btPersistentManifold* manifold = dynamicsWorld->getDispatcher()->getNewManifold(p_body->getBtBody(), btRigid);
-				//    btManifoldPoint manifoldPoint(result_callabck.m_pointWorld, result_callabck.m_pointWorld, result_callabck.m_pointNormalWorld, result_callabck.m_penetration_distance);
-				//    manifoldPoint.m_index0 = r_result->collision_local_shape;
-				//    manifoldPoint.m_index1 = r_result->collider_shape;
-				//    manifold->addManifoldPoint(manifoldPoint);
-				//    p_body->get_kinematic_utilities()->m_generatedManifold.push_back(manifold);
-				//}
+					l_penetration_distance = r_recover_result.penetration_distance;
+
+					const btRigidBody *btRigid = static_cast<const btRigidBody *>(r_recover_result.other_collision_object);
+					CollisionObjectBullet *collisionObject = static_cast<CollisionObjectBullet *>(btRigid->getUserPointer());
+
+					r_result->remainder = p_motion - r_result->motion; // is the remaining movements
+					B_TO_G(r_recover_result.pointWorld, r_result->collision_point);
+					B_TO_G(r_recover_result.normal, r_result->collision_normal);
+					B_TO_G(btRigid->getVelocityInLocalPoint(r_recover_result.pointWorld - btRigid->getWorldTransform().getOrigin()), r_result->collider_velocity); // It calculates velocity at point and assign it using special function Bullet_to_Godot
+					r_result->collider = collisionObject->get_self();
+					r_result->collider_id = collisionObject->get_instance_id();
+					r_result->collider_shape = r_recover_result.other_compound_shape_index;
+					r_result->collision_local_shape = r_recover_result.local_shape_most_recovered;
+
+					//{ /// Add manifold point to manage collisions
+					//    btPersistentManifold* manifold = dynamicsWorld->getDispatcher()->getNewManifold(p_body->getBtBody(), btRigid);
+					//    btManifoldPoint manifoldPoint(result_callabck.m_pointWorld, result_callabck.m_pointWorld, result_callabck.m_pointNormalWorld, result_callabck.m_penetration_distance);
+					//    manifoldPoint.m_index0 = r_result->collision_local_shape;
+					//    manifoldPoint.m_index1 = r_result->collider_shape;
+					//    manifold->addManifoldPoint(manifoldPoint);
+					//    p_body->get_kinematic_utilities()->m_generatedManifold.push_back(manifold);
+					//}
 
 #if debug_test_motion
-				Vector3 sup_line2;
-				B_TO_G(recovered_motion, sup_line2);
-				//Vector3 sup_pos;
-				//B_TO_G( pt.getPositionWorldOnB(), sup_pos);
-				normalLine->clear();
-				normalLine->begin(Mesh::PRIMITIVE_LINES, NULL);
-				normalLine->add_vertex(r_result->collision_point);
-				normalLine->add_vertex(r_result->collision_point + r_result->collision_normal * 10);
-				normalLine->end();
+//Vector3 sup_line2;
+//B_TO_G(recovered_motion, sup_line2);
+////Vector3 sup_pos;
+////B_TO_G( pt.getPositionWorldOnB(), sup_pos);
+//normalLine->clear();
+//normalLine->begin(Mesh::PRIMITIVE_LINES, NULL);
+//normalLine->add_vertex(r_result->collision_point);
+//normalLine->add_vertex(r_result->collision_point + r_result->collision_normal * 10);
+//normalLine->end();
 #endif
 
+				} else {
+					r_result->remainder = Vector3();
+				}
 			} else {
-				r_result->remainder = Vector3();
+				if (!l_has_penetration)
+					break;
 			}
 		}
 	}
 
-	return hasPenetration;
+	return has_penetration;
 }
 
 struct RecoverPenetrationBroadPhaseCallback : public btBroadphaseAabbCallback {
@@ -1004,7 +1013,7 @@ public:
 	}
 };
 
-bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTransform &p_body_position, btVector3 &r_recover_position, RecoverResult *r_recover_result) {
+bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTransform &p_body_position, btScalar p_recover_movement_scale, btVector3 &r_recover_position, RecoverResult *r_recover_result) {
 
 	RecoverPenetrationBroadPhaseCallback recover_broad_result(p_body->get_bt_collision_object(), p_body->get_collision_layer(), p_body->get_collision_mask());
 
@@ -1045,24 +1054,24 @@ bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTran
 				for (int x = cs->getNumChildShapes() - 1; 0 <= x; --x) {
 
 					if (cs->getChildShape(x)->isConvex()) {
-						if (RFP_convex_convex_test(kin_shape.shape, static_cast<const btConvexShape *>(cs->getChildShape(x)), otherObject, x, body_shape_position, otherObject->getWorldTransform() * cs->getChildTransform(x), r_recover_position, r_recover_result)) {
+						if (RFP_convex_convex_test(kin_shape.shape, static_cast<const btConvexShape *>(cs->getChildShape(x)), otherObject, x, body_shape_position_recovered, otherObject->getWorldTransform() * cs->getChildTransform(x), p_recover_movement_scale, r_recover_position, r_recover_result)) {
 
 							penetration = true;
 						}
 					} else {
-						if (RFP_convex_world_test(kin_shape.shape, cs->getChildShape(x), p_body->get_bt_collision_object(), otherObject, kinIndex, x, body_shape_position, otherObject->getWorldTransform() * cs->getChildTransform(x), r_recover_position, r_recover_result)) {
+						if (RFP_convex_world_test(kin_shape.shape, cs->getChildShape(x), p_body->get_bt_collision_object(), otherObject, kinIndex, x, body_shape_position_recovered, otherObject->getWorldTransform() * cs->getChildTransform(x), p_recover_movement_scale, r_recover_position, r_recover_result)) {
 
 							penetration = true;
 						}
 					}
 				}
 			} else if (otherObject->getCollisionShape()->isConvex()) { /// Execute GJK test against object shape
-				if (RFP_convex_convex_test(kin_shape.shape, static_cast<const btConvexShape *>(otherObject->getCollisionShape()), otherObject, 0, body_shape_position, otherObject->getWorldTransform(), r_recover_position, r_recover_result)) {
+				if (RFP_convex_convex_test(kin_shape.shape, static_cast<const btConvexShape *>(otherObject->getCollisionShape()), otherObject, 0, body_shape_position_recovered, otherObject->getWorldTransform(), p_recover_movement_scale, r_recover_position, r_recover_result)) {
 
 					penetration = true;
 				}
 			} else {
-				if (RFP_convex_world_test(kin_shape.shape, otherObject->getCollisionShape(), p_body->get_bt_collision_object(), otherObject, kinIndex, 0, body_shape_position, otherObject->getWorldTransform(), r_recover_position, r_recover_result)) {
+				if (RFP_convex_world_test(kin_shape.shape, otherObject->getCollisionShape(), p_body->get_bt_collision_object(), otherObject, kinIndex, 0, body_shape_position_recovered, otherObject->getWorldTransform(), p_recover_movement_scale, r_recover_position, r_recover_result)) {
 
 					penetration = true;
 				}
@@ -1070,15 +1079,26 @@ bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTran
 		}
 	}
 
+#if debug_test_motion
+	Vector3 pos;
+	B_TO_G(p_body_position.getOrigin(), pos);
+	Vector3 sup_line;
+	B_TO_G(sum_recover_normals, sup_line);
+	motionVec->clear();
+	motionVec->begin(Mesh::PRIMITIVE_LINES, NULL);
+	motionVec->add_vertex(pos);
+	motionVec->add_vertex(pos + (sup_line * 10));
+	motionVec->end();
+#endif
+
 	return penetration;
 }
 
-bool SpaceBullet::RFP_convex_convex_test(const btConvexShape *p_shapeA, const btConvexShape *p_shapeB, btCollisionObject *p_objectB, int p_shapeId_B, const btTransform &p_transformA, const btTransform &p_transformB, btVector3 &r_recover_position, RecoverResult *r_recover_result) {
+bool SpaceBullet::RFP_convex_convex_test(const btConvexShape *p_shapeA, const btConvexShape *p_shapeB, btCollisionObject *p_objectB, int p_shapeId_B, const btTransform &p_transformA, const btTransform &p_transformB, btScalar p_recover_movement_scale, btVector3 &r_recover_position, RecoverResult *r_recover_result) {
 
 	// Initialize GJK input
 	btGjkPairDetector::ClosestPointInput gjk_input;
 	gjk_input.m_transformA = p_transformA;
-	gjk_input.m_transformA.getOrigin() += r_recover_position;
 	gjk_input.m_transformB = p_transformB;
 
 	// Perform GJK test
@@ -1087,30 +1107,28 @@ bool SpaceBullet::RFP_convex_convex_test(const btConvexShape *p_shapeA, const bt
 	gjk_pair_detector.getClosestPoints(gjk_input, result, 0);
 	if (0 > result.m_distance) {
 		// Has penetration
-		r_recover_position += result.m_normalOnBInWorld * (result.m_distance * -1);
+		r_recover_position += result.m_normalOnBInWorld * (result.m_distance * -1 * p_recover_movement_scale);
 
 		if (r_recover_result) {
-
-			r_recover_result->hasPenetration = true;
-			r_recover_result->other_collision_object = p_objectB;
-			r_recover_result->other_compound_shape_index = p_shapeId_B;
-			r_recover_result->penetration_distance = result.m_distance;
-			r_recover_result->pointNormalWorld = result.m_normalOnBInWorld;
-			r_recover_result->pointWorld = result.m_pointInWorld;
+			if (result.m_distance < r_recover_result->penetration_distance) {
+				r_recover_result->hasPenetration = true;
+				r_recover_result->other_collision_object = p_objectB;
+				r_recover_result->other_compound_shape_index = p_shapeId_B;
+				r_recover_result->penetration_distance = result.m_distance;
+				r_recover_result->pointWorld = result.m_pointInWorld;
+				r_recover_result->normal = result.m_normalOnBInWorld;
+			}
 		}
 		return true;
 	}
 	return false;
 }
 
-bool SpaceBullet::RFP_convex_world_test(const btConvexShape *p_shapeA, const btCollisionShape *p_shapeB, btCollisionObject *p_objectA, btCollisionObject *p_objectB, int p_shapeId_A, int p_shapeId_B, const btTransform &p_transformA, const btTransform &p_transformB, btVector3 &r_recover_position, RecoverResult *r_recover_result) {
+bool SpaceBullet::RFP_convex_world_test(const btConvexShape *p_shapeA, const btCollisionShape *p_shapeB, btCollisionObject *p_objectA, btCollisionObject *p_objectB, int p_shapeId_A, int p_shapeId_B, const btTransform &p_transformA, const btTransform &p_transformB, btScalar p_recover_movement_scale, btVector3 &r_recover_position, RecoverResult *r_recover_result) {
 
 	/// Contact test
 
-	btTransform p_recovered_transformA(p_transformA);
-	p_recovered_transformA.getOrigin() += r_recover_position;
-
-	btCollisionObjectWrapper obA(NULL, p_shapeA, p_objectA, p_recovered_transformA, -1, p_shapeId_A);
+	btCollisionObjectWrapper obA(NULL, p_shapeA, p_objectA, p_transformA, -1, p_shapeId_A);
 	btCollisionObjectWrapper obB(NULL, p_shapeB, p_objectB, p_transformB, -1, p_shapeId_B);
 
 	btCollisionAlgorithm *algorithm = dispatcher->findAlgorithm(&obA, &obB, NULL, BT_CLOSEST_POINT_ALGORITHMS);
@@ -1123,16 +1141,17 @@ bool SpaceBullet::RFP_convex_world_test(const btConvexShape *p_shapeA, const btC
 		dispatcher->freeCollisionAlgorithm(algorithm);
 
 		if (contactPointResult.hasHit()) {
-			r_recover_position += contactPointResult.m_pointNormalWorld * (contactPointResult.m_penetration_distance * -1);
+			r_recover_position += contactPointResult.m_pointNormalWorld * (contactPointResult.m_penetration_distance * -1 * p_recover_movement_scale);
 
 			if (r_recover_result) {
-
-				r_recover_result->hasPenetration = true;
-				r_recover_result->other_collision_object = p_objectB;
-				r_recover_result->other_compound_shape_index = p_shapeId_B;
-				r_recover_result->penetration_distance = contactPointResult.m_penetration_distance;
-				r_recover_result->pointNormalWorld = contactPointResult.m_pointNormalWorld;
-				r_recover_result->pointWorld = contactPointResult.m_pointWorld;
+				if (contactPointResult.m_penetration_distance < r_recover_result->penetration_distance) {
+					r_recover_result->hasPenetration = true;
+					r_recover_result->other_collision_object = p_objectB;
+					r_recover_result->other_compound_shape_index = p_shapeId_B;
+					r_recover_result->penetration_distance = contactPointResult.m_penetration_distance;
+					r_recover_result->pointWorld = contactPointResult.m_pointWorld;
+					r_recover_result->normal = contactPointResult.m_pointNormalWorld;
+				}
 			}
 			return true;
 		}
diff --git a/modules/bullet/space_bullet.h b/modules/bullet/space_bullet.h
index e5267c01a9..5bc922f43c 100644
--- a/modules/bullet/space_bullet.h
+++ b/modules/bullet/space_bullet.h
@@ -178,23 +178,24 @@ private:
 
 	struct RecoverResult {
 		bool hasPenetration;
-		btVector3 pointNormalWorld;
+		btVector3 normal;
 		btVector3 pointWorld;
-		btScalar penetration_distance; // Negative is penetration
+		btScalar penetration_distance; // Negative mean penetration
 		int other_compound_shape_index;
 		const btCollisionObject *other_collision_object;
 		int local_shape_most_recovered;
 
 		RecoverResult() :
-				hasPenetration(false) {}
+				hasPenetration(false),
+				penetration_distance(1e20) {}
 	};
 
-	bool recover_from_penetration(RigidBodyBullet *p_body, const btTransform &p_from, btVector3 &r_recover_position, RecoverResult *r_recover_result = NULL);
+	bool recover_from_penetration(RigidBodyBullet *p_body, const btTransform &p_from, btScalar p_recover_movement_scale, btVector3 &r_recover_position, RecoverResult *r_recover_result = NULL);
 	/// This is an API that recover a kinematic object from penetration
 	/// This allow only Convex Convex test and it always use GJK algorithm, With this API we don't benefit of Bullet special accelerated functions
-	bool RFP_convex_convex_test(const btConvexShape *p_shapeA, const btConvexShape *p_shapeB, btCollisionObject *p_objectB, int p_shapeId_B, const btTransform &p_transformA, const btTransform &p_transformB, btVector3 &r_recover_position, RecoverResult *r_recover_result);
+	bool RFP_convex_convex_test(const btConvexShape *p_shapeA, const btConvexShape *p_shapeB, btCollisionObject *p_objectB, int p_shapeId_B, const btTransform &p_transformA, const btTransform &p_transformB, btScalar p_movement_scale, btVector3 &r_recover_position, RecoverResult *r_recover_result = NULL);
 	/// This is an API that recover a kinematic object from penetration
 	/// Using this we leave Bullet to select the best algorithm, For example GJK in case we have Convex Convex, or a Bullet accelerated algorithm
-	bool RFP_convex_world_test(const btConvexShape *p_shapeA, const btCollisionShape *p_shapeB, btCollisionObject *p_objectA, btCollisionObject *p_objectB, int p_shapeId_A, int p_shapeId_B, const btTransform &p_transformA, const btTransform &p_transformB, btVector3 &r_recover_position, RecoverResult *r_recover_result);
+	bool RFP_convex_world_test(const btConvexShape *p_shapeA, const btCollisionShape *p_shapeB, btCollisionObject *p_objectA, btCollisionObject *p_objectB, int p_shapeId_A, int p_shapeId_B, const btTransform &p_transformA, const btTransform &p_transformB, btScalar p_movement_scale, btVector3 &r_recover_position, RecoverResult *r_recover_result = NULL);
 };
 #endif