Support for 3D sync to physics

Same implementation as in 2D.

5 files changed, 154 insertions, 26 deletions

diff --git a/doc/classes/StaticBody3D.xml b/doc/classes/StaticBody3D.xml
index f83d440f10..69c123002f 100644
--- a/doc/classes/StaticBody3D.xml
+++ b/doc/classes/StaticBody3D.xml
@@ -32,6 +32,9 @@
 			The physics material override for the body.
 			If a material is assigned to this property, it will be used instead of any other physics material, such as an inherited one.
 		</member>
+		<member name="sync_to_physics" type="bool" setter="set_sync_to_physics" getter="is_sync_to_physics_enabled" default="false">
+			If [code]true[/code] and [member kinematic_motion] is enabled, the body's movement will be synchronized to the physics frame. This is useful when animating movement via [AnimationPlayer], for example on moving platforms. Do [b]not[/b] use together with [method PhysicsBody3D.move_and_collide].
+		</member>
 	</members>
 	<constants>
 	</constants>
diff --git a/scene/3d/collision_object_3d.cpp b/scene/3d/collision_object_3d.cpp
index 40e3f7c764..dd1f25da68 100644
--- a/scene/3d/collision_object_3d.cpp
+++ b/scene/3d/collision_object_3d.cpp
@@ -77,6 +77,10 @@ void CollisionObject3D::_notification(int p_what) {
 		} break;
 
 		case NOTIFICATION_TRANSFORM_CHANGED: {
+			if (only_update_transform_changes) {
+				return;
+			}
+
 			if (area) {
 				PhysicsServer3D::get_singleton()->area_set_transform(rid, get_global_transform());
 			} else {
@@ -284,6 +288,14 @@ void CollisionObject3D::set_body_mode(PhysicsServer3D::BodyMode p_mode) {
 	PhysicsServer3D::get_singleton()->body_set_mode(rid, p_mode);
 }
 
+void CollisionObject3D::set_only_update_transform_changes(bool p_enable) {
+	only_update_transform_changes = p_enable;
+}
+
+bool CollisionObject3D::is_only_update_transform_changes_enabled() const {
+	return only_update_transform_changes;
+}
+
 void CollisionObject3D::_update_pickable() {
 	if (!is_inside_tree()) {
 		return;
diff --git a/scene/3d/collision_object_3d.h b/scene/3d/collision_object_3d.h
index a3a890db75..7c30a5cd98 100644
--- a/scene/3d/collision_object_3d.h
+++ b/scene/3d/collision_object_3d.h
@@ -74,6 +74,8 @@ private:
 
 	Map<uint32_t, ShapeData> shapes;
 
+	bool only_update_transform_changes = false; // This is used for sync to physics.
+
 	bool capture_input_on_drag = false;
 	bool ray_pickable = true;
 
@@ -107,6 +109,9 @@ protected:
 
 	void set_body_mode(PhysicsServer3D::BodyMode p_mode);
 
+	void set_only_update_transform_changes(bool p_enable);
+	bool is_only_update_transform_changes_enabled() const;
+
 public:
 	void set_collision_layer(uint32_t p_layer);
 	uint32_t get_collision_layer() const;
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());
 
diff --git a/scene/3d/physics_body_3d.h b/scene/3d/physics_body_3d.h
index 9d45ce3799..0ef9c78f3b 100644
--- a/scene/3d/physics_body_3d.h
+++ b/scene/3d/physics_body_3d.h
@@ -82,13 +82,16 @@ class StaticBody3D : public PhysicsBody3D {
 	Ref<PhysicsMaterial> physics_material_override;
 
 	bool kinematic_motion = false;
+	bool sync_to_physics = false;
+
+	Transform3D last_valid_transform;
+
+	void _direct_state_changed(Object *p_state);
 
 protected:
 	void _notification(int p_what);
 	static void _bind_methods();
 
-	void _direct_state_changed(Object *p_state);
-
 public:
 	void set_physics_material_override(const Ref<PhysicsMaterial> &p_physics_material_override);
 	Ref<PhysicsMaterial> get_physics_material_override() const;
@@ -102,6 +105,8 @@ public:
 	virtual Vector3 get_linear_velocity() const override;
 	virtual Vector3 get_angular_velocity() const override;
 
+	virtual TypedArray<String> get_configuration_warnings() const override;
+
 	StaticBody3D();
 
 private:
@@ -111,6 +116,9 @@ private:
 
 	void set_kinematic_motion_enabled(bool p_enabled);
 	bool is_kinematic_motion_enabled() const;
+
+	void set_sync_to_physics(bool p_enable);
+	bool is_sync_to_physics_enabled() const;
 };
 
 class RigidBody3D : public PhysicsBody3D {
@@ -251,7 +259,7 @@ public:
 	void apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3());
 	void apply_torque_impulse(const Vector3 &p_impulse);
 
-	TypedArray<String> get_configuration_warnings() const override;
+	virtual TypedArray<String> get_configuration_warnings() const override;
 
 	RigidBody3D();
 	~RigidBody3D();