1 files changed, 91 insertions, 106 deletions

diff --git a/scene/3d/physics_body_3d.cpp b/scene/3d/physics_body_3d.cpp
index 092efc55d7..0356994cdb 100644
--- a/scene/3d/physics_body_3d.cpp
+++ b/scene/3d/physics_body_3d.cpp
@@ -111,9 +111,9 @@ Ref<KinematicCollision3D> PhysicsBody3D::_move(const Vector3 &p_motion, bool p_t
 	return Ref<KinematicCollision3D>();
 }
 
-bool PhysicsBody3D::move_and_collide(const Vector3 &p_motion, PhysicsServer3D::MotionResult &r_result, real_t p_margin, bool p_test_only, bool p_cancel_sliding, const Set<RID> &p_exclude) {
+bool PhysicsBody3D::move_and_collide(const Vector3 &p_motion, PhysicsServer3D::MotionResult &r_result, real_t p_margin, bool p_test_only, bool p_cancel_sliding, bool p_collide_separation_ray, const Set<RID> &p_exclude) {
 	Transform3D gt = get_global_transform();
-	bool colliding = PhysicsServer3D::get_singleton()->body_test_motion(get_rid(), gt, p_motion, p_margin, &r_result, p_exclude);
+	bool colliding = PhysicsServer3D::get_singleton()->body_test_motion(get_rid(), gt, p_motion, p_margin, &r_result, p_collide_separation_ray, p_exclude);
 
 	// Restore direction of motion to be along original motion,
 	// in order to avoid sliding due to recovery,
@@ -283,18 +283,20 @@ bool StaticBody3D::is_sync_to_physics_enabled() const {
 	return sync_to_physics;
 }
 
-void StaticBody3D::_direct_state_changed(Object *p_state) {
-	PhysicsDirectBodyState3D *state = Object::cast_to<PhysicsDirectBodyState3D>(p_state);
-	ERR_FAIL_NULL_MSG(state, "Method '_direct_state_changed' must receive a valid PhysicsDirectBodyState3D object as argument");
+void StaticBody3D::_body_state_changed_callback(void *p_instance, PhysicsDirectBodyState3D *p_state) {
+	StaticBody3D *body = (StaticBody3D *)p_instance;
+	body->_body_state_changed(p_state);
+}
 
-	linear_velocity = state->get_linear_velocity();
-	angular_velocity = state->get_angular_velocity();
+void StaticBody3D::_body_state_changed(PhysicsDirectBodyState3D *p_state) {
+	linear_velocity = p_state->get_linear_velocity();
+	angular_velocity = p_state->get_angular_velocity();
 
 	if (!sync_to_physics) {
 		return;
 	}
 
-	last_valid_transform = state->get_transform();
+	last_valid_transform = p_state->get_transform();
 	set_notify_local_transform(false);
 	set_global_transform(last_valid_transform);
 	set_notify_local_transform(true);
@@ -347,7 +349,7 @@ void StaticBody3D::_notification(int p_what) {
 			// Used by sync to physics, send the new transform to the physics...
 			Transform3D new_transform = get_global_transform();
 
-			real_t delta_time = get_physics_process_delta_time();
+			double delta_time = get_physics_process_delta_time();
 			new_transform.origin += constant_linear_velocity * delta_time;
 
 			real_t ang_vel = constant_angular_velocity.length();
@@ -378,7 +380,7 @@ void StaticBody3D::_notification(int p_what) {
 
 			Transform3D new_transform = get_global_transform();
 
-			real_t delta_time = get_physics_process_delta_time();
+			double delta_time = get_physics_process_delta_time();
 			new_transform.origin += constant_linear_velocity * delta_time;
 
 			real_t ang_vel = constant_angular_velocity.length();
@@ -458,13 +460,13 @@ void StaticBody3D::_update_kinematic_motion() {
 
 	bool needs_physics_process = false;
 	if (kinematic_motion) {
-		PhysicsServer3D::get_singleton()->body_set_force_integration_callback(get_rid(), callable_mp(this, &StaticBody3D::_direct_state_changed));
+		PhysicsServer3D::get_singleton()->body_set_state_sync_callback(get_rid(), this, &StaticBody3D::_body_state_changed_callback);
 
 		if (!constant_angular_velocity.is_equal_approx(Vector3()) || !constant_linear_velocity.is_equal_approx(Vector3())) {
 			needs_physics_process = true;
 		}
 	} else {
-		PhysicsServer3D::get_singleton()->body_set_force_integration_callback(get_rid(), Callable());
+		PhysicsServer3D::get_singleton()->body_set_state_sync_callback(get_rid(), nullptr, nullptr);
 	}
 
 	set_physics_process_internal(needs_physics_process);
@@ -582,26 +584,27 @@ struct _RigidBodyInOut {
 	int local_shape = 0;
 };
 
-void RigidBody3D::_direct_state_changed(Object *p_state) {
-#ifdef DEBUG_ENABLED
-	state = Object::cast_to<PhysicsDirectBodyState3D>(p_state);
-	ERR_FAIL_NULL_MSG(state, "Method '_direct_state_changed' must receive a valid PhysicsDirectBodyState3D object as argument");
-#else
-	state = (PhysicsDirectBodyState3D *)p_state; //trust it
-#endif
+void RigidBody3D::_body_state_changed_callback(void *p_instance, PhysicsDirectBodyState3D *p_state) {
+	RigidBody3D *body = (RigidBody3D *)p_instance;
+	body->_body_state_changed(p_state);
+}
 
+void RigidBody3D::_body_state_changed(PhysicsDirectBodyState3D *p_state) {
 	set_ignore_transform_notification(true);
-	set_global_transform(state->get_transform());
-	linear_velocity = state->get_linear_velocity();
-	angular_velocity = state->get_angular_velocity();
-	inverse_inertia_tensor = state->get_inverse_inertia_tensor();
-	if (sleeping != state->is_sleeping()) {
-		sleeping = state->is_sleeping();
+	set_global_transform(p_state->get_transform());
+
+	linear_velocity = p_state->get_linear_velocity();
+	angular_velocity = p_state->get_angular_velocity();
+
+	inverse_inertia_tensor = p_state->get_inverse_inertia_tensor();
+
+	if (sleeping != p_state->is_sleeping()) {
+		sleeping = p_state->is_sleeping();
 		emit_signal(SceneStringNames::get_singleton()->sleeping_state_changed);
 	}
-	if (get_script_instance()) {
-		get_script_instance()->call("_integrate_forces", state);
-	}
+
+	GDVIRTUAL_CALL(_integrate_forces, p_state);
+
 	set_ignore_transform_notification(false);
 	_on_transform_changed();
 
@@ -617,18 +620,18 @@ void RigidBody3D::_direct_state_changed(Object *p_state) {
 			}
 		}
 
-		_RigidBodyInOut *toadd = (_RigidBodyInOut *)alloca(state->get_contact_count() * sizeof(_RigidBodyInOut));
+		_RigidBodyInOut *toadd = (_RigidBodyInOut *)alloca(p_state->get_contact_count() * sizeof(_RigidBodyInOut));
 		int toadd_count = 0; //state->get_contact_count();
 		RigidBody3D_RemoveAction *toremove = (RigidBody3D_RemoveAction *)alloca(rc * sizeof(RigidBody3D_RemoveAction));
 		int toremove_count = 0;
 
 		//put the ones to add
 
-		for (int i = 0; i < state->get_contact_count(); i++) {
-			RID rid = state->get_contact_collider(i);
-			ObjectID obj = state->get_contact_collider_id(i);
-			int local_shape = state->get_contact_local_shape(i);
-			int shape = state->get_contact_collider_shape(i);
+		for (int i = 0; i < p_state->get_contact_count(); i++) {
+			RID rid = p_state->get_contact_collider(i);
+			ObjectID obj = p_state->get_contact_collider_id(i);
+			int local_shape = p_state->get_contact_local_shape(i);
+			int shape = p_state->get_contact_collider_shape(i);
 
 			//bool found=false;
 
@@ -683,8 +686,6 @@ void RigidBody3D::_direct_state_changed(Object *p_state) {
 
 		contact_monitor->locked = false;
 	}
-
-	state = nullptr;
 }
 
 void RigidBody3D::_notification(int p_what) {
@@ -787,25 +788,15 @@ real_t RigidBody3D::get_angular_damp() const {
 }
 
 void RigidBody3D::set_axis_velocity(const Vector3 &p_axis) {
-	Vector3 v = state ? state->get_linear_velocity() : linear_velocity;
 	Vector3 axis = p_axis.normalized();
-	v -= axis * axis.dot(v);
-	v += p_axis;
-	if (state) {
-		set_linear_velocity(v);
-	} else {
-		PhysicsServer3D::get_singleton()->body_set_axis_velocity(get_rid(), p_axis);
-		linear_velocity = v;
-	}
+	linear_velocity -= axis * axis.dot(linear_velocity);
+	linear_velocity += p_axis;
+	PhysicsServer3D::get_singleton()->body_set_state(get_rid(), PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY, linear_velocity);
 }
 
 void RigidBody3D::set_linear_velocity(const Vector3 &p_velocity) {
 	linear_velocity = p_velocity;
-	if (state) {
-		state->set_linear_velocity(linear_velocity);
-	} else {
-		PhysicsServer3D::get_singleton()->body_set_state(get_rid(), PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY, linear_velocity);
-	}
+	PhysicsServer3D::get_singleton()->body_set_state(get_rid(), PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY, linear_velocity);
 }
 
 Vector3 RigidBody3D::get_linear_velocity() const {
@@ -814,11 +805,7 @@ Vector3 RigidBody3D::get_linear_velocity() const {
 
 void RigidBody3D::set_angular_velocity(const Vector3 &p_velocity) {
 	angular_velocity = p_velocity;
-	if (state) {
-		state->set_angular_velocity(angular_velocity);
-	} else {
-		PhysicsServer3D::get_singleton()->body_set_state(get_rid(), PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY, angular_velocity);
-	}
+	PhysicsServer3D::get_singleton()->body_set_state(get_rid(), PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY, angular_velocity);
 }
 
 Vector3 RigidBody3D::get_angular_velocity() const {
@@ -1022,7 +1009,7 @@ void RigidBody3D::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("get_colliding_bodies"), &RigidBody3D::get_colliding_bodies);
 
-	BIND_VMETHOD(MethodInfo("_integrate_forces", PropertyInfo(Variant::OBJECT, "state", PROPERTY_HINT_RESOURCE_TYPE, "PhysicsDirectBodyState3D")));
+	GDVIRTUAL_BIND(_integrate_forces, "state");
 
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "mode", PROPERTY_HINT_ENUM, "Dynamic,Static,DynamicLocked,Kinematic"), "set_mode", "get_mode");
 	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "mass", PROPERTY_HINT_RANGE, "0.01,65535,0.01,exp"), "set_mass", "get_mass");
@@ -1055,7 +1042,7 @@ void RigidBody3D::_bind_methods() {
 
 RigidBody3D::RigidBody3D() :
 		PhysicsBody3D(PhysicsServer3D::BODY_MODE_DYNAMIC) {
-	PhysicsServer3D::get_singleton()->body_set_force_integration_callback(get_rid(), callable_mp(this, &RigidBody3D::_direct_state_changed));
+	PhysicsServer3D::get_singleton()->body_set_state_sync_callback(get_rid(), this, &RigidBody3D::_body_state_changed_callback);
 }
 
 RigidBody3D::~RigidBody3D() {
@@ -1080,12 +1067,10 @@ void RigidBody3D::_reload_physics_characteristics() {
 #define FLOOR_ANGLE_THRESHOLD 0.01
 
 bool CharacterBody3D::move_and_slide() {
-	Vector3 body_velocity_normal = linear_velocity.normalized();
-
 	bool was_on_floor = on_floor;
 
 	// Hack in order to work with calling from _process as well as from _physics_process; calling from thread is risky
-	float delta = Engine::get_singleton()->is_in_physics_frame() ? get_physics_process_delta_time() : get_process_delta_time();
+	double delta = Engine::get_singleton()->is_in_physics_frame() ? get_physics_process_delta_time() : get_process_delta_time();
 
 	for (int i = 0; i < 3; i++) {
 		if (locked_axis & (1 << i)) {
@@ -1111,11 +1096,11 @@ bool CharacterBody3D::move_and_slide() {
 	floor_normal = Vector3();
 	floor_velocity = Vector3();
 
-	if (current_floor_velocity != Vector3() && on_floor_body.is_valid()) {
+	if (!current_floor_velocity.is_equal_approx(Vector3()) && on_floor_body.is_valid()) {
 		PhysicsServer3D::MotionResult floor_result;
 		Set<RID> exclude;
 		exclude.insert(on_floor_body);
-		if (move_and_collide(current_floor_velocity * delta, floor_result, margin, false, false, exclude)) {
+		if (move_and_collide(current_floor_velocity * delta, floor_result, margin, false, false, false, exclude)) {
 			motion_results.push_back(floor_result);
 			_set_collision_direction(floor_result);
 		}
@@ -1130,39 +1115,35 @@ bool CharacterBody3D::move_and_slide() {
 
 	for (int iteration = 0; iteration < max_slides; ++iteration) {
 		PhysicsServer3D::MotionResult result;
-		bool found_collision = false;
-
 		bool collided = move_and_collide(motion, result, margin, false, !sliding_enabled);
-		if (!collided) {
-			motion = Vector3(); //clear because no collision happened and motion completed
-		} else {
-			found_collision = true;
-
+		if (collided) {
 			motion_results.push_back(result);
 			_set_collision_direction(result);
 
-			if (on_floor && floor_stop_on_slope) {
-				if ((body_velocity_normal + up_direction).length() < 0.01) {
-					Transform3D gt = get_global_transform();
-					if (result.travel.length() > margin) {
-						gt.origin -= result.travel.slide(up_direction);
-					} else {
-						gt.origin -= result.travel;
-					}
-					set_global_transform(gt);
-					linear_velocity = Vector3();
-					return true;
+			if (on_floor && floor_stop_on_slope && (linear_velocity.normalized() + up_direction).length() < 0.01) {
+				Transform3D gt = get_global_transform();
+				if (result.travel.length() > margin) {
+					gt.origin -= result.travel.slide(up_direction);
+				} else {
+					gt.origin -= result.travel;
 				}
+				set_global_transform(gt);
+				linear_velocity = Vector3();
+				motion = Vector3();
+				break;
 			}
 
-			if (sliding_enabled || !on_floor) {
-				motion = result.remainder.slide(result.collision_normal);
-				linear_velocity = linear_velocity.slide(result.collision_normal);
+			if (result.remainder.is_equal_approx(Vector3())) {
+				motion = Vector3();
+				break;
+			}
 
-				for (int j = 0; j < 3; j++) {
-					if (locked_axis & (1 << j)) {
-						linear_velocity[j] = 0.0;
-					}
+			if (sliding_enabled || !on_floor) {
+				Vector3 slide_motion = result.remainder.slide(result.collision_normal);
+				if (slide_motion.dot(linear_velocity) > 0.0) {
+					motion = slide_motion;
+				} else {
+					motion = Vector3();
 				}
 			} else {
 				motion = result.remainder;
@@ -1171,16 +1152,16 @@ bool CharacterBody3D::move_and_slide() {
 
 		sliding_enabled = true;
 
-		if (!found_collision || motion == Vector3()) {
+		if (!collided || motion.is_equal_approx(Vector3())) {
 			break;
 		}
 	}
 
-	if (was_on_floor && snap != Vector3()) {
+	if (was_on_floor && !on_floor && !snap.is_equal_approx(Vector3())) {
 		// Apply snap.
 		Transform3D gt = get_global_transform();
 		PhysicsServer3D::MotionResult result;
-		if (move_and_collide(snap, result, margin, true, false)) {
+		if (move_and_collide(snap, result, margin, true, false, true)) {
 			bool apply = true;
 			if (up_direction != Vector3()) {
 				if (result.get_angle(up_direction) <= floor_max_angle + FLOOR_ANGLE_THRESHOLD) {
@@ -1213,6 +1194,11 @@ bool CharacterBody3D::move_and_slide() {
 		linear_velocity += current_floor_velocity;
 	}
 
+	// Reset the gravity accumulation when touching the ground.
+	if (on_floor && linear_velocity.dot(up_direction) <= 0) {
+		linear_velocity = linear_velocity.slide(up_direction);
+	}
+
 	return motion_results.size() > 0;
 }
 
@@ -1230,8 +1216,11 @@ void CharacterBody3D::_set_collision_direction(const PhysicsServer3D::MotionResu
 			on_ceiling = true;
 		} else {
 			on_wall = true;
-			on_floor_body = p_result.collider;
-			floor_velocity = p_result.collider_velocity;
+			// Don't apply wall velocity when the collider is a CharacterBody3D.
+			if (Object::cast_to<CharacterBody3D>(ObjectDB::get_instance(p_result.collider_id)) == nullptr) {
+				on_floor_body = p_result.collider;
+				floor_velocity = p_result.collider_velocity;
+			}
 		}
 	}
 }
@@ -2250,23 +2239,19 @@ void PhysicalBone3D::_notification(int p_what) {
 	}
 }
 
-void PhysicalBone3D::_direct_state_changed(Object *p_state) {
+void PhysicalBone3D::_body_state_changed_callback(void *p_instance, PhysicsDirectBodyState3D *p_state) {
+	PhysicalBone3D *bone = (PhysicalBone3D *)p_instance;
+	bone->_body_state_changed(p_state);
+}
+
+void PhysicalBone3D::_body_state_changed(PhysicsDirectBodyState3D *p_state) {
 	if (!simulate_physics || !_internal_simulate_physics) {
 		return;
 	}
 
-	/// Update bone transform
-
-	PhysicsDirectBodyState3D *state;
-
-#ifdef DEBUG_ENABLED
-	state = Object::cast_to<PhysicsDirectBodyState3D>(p_state);
-	ERR_FAIL_NULL_MSG(state, "Method '_direct_state_changed' must receive a valid PhysicsDirectBodyState3D object as argument");
-#else
-	state = (PhysicsDirectBodyState3D *)p_state; //trust it
-#endif
+	/// Update bone transform.
 
-	Transform3D global_transform(state->get_transform());
+	Transform3D global_transform(p_state->get_transform());
 
 	set_ignore_transform_notification(true);
 	set_global_transform(global_transform);
@@ -2728,7 +2713,7 @@ void PhysicalBone3D::_start_physics_simulation() {
 	set_body_mode(PhysicsServer3D::BODY_MODE_DYNAMIC);
 	PhysicsServer3D::get_singleton()->body_set_collision_layer(get_rid(), get_collision_layer());
 	PhysicsServer3D::get_singleton()->body_set_collision_mask(get_rid(), get_collision_mask());
-	PhysicsServer3D::get_singleton()->body_set_force_integration_callback(get_rid(), callable_mp(this, &PhysicalBone3D::_direct_state_changed));
+	PhysicsServer3D::get_singleton()->body_set_state_sync_callback(get_rid(), this, &PhysicalBone3D::_body_state_changed_callback);
 	set_as_top_level(true);
 	_internal_simulate_physics = true;
 }
@@ -2747,7 +2732,7 @@ void PhysicalBone3D::_stop_physics_simulation() {
 		PhysicsServer3D::get_singleton()->body_set_collision_mask(get_rid(), 0);
 	}
 	if (_internal_simulate_physics) {
-		PhysicsServer3D::get_singleton()->body_set_force_integration_callback(get_rid(), Callable());
+		PhysicsServer3D::get_singleton()->body_set_state_sync_callback(get_rid(), nullptr, nullptr);
 		parent_skeleton->set_bone_global_pose_override(bone_id, Transform3D(), 0.0, false);
 		set_as_top_level(false);
 		_internal_simulate_physics = false;