Merge pull request #22257 from AndreaCatania/fixes

Daily physics Fixes

2 files changed, 27 insertions, 22 deletions

diff --git a/modules/bullet/rigid_body_bullet.cpp b/modules/bullet/rigid_body_bullet.cpp
index f81cfe84fb..f24c8670a3 100644
--- a/modules/bullet/rigid_body_bullet.cpp
+++ b/modules/bullet/rigid_body_bullet.cpp
@@ -351,7 +351,7 @@ 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->isActive() || previousActiveState != btBody->isActive()) && force_integration_callback && isTransformChanged) {
+	if ((btBody->isKinematicObject() || btBody->isActive() || previousActiveState != btBody->isActive()) && force_integration_callback && isTransformChanged) {
 
 		if (omit_forces_integration)
 			btBody->clearForces();
@@ -774,10 +774,13 @@ Vector3 RigidBodyBullet::get_angular_velocity() const {
 
 void RigidBodyBullet::set_transform__bullet(const btTransform &p_global_transform) {
 	if (mode == PhysicsServer::BODY_MODE_KINEMATIC) {
+		if (space)
+			btBody->setLinearVelocity((p_global_transform.getOrigin() - btBody->getWorldTransform().getOrigin()) / space->get_delta_time());
 		// The kinematic use MotionState class
 		godotMotionState->moveBody(p_global_transform);
 	}
 	btBody->setWorldTransform(p_global_transform);
+	scratch();
 }
 
 const btTransform &RigidBodyBullet::get_transform__bullet() const {
diff --git a/modules/bullet/space_bullet.cpp b/modules/bullet/space_bullet.cpp
index 5b220e1039..404cb8e37b 100644
--- a/modules/bullet/space_bullet.cpp
+++ b/modules/bullet/space_bullet.cpp
@@ -786,30 +786,32 @@ void SpaceBullet::check_body_collision() {
 			if (numContacts) {
 				btManifoldPoint &pt = contactManifold->getContactPoint(0);
 #endif
-				Vector3 collisionWorldPosition;
-				Vector3 collisionLocalPosition;
-				Vector3 normalOnB;
-				float appliedImpulse = pt.m_appliedImpulse;
-				B_TO_G(pt.m_normalWorldOnB, normalOnB);
-
-				if (bodyA->can_add_collision()) {
-					B_TO_G(pt.getPositionWorldOnB(), collisionWorldPosition);
-					/// pt.m_localPointB Doesn't report the exact point in local space
-					B_TO_G(pt.getPositionWorldOnB() - contactManifold->getBody1()->getWorldTransform().getOrigin(), collisionLocalPosition);
-					bodyA->add_collision_object(bodyB, collisionWorldPosition, collisionLocalPosition, normalOnB, appliedImpulse, pt.m_index1, pt.m_index0);
-				}
-				if (bodyB->can_add_collision()) {
-					B_TO_G(pt.getPositionWorldOnA(), collisionWorldPosition);
-					/// pt.m_localPointA Doesn't report the exact point in local space
-					B_TO_G(pt.getPositionWorldOnA() - contactManifold->getBody0()->getWorldTransform().getOrigin(), collisionLocalPosition);
-					bodyB->add_collision_object(bodyA, collisionWorldPosition, collisionLocalPosition, normalOnB * -1, appliedImpulse * -1, pt.m_index0, pt.m_index1);
-				}
+				if (pt.getDistance() <= 0.0) {
+					Vector3 collisionWorldPosition;
+					Vector3 collisionLocalPosition;
+					Vector3 normalOnB;
+					float appliedImpulse = pt.m_appliedImpulse;
+					B_TO_G(pt.m_normalWorldOnB, normalOnB);
+
+					if (bodyA->can_add_collision()) {
+						B_TO_G(pt.getPositionWorldOnB(), collisionWorldPosition);
+						/// pt.m_localPointB Doesn't report the exact point in local space
+						B_TO_G(pt.getPositionWorldOnB() - contactManifold->getBody1()->getWorldTransform().getOrigin(), collisionLocalPosition);
+						bodyA->add_collision_object(bodyB, collisionWorldPosition, collisionLocalPosition, normalOnB, appliedImpulse, pt.m_index1, pt.m_index0);
+					}
+					if (bodyB->can_add_collision()) {
+						B_TO_G(pt.getPositionWorldOnA(), collisionWorldPosition);
+						/// pt.m_localPointA Doesn't report the exact point in local space
+						B_TO_G(pt.getPositionWorldOnA() - contactManifold->getBody0()->getWorldTransform().getOrigin(), collisionLocalPosition);
+						bodyB->add_collision_object(bodyA, collisionWorldPosition, collisionLocalPosition, normalOnB * -1, appliedImpulse * -1, pt.m_index0, pt.m_index1);
+					}
 
 #ifdef DEBUG_ENABLED
-				if (is_debugging_contacts()) {
-					add_debug_contact(collisionWorldPosition);
-				}
+					if (is_debugging_contacts()) {
+						add_debug_contact(collisionWorldPosition);
+					}
 #endif
+				}
 			}
 		}
 	}