[3.x] Support for 3D sync to physics

doc/classes/KinematicBody.xml

@@ -173,6 +173,9 @@
 			A higher value means it's more flexible for detecting collision, which helps with consistently detecting walls and floors.
 			A lower value forces the collision algorithm to use more exact detection, so it can be used in cases that specifically require precision, e.g at very low scale to avoid visible jittering, or for stability with a stack of kinematic bodies.
 		</member>
+		<member name="motion/sync_to_physics" type="bool" setter="set_sync_to_physics" getter="is_sync_to_physics_enabled" default="false">
+			If [code]true[/code], 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 move_and_slide] or [method move_and_collide] functions.
+		</member>
 		<member name="move_lock_x" type="bool" setter="set_axis_lock" getter="get_axis_lock" default="false">
 			Lock the body's X axis movement.
 		</member>

scene/3d/collision_object.cpp

@@ -72,6 +72,10 @@ void CollisionObject::_notification(int p_what) {
 		} break;
 
 		case NOTIFICATION_TRANSFORM_CHANGED: {
+			if (only_update_transform_changes) {
+				return;
+			}
+
 			if (area) {
 				PhysicsServer::get_singleton()->area_set_transform(rid, get_global_transform());
 			} else {
@@ -171,6 +175,10 @@ void CollisionObject::_mouse_exit() {
 	emit_signal(SceneStringNames::get_singleton()->mouse_exited);
 }
 
+void CollisionObject::set_only_update_transform_changes(bool p_enable) {
+	only_update_transform_changes = p_enable;
+}
+
 void CollisionObject::_update_pickable() {
 	if (!is_inside_tree()) {
 		return;

scene/3d/collision_object.h

@@ -65,6 +65,7 @@ class CollisionObject : public Spatial {
 	int total_subshapes;
 
 	Map<uint32_t, ShapeData> shapes;
+	bool only_update_transform_changes = false; //this is used for sync physics in KinematicBody
 
 	bool capture_input_on_drag;
 	bool ray_pickable;
@@ -91,6 +92,8 @@ class CollisionObject : public Spatial {
 	virtual void _mouse_enter();
 	virtual void _mouse_exit();
 
+	void set_only_update_transform_changes(bool p_enable);
+
 	void _on_transform_changed();
 
 public:

scene/3d/physics_body.cpp

@@ -974,6 +974,10 @@ Ref<KinematicCollision> KinematicBody::_move(const Vector3 &p_motion, bool p_inf
 }
 
 bool KinematicBody::move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, Collision &r_collision, bool p_exclude_raycast_shapes, bool p_test_only) {
+	if (sync_to_physics) {
+		ERR_PRINT("Functions move_and_slide and move_and_collide do not work together with 'sync to physics' option. Please read the documentation.");
+	}
+
 	Transform gt = get_global_transform();
 	PhysicsServer::MotionResult result;
 	bool colliding = PhysicsServer::get_singleton()->body_test_motion(get_rid(), gt, p_motion, p_infinite_inertia, &result, p_exclude_raycast_shapes);
@@ -1019,8 +1023,17 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve
 		}
 	}
 
+	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.
+		PhysicsDirectBodyState *bs = PhysicsServer::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 + body_velocity) * (Engine::get_singleton()->is_in_physics_frame() ? get_physics_process_delta_time() : get_process_delta_time());
+	Vector3 motion = (current_floor_velocity + body_velocity) * (Engine::get_singleton()->is_in_physics_frame() ? get_physics_process_delta_time() : get_process_delta_time());
 
 	on_floor = false;
 	on_floor_body = RID();
@@ -1241,8 +1254,50 @@ Ref<KinematicCollision> KinematicBody::_get_slide_collision(int p_bounce) {
 	return slide_colliders[p_bounce];
 }
 
+void KinematicBody::set_sync_to_physics(bool p_enable) {
+	if (sync_to_physics == p_enable) {
+		return;
+	}
+	sync_to_physics = p_enable;
+
+	if (Engine::get_singleton()->is_editor_hint()) {
+		return;
+	}
+
+	if (p_enable) {
+		PhysicsServer::get_singleton()->body_set_force_integration_callback(get_rid(), this, "_direct_state_changed");
+		set_only_update_transform_changes(true);
+		set_notify_local_transform(true);
+	} else {
+		PhysicsServer::get_singleton()->body_set_force_integration_callback(get_rid(), nullptr, "");
+		set_only_update_transform_changes(false);
+		set_notify_local_transform(false);
+	}
+}
+
+bool KinematicBody::is_sync_to_physics_enabled() const {
+	return sync_to_physics;
+}
+
+void KinematicBody::_direct_state_changed(Object *p_state) {
+	if (!sync_to_physics) {
+		return;
+	}
+
+	PhysicsDirectBodyState *state = Object::cast_to<PhysicsDirectBodyState>(p_state);
+	ERR_FAIL_COND_MSG(!state, "Method '_direct_state_changed' must receive a valid PhysicsDirectBodyState object as argument");
+
+	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();
+}
+
 void KinematicBody::_notification(int p_what) {
 	if (p_what == NOTIFICATION_ENTER_TREE) {
+		last_valid_transform = get_global_transform();
+
 		// Reset move_and_slide() data.
 		on_floor = false;
 		on_floor_body = RID();
@@ -1251,6 +1306,17 @@ void KinematicBody::_notification(int p_what) {
 		colliders.clear();
 		floor_velocity = Vector3();
 	}
+
+	if (p_what == NOTIFICATION_LOCAL_TRANSFORM_CHANGED) {
+		//used by sync to physics, send the new transform to the physics
+		Transform new_transform = get_global_transform();
+		PhysicsServer::get_singleton()->body_set_state(get_rid(), PhysicsServer::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();
+	}
 }
 
 void KinematicBody::_bind_methods() {
@@ -1275,11 +1341,17 @@ void KinematicBody::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("get_slide_count"), &KinematicBody::get_slide_count);
 	ClassDB::bind_method(D_METHOD("get_slide_collision", "slide_idx"), &KinematicBody::_get_slide_collision);
 
+	ClassDB::bind_method(D_METHOD("set_sync_to_physics", "enable"), &KinematicBody::set_sync_to_physics);
+	ClassDB::bind_method(D_METHOD("is_sync_to_physics_enabled"), &KinematicBody::is_sync_to_physics_enabled);
+
+	ClassDB::bind_method(D_METHOD("_direct_state_changed"), &KinematicBody::_direct_state_changed);
+
 	ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "move_lock_x", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NOEDITOR), "set_axis_lock", "get_axis_lock", PhysicsServer::BODY_AXIS_LINEAR_X);
 	ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "move_lock_y", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NOEDITOR), "set_axis_lock", "get_axis_lock", PhysicsServer::BODY_AXIS_LINEAR_Y);
 	ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "move_lock_z", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NOEDITOR), "set_axis_lock", "get_axis_lock", PhysicsServer::BODY_AXIS_LINEAR_Z);
 
 	ADD_PROPERTY(PropertyInfo(Variant::REAL, "collision/safe_margin", PROPERTY_HINT_RANGE, "0.001,256,0.001"), "set_safe_margin", "get_safe_margin");
