Merge pull request #15490 from AndreaCatania/kin

Fixed #15417 kinematics char jumping

modules/bullet/collision_object_bullet.cpp

@@ -49,7 +49,9 @@
 CollisionObjectBullet::ShapeWrapper::~ShapeWrapper() {}
 
 void CollisionObjectBullet::ShapeWrapper::set_transform(const Transform &p_transform) {
+	G_TO_B(p_transform.get_basis().get_scale(), scale);
 	G_TO_B(p_transform, transform);
+	UNSCALE_BT_BASIS(transform);
 }
 void CollisionObjectBullet::ShapeWrapper::set_transform(const btTransform &p_transform) {
 	transform = p_transform;
@@ -235,7 +237,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) {
@@ -320,7 +322,7 @@ void RigidCollisionObjectBullet::on_shapes_changed() {
 		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);
 			}
 			compoundShape->addChildShape(shpWrapper->transform, shpWrapper->bt_shape);
 		} else {

modules/bullet/collision_object_bullet.h

@@ -72,6 +72,7 @@ public:
 		ShapeBullet *shape;
 		btCollisionShape *bt_shape;
 		btTransform transform;
+		btVector3 scale;
 		bool active;
 
 		ShapeWrapper() :
@@ -102,6 +103,7 @@ public:
 			shape = otherShape.shape;
 			bt_shape = otherShape.bt_shape;
 			transform = otherShape.transform;
+			scale = otherShape.scale;
 			active = otherShape.active;
 		}
 

modules/bullet/rigid_body_bullet.cpp

@@ -204,7 +204,7 @@ void RigidBodyBullet::KinematicUtilities::copyAllOwnerShapes() {
 
 	const CollisionObjectBullet::ShapeWrapper *shape_wrapper;
 
-	btVector3 owner_body_scale(owner->get_bt_body_scale());
+	btVector3 owner_scale(owner->get_bt_body_scale());
 
 	for (int i = shapes_count - 1; 0 <= i; --i) {
 		shape_wrapper = &shapes_wrappers[i];
@@ -213,14 +213,14 @@ void RigidBodyBullet::KinematicUtilities::copyAllOwnerShapes() {
 		}
 
 		shapes[i].transform = shape_wrapper->transform;
-		shapes[i].transform.getOrigin() *= owner_body_scale;
+		shapes[i].transform.getOrigin() *= owner_scale;
 		switch (shape_wrapper->shape->get_type()) {
 			case PhysicsServer::SHAPE_SPHERE:
 			case PhysicsServer::SHAPE_BOX:
 			case PhysicsServer::SHAPE_CAPSULE:
 			case PhysicsServer::SHAPE_CONVEX_POLYGON:
 			case PhysicsServer::SHAPE_RAY: {
-				shapes[i].shape = static_cast<btConvexShape *>(shape_wrapper->shape->create_bt_shape(owner_body_scale, safe_margin));
+				shapes[i].shape = static_cast<btConvexShape *>(shape_wrapper->shape->create_bt_shape(owner_scale * shape_wrapper->scale, safe_margin));
 			} break;
 			default:
 				WARN_PRINT("This shape is not supported to be kinematic!");

modules/bullet/shape_bullet.cpp

@@ -48,11 +48,6 @@ 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);
@@ -82,7 +77,7 @@ void ShapeBullet::add_owner(ShapeOwnerBullet *p_owner) {
 
 void ShapeBullet::remove_owner(ShapeOwnerBullet *p_owner, bool p_permanentlyFromThisBody) {
 	Map<ShapeOwnerBullet *, int>::Element *E = owners.find(p_owner);
-	ERR_FAIL_COND(!E);
+	if (!E) return;
 	E->get()--;
 	if (p_permanentlyFromThisBody || 0 >= E->get()) {
 		owners.erase(E);

modules/bullet/shape_bullet.h

@@ -62,7 +62,6 @@ public:
 	ShapeBullet();
 	virtual ~ShapeBullet();
 
-	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;
 

modules/bullet/space_bullet.cpp

@@ -857,31 +857,31 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
 	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;
-			} else {
+			if (!recover_from_penetration(p_body, body_safe_position, RECOVERING_MOVEMENT_SCALE, recover_initial_position)) {
 				break;
 			}
 		}
+
+		// Add recover movement in order to make it safe
+		body_safe_position.getOrigin() += recover_initial_position;
 	}
 #endif
 
-	btVector3 recovered_motion;
-	G_TO_B(p_motion, recovered_motion);
-	const int shape_count(p_body->get_shape_count());
+	btVector3 motion;
+	G_TO_B(p_motion, motion);
 
 	{ /// phase two - sweep test, from a secure position without margin
 
+		const int shape_count(p_body->get_shape_count());
+
 #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) {
@@ -898,7 +898,7 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
 			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;
+			shape_world_to.getOrigin() += motion;
 
 			GodotKinClosestConvexResultCallback btResult(shape_world_from.getOrigin(), shape_world_to.getOrigin(), p_body, IGNORE_AREAS_TRUE);
 			btResult.m_collisionFilterGroup = p_body->get_collision_layer();
@@ -909,27 +909,30 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
 			if (btResult.hasHit()) {
 				/// Since for each sweep test I fix the motion of new shapes in base the recover result,
 				/// if another shape will hit something it means that has a deepest penetration respect the previous shape
-				recovered_motion *= btResult.m_closestHitFraction;
+				motion *= btResult.m_closestHitFraction;
 			}
 		}
+
+		body_safe_position.getOrigin() += motion;
 	}
 
 	bool has_penetration = false;
 
 	{ /// Phase three - Recover + contact test with margin
 
+		btVector3 delta_recover_movement(0, 0, 0);
 		RecoverResult r_recover_result;
 		bool l_has_penetration;
 		real_t l_penetration_distance = 1e20;
 
 		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);
+			l_has_penetration = recover_from_penetration(p_body, body_safe_position, RECOVERING_MOVEMENT_SCALE, delta_recover_movement, &r_recover_result);
 
 			if (r_result) {
 #if PERFORM_INITIAL_UNSTACK
-				B_TO_G(recovered_motion + recover_initial_position, r_result->motion);
+				B_TO_G(motion + delta_recover_movement + recover_initial_position, r_result->motion);
 #else
-				B_TO_G(recovered_motion, r_result->motion);
+				B_TO_G(motion + delta_recover_movement, r_result->motion);
 #endif
 				if (l_has_penetration) {
 					has_penetration = true;
@@ -942,7 +945,8 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
 					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(motion, r_result->remainder); // is the remaining movements
+					r_result->remainder = p_motion - r_result->remainder;
 					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
@@ -961,17 +965,14 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
 					//}
 
 #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(motion, sup_line2);
+					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();
 				}
@@ -1019,7 +1020,7 @@ public:
 	}
 };
 
-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) {
+bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTransform &p_body_position, btScalar p_recover_movement_scale, btVector3 &r_delta_recover_movement, RecoverResult *r_recover_result) {
 
 	RecoverPenetrationBroadPhaseCallback recover_broad_result(p_body->get_bt_collision_object(), p_body->get_collision_layer(), p_body->get_collision_mask());
 
@@ -1043,7 +1044,7 @@ bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTran
 
 		body_shape_position = p_body_position * kin_shape.transform;
 		body_shape_position_recovered = body_shape_position;
-		body_shape_position_recovered.getOrigin() += r_recover_position;
+		body_shape_position_recovered.getOrigin() += r_delta_recover_movement;
 
 		kin_shape.shape->getAabb(body_shape_position_recovered, minAabb, maxAabb);
 		dynamicsWorld->getBroadphase()->aabbTest(minAabb, maxAabb, recover_broad_result);
@@ -1060,24 +1061,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_recovered, otherObject->getWorldTransform() * cs->getChildTransform(x), p_recover_movement_scale, 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, otherObject->getWorldTransform() * cs->getChildTransform(x), p_recover_movement_scale, r_delta_recover_movement, 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_recovered, otherObject->getWorldTransform() * cs->getChildTransform(x), p_recover_movement_scale, 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, otherObject->getWorldTransform() * cs->getChildTransform(x), p_recover_movement_scale, r_delta_recover_movement, 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_recovered, otherObject->getWorldTransform(), p_recover_movement_scale, 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, otherObject->getWorldTransform(), p_recover_movement_scale, r_delta_recover_movement, 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_recovered, otherObject->getWorldTransform(), p_recover_movement_scale, 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, otherObject->getWorldTransform(), p_recover_movement_scale, r_delta_recover_movement, r_recover_result)) {
 
 					penetration = true;
 				}
