Initial commit

floating_origin.gd

@@ -0,0 +1,94 @@
+extends Node
+## Sibling FO. Shift registered world roots.
+## Move focus only when it is NOT already under a root (capsule = sibling).
+## Never zero velocity.
+
+const THRESHOLD: float = 50000.0
+const COOLDOWN: float = 0.25
+
+var focus: Node3D = null
+var _shift_roots: Array[Node3D] = []
+var _cooldown: float = 0.0
+
+
+func _ready() -> void:
+	process_mode = Node.PROCESS_MODE_ALWAYS
+
+
+func set_focus(node: Node3D) -> void:
+	focus = node
+
+
+func register_root(node: Node3D) -> void:
+	if node == null:
+		return
+	if not _shift_roots.has(node):
+		_shift_roots.append(node)
+		print("FloatingOrigin registered: ", node.name)
+
+
+func unregister_root(node: Node3D) -> void:
+	_shift_roots.erase(node)
+
+
+func shift(offset: Vector3) -> void:
+	if offset.length_squared() < 1.0:
+		return
+	_apply_shift(offset)
+	_cooldown = COOLDOWN
+
+
+func _physics_process(delta: float) -> void:
+	if _cooldown > 0.0:
+		_cooldown -= delta
+		return
+	if focus == null or not is_instance_valid(focus) or not focus.is_inside_tree():
+		return
+	if _shift_roots.is_empty():
+		return
+	var fp: Vector3 = focus.global_position
+	if fp.length() < THRESHOLD:
+		return
+	_apply_shift(-fp)
+	_cooldown = COOLDOWN
+
+
+func _apply_shift(offset: Vector3) -> void:
+	for root in _shift_roots:
+		if is_instance_valid(root) and root.is_inside_tree():
+			root.global_position += offset
+	if is_instance_valid(focus) and focus.is_inside_tree() and not _focus_under_a_root():
+		focus.global_position += offset
+	_sync_focus_physics()
+	print("FloatingOrigin shift: ", offset, " focus now: ", focus.global_position if focus else Vector3.ZERO)
+
+
+func _focus_under_a_root() -> bool:
+	var n: Node = focus
+	while n != null:
+		if n is Node3D and _shift_roots.has(n as Node3D):
+			return true
+		n = n.get_parent()
+	return false
+
+
+func _sync_focus_physics() -> void:
+	if focus == null or not is_instance_valid(focus):
+		return
+	if not (focus is CharacterBody3D):
+		return
+	var body := focus as CharacterBody3D
+	var saved_vel: Vector3 = body.velocity
+	PhysicsServer3D.body_set_state(
+		body.get_rid(),
+		PhysicsServer3D.BODY_STATE_TRANSFORM,
+		body.global_transform
+	)
+	PhysicsServer3D.body_set_state(
+		body.get_rid(),
+		PhysicsServer3D.BODY_STATE_LINEAR_VELOCITY,
+		saved_vel
+	)
+	body.velocity = saved_vel
+	if body.has_method("reset_physics_interpolation"):
+		body.reset_physics_interpolation()