1 files changed, 123 insertions, 23 deletions

diff --git a/scene/3d/physics_body_3d.cpp b/scene/3d/physics_body_3d.cpp
index 4568145458..760dbdadca 100644
--- a/scene/3d/physics_body_3d.cpp
+++ b/scene/3d/physics_body_3d.cpp
@@ -243,6 +243,13 @@ void StaticBody3D::set_kinematic_motion_enabled(bool p_enabled) {
 		set_body_mode(PhysicsServer3D::BODY_MODE_STATIC);
 	}
 
+#ifdef TOOLS_ENABLED
+	if (Engine::get_singleton()->is_editor_hint()) {
+		update_configuration_warnings();
+		return;
+	}
+#endif
+
 	_update_kinematic_motion();
 }
 
@@ -260,6 +267,57 @@ void StaticBody3D::set_constant_linear_velocity(const Vector3 &p_vel) {
 	}
 }
 
+void StaticBody3D::set_sync_to_physics(bool p_enable) {
+	if (sync_to_physics == p_enable) {
+		return;
+	}
+
+	sync_to_physics = p_enable;
+
+#ifdef TOOLS_ENABLED
+	if (Engine::get_singleton()->is_editor_hint()) {
+		update_configuration_warnings();
+		return;
+	}
+#endif
+
+	if (kinematic_motion) {
+		_update_kinematic_motion();
+	}
+}
+
+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");
+
+	linear_velocity = state->get_linear_velocity();
+	angular_velocity = state->get_angular_velocity();
+
+	if (!sync_to_physics) {
+		return;
+	}
+
+	last_valid_transform = state->get_transform();
+	set_notify_local_transform(false);
+	set_global_transform(last_valid_transform);
+	set_notify_local_transform(true);
+	_on_transform_changed();
+}
+
+TypedArray<String> StaticBody3D::get_configuration_warnings() const {
+	TypedArray<String> warnings = PhysicsBody3D::get_configuration_warnings();
+
+	if (sync_to_physics && !kinematic_motion) {
+		warnings.push_back(TTR("Sync to physics works only when kinematic motion is enabled."));
+	}
+
+	return warnings;
+}
+
 void StaticBody3D::set_constant_angular_velocity(const Vector3 &p_vel) {
 	constant_angular_velocity = p_vel;
 
@@ -288,6 +346,34 @@ Vector3 StaticBody3D::get_angular_velocity() const {
 
 void StaticBody3D::_notification(int p_what) {
 	switch (p_what) {
+		case NOTIFICATION_ENTER_TREE: {
+			last_valid_transform = get_global_transform();
+		} break;
+
+		case NOTIFICATION_LOCAL_TRANSFORM_CHANGED: {
+			// 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();
+			new_transform.origin += constant_linear_velocity * delta_time;
+
+			real_t ang_vel = constant_angular_velocity.length();
+			if (!Math::is_zero_approx(ang_vel)) {
+				Vector3 ang_vel_axis = constant_angular_velocity / ang_vel;
+				Basis rot(ang_vel_axis, ang_vel * delta_time);
+				new_transform.basis = rot * new_transform.basis;
+				new_transform.orthonormalize();
+			}
+
+			PhysicsServer3D::get_singleton()->body_set_state(get_rid(), PhysicsServer3D::BODY_STATE_TRANSFORM, new_transform);
+
+			// ... but then revert changes.
+			set_notify_local_transform(false);
+			set_global_transform(last_valid_transform);
+			set_notify_local_transform(true);
+			_on_transform_changed();
+		} break;
+
 		case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: {
 #ifdef TOOLS_ENABLED
 			if (Engine::get_singleton()->is_editor_hint()) {
@@ -297,9 +383,9 @@ void StaticBody3D::_notification(int p_what) {
 
 			ERR_FAIL_COND(!kinematic_motion);
 
-			real_t delta_time = get_physics_process_delta_time();
-
 			Transform3D new_transform = get_global_transform();
+
+			real_t delta_time = get_physics_process_delta_time();
 			new_transform.origin += constant_linear_velocity * delta_time;
 
 			real_t ang_vel = constant_angular_velocity.length();
@@ -310,13 +396,18 @@ void StaticBody3D::_notification(int p_what) {
 				new_transform.orthonormalize();
 			}
 
-			PhysicsServer3D::get_singleton()->body_set_state(get_rid(), PhysicsServer3D::BODY_STATE_TRANSFORM, new_transform);
+			if (sync_to_physics) {
+				// Propagate transform change to node.
+				set_global_transform(new_transform);
+			} else {
+				PhysicsServer3D::get_singleton()->body_set_state(get_rid(), PhysicsServer3D::BODY_STATE_TRANSFORM, new_transform);
 
-			// Propagate transform change to node.
-			set_ignore_transform_notification(true);
-			set_global_transform(new_transform);
-			set_ignore_transform_notification(false);
-			_on_transform_changed();
+				// Propagate transform change to node.
+				set_ignore_transform_notification(true);
+				set_global_transform(new_transform);
+				set_ignore_transform_notification(false);
+				_on_transform_changed();
+			}
 		} break;
 	}
 }
@@ -333,22 +424,14 @@ void StaticBody3D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_physics_material_override", "physics_material_override"), &StaticBody3D::set_physics_material_override);
 	ClassDB::bind_method(D_METHOD("get_physics_material_override"), &StaticBody3D::get_physics_material_override);
 
+	ClassDB::bind_method(D_METHOD("set_sync_to_physics", "enable"), &StaticBody3D::set_sync_to_physics);
+	ClassDB::bind_method(D_METHOD("is_sync_to_physics_enabled"), &StaticBody3D::is_sync_to_physics_enabled);
+
 	ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "physics_material_override", PROPERTY_HINT_RESOURCE_TYPE, "PhysicsMaterial"), "set_physics_material_override", "get_physics_material_override");
 	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "constant_linear_velocity"), "set_constant_linear_velocity", "get_constant_linear_velocity");
 	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "constant_angular_velocity"), "set_constant_angular_velocity", "get_constant_angular_velocity");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "kinematic_motion"), "set_kinematic_motion_enabled", "is_kinematic_motion_enabled");