@@ -1085,26 +1086,15 @@ 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, btScalar p_recover_movement_scale, 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_delta_recover_movement, RecoverResult *r_recover_result) {
 
 	// Initialize GJK input
 	btGjkPairDetector::ClosestPointInput gjk_input;
 	gjk_input.m_transformA = p_transformA;
+	gjk_input.m_transformA.getOrigin() += r_delta_recover_movement;
 	gjk_input.m_transformB = p_transformB;
 
 	// Perform GJK test
@@ -1113,7 +1103,7 @@ 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 * p_recover_movement_scale);
+		r_delta_recover_movement += result.m_normalOnBInWorld * (result.m_distance * -1 * p_recover_movement_scale);
 
 		if (r_recover_result) {
 			if (result.m_distance < r_recover_result->penetration_distance) {
@@ -1130,11 +1120,14 @@ bool SpaceBullet::RFP_convex_convex_test(const btConvexShape *p_shapeA, const bt
 	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, btScalar p_recover_movement_scale, 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_delta_recover_movement, RecoverResult *r_recover_result) {
 
 	/// Contact test
 
-	btCollisionObjectWrapper obA(NULL, p_shapeA, p_objectA, p_transformA, -1, p_shapeId_A);
+	btTransform tA(p_transformA);
+	tA.getOrigin() += r_delta_recover_movement;
+
+	btCollisionObjectWrapper obA(NULL, p_shapeA, p_objectA, tA, -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);
@@ -1147,7 +1140,7 @@ 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 * p_recover_movement_scale);
+			r_delta_recover_movement += contactPointResult.m_pointNormalWorld * (contactPointResult.m_penetration_distance * -1 * p_recover_movement_scale);
 
 			if (r_recover_result) {
 				if (contactPointResult.m_penetration_distance < r_recover_result->penetration_distance) {

modules/bullet/space_bullet.h

@@ -191,15 +191,20 @@ private:
 
 		RecoverResult() :
 				hasPenetration(false),
-				penetration_distance(1e20) {}
+				normal(0, 0, 0),
+				pointWorld(0, 0, 0),
+				penetration_distance(1e20),
+				other_compound_shape_index(0),
+				other_collision_object(NULL),
+				local_shape_most_recovered(0) {}
 	};
 
-	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);
+	bool recover_from_penetration(RigidBodyBullet *p_body, const btTransform &p_from, btScalar p_recover_movement_scale, btVector3 &r_delta_recover_movement, 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, btScalar p_movement_scale, btVector3 &r_recover_position, RecoverResult *r_recover_result = NULL);
+	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_delta_recover_movement, 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, btScalar p_movement_scale, btVector3 &r_recover_position, RecoverResult *r_recover_result = NULL);
+	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_delta_recover_movement, RecoverResult *r_recover_result = NULL);
 };
 #endif