+	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "motion/sync_to_physics"), "set_sync_to_physics", "is_sync_to_physics_enabled");
 }
 
 KinematicBody::KinematicBody() :

scene/3d/physics_body.h

@@ -288,6 +288,7 @@ class KinematicBody : public PhysicsBody {
 	bool on_floor;
 	bool on_ceiling;
 	bool on_wall;
+	bool sync_to_physics = false;
 	Vector<Collision> colliders;
 	Vector<Ref<KinematicCollision>> slide_colliders;
 	Ref<KinematicCollision> motion_cache;
@@ -297,6 +298,9 @@ class KinematicBody : public PhysicsBody {
 	Ref<KinematicCollision> _move(const Vector3 &p_motion, bool p_infinite_inertia = true, bool p_exclude_raycast_shapes = true, bool p_test_only = false);
 	Ref<KinematicCollision> _get_slide_collision(int p_bounce);
 
+	Transform last_valid_transform;
+	void _direct_state_changed(Object *p_state);
+
 protected:
 	void _notification(int p_what);
 	static void _bind_methods();
@@ -324,6 +328,9 @@ class KinematicBody : public PhysicsBody {
 	int get_slide_count() const;
 	Collision get_slide_collision(int p_bounce) const;
 
+	void set_sync_to_physics(bool p_enable);
+	bool is_sync_to_physics_enabled() const;
+
 	KinematicBody();
 	~KinematicBody();
 };