-}
-
-void StaticBody3D::_direct_state_changed(Object *p_state) {
-#ifdef DEBUG_ENABLED
-	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");
-#else
-	PhysicsDirectBodyState3D *state = (PhysicsDirectBodyState3D *)p_state; //trust it
-#endif
-
-	linear_velocity = state->get_linear_velocity();
-	angular_velocity = state->get_angular_velocity();
+	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "sync_to_physics"), "set_sync_to_physics", "is_sync_to_physics_enabled");
 }
 
 StaticBody3D::StaticBody3D() :
@@ -372,18 +455,26 @@ void StaticBody3D::_update_kinematic_motion() {
 	}
 #endif
 
+	if (kinematic_motion && sync_to_physics) {
+		set_only_update_transform_changes(true);
+		set_notify_local_transform(true);
+	} else {
+		set_only_update_transform_changes(false);
+		set_notify_local_transform(false);
+	}
+
+	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));
 
 		if (!constant_angular_velocity.is_equal_approx(Vector3()) || !constant_linear_velocity.is_equal_approx(Vector3())) {
-			set_physics_process_internal(true);
-			return;
+			needs_physics_process = true;
 		}
 	} else {
 		PhysicsServer3D::get_singleton()->body_set_force_integration_callback(get_rid(), Callable());
 	}
 
-	set_physics_process_internal(false);
+	set_physics_process_internal(needs_physics_process);
 }
 
 void RigidBody3D::_body_enter_tree(ObjectID p_id) {
@@ -1006,6 +1097,15 @@ void CharacterBody3D::move_and_slide() {
 		}
 	}
 
+	Vector3 current_floor_velocity = floor_velocity;
+	if (on_floor && on_floor_body.is_valid()) {
+		//this approach makes sure there is less delay between the actual body velocity and the one we saved
+		PhysicsDirectBodyState3D *bs = PhysicsServer3D::get_singleton()->body_get_direct_state(on_floor_body);
+		if (bs) {
+			current_floor_velocity = bs->get_linear_velocity();
+		}
+	}
+
 	// Hack in order to work with calling from _process as well as from _physics_process; calling from thread is risky
 	Vector3 motion = (floor_velocity + linear_velocity) * (Engine::get_singleton()->is_in_physics_frame() ? get_physics_process_delta_time() : get_process_delta_time());