VR-Mess/scripts/Locomotion.cs

using Godot;

public partial class Locomotion : Node
{
    private const float GripThreshold = 0.6f;
    private const float TriggerThreshold = 0.6f;
    private const float RayLength = 50f;
    private const float LaserMinDistance = 0.35f;
    private const float MarkerNearDistance = 1.2f;
    private const float LaserStart = 0.12f;

    private XROrigin3D _origin;
    private XRCamera3D _camera;
    private XRController3D _left;
    private XRController3D _right;
    private Marker3D _spawn;
    private ColorRect _fade;
    private Main _main;

    private Node3D _leftLaser;
    private Node3D _rightLaser;

    private bool _placedAtSpawn;
    private bool _teleporting;
    private bool _loggedRecentered;
    private TeleportMarker _highlighted;

    private bool _leftTrigger;
    private bool _rightTrigger;

    public override void _Ready()
    {
        _main = GetNode<Main>("..");
        _origin = GetNode<XROrigin3D>("../XROrigin3D");
        _camera = GetNode<XRCamera3D>("../XROrigin3D/XRCamera3D");
        _left = GetNode<XRController3D>("../XROrigin3D/LeftHand");
        _right = GetNode<XRController3D>("../XROrigin3D/RightHand");
        _spawn = GetNode<Marker3D>("../SpawnPoint");
        _fade = GetNode<ColorRect>("../FadeLayer/Fade");
        _main.FocusGained += OnFocusGained;
        _main.PoseRecentered += OnPoseRecentered;
        _leftLaser = MakeLaser(_left);
        _rightLaser = MakeLaser(_right);
        MakeTip(_left);
        MakeTip(_right);
    }

    public override void _Process(double delta)
    {
        if (_teleporting)
            return;

        UpdateHighlight();
        UpdateNearMarkers();
        UpdatePointer(_left, _leftLaser);
        UpdatePointer(_right, _rightLaser);

        CheckTeleport(_left, ref _leftTrigger);
        CheckTeleport(_right, ref _rightTrigger);
    }

    private static Node3D MakeLaser(XRController3D controller)
    {
        StandardMaterial3D material = new()
        {
            AlbedoColor = new Color(1f, 0.55f, 0.15f),
            EmissionEnabled = true,
            Emission = new Color(1f, 0.55f, 0.15f),
            EmissionEnergyMultiplier = 2.0f,
            ShadingMode = BaseMaterial3D.ShadingModeEnum.Unshaded,
        };
        MeshInstance3D beam = new()
        {
            Mesh = new CylinderMesh { TopRadius = 0.0035f, BottomRadius = 0.0035f, Height = 1f },
            Position = new Vector3(0, 0.5f, 0),
            CastShadow = GeometryInstance3D.ShadowCastingSetting.Off,
        };
        beam.SetSurfaceOverrideMaterial(0, material);
        Node3D pivot = new() { Visible = false };
        pivot.AddChild(beam);
        Basis rot = Basis.FromEuler(new Vector3(-Mathf.Pi / 2f, 0, 0));
        pivot.Transform = new Transform3D(rot, new Vector3(0, 0, -LaserStart));
        controller.AddChild(pivot);
        return pivot;
    }

    private static void MakeTip(XRController3D controller)
    {
        StandardMaterial3D material = new()
        {
            AlbedoColor = new Color(1f, 0.9f, 0.6f),
            EmissionEnabled = true,
            Emission = new Color(1f, 0.9f, 0.6f),
            EmissionEnergyMultiplier = 1.5f,
            ShadingMode = BaseMaterial3D.ShadingModeEnum.Unshaded,
        };
        MeshInstance3D tip = new()
        {
            Mesh = new CylinderMesh { TopRadius = 0f, BottomRadius = 0.009f, Height = 0.09f },
            CastShadow = GeometryInstance3D.ShadowCastingSetting.Off,
        };
        tip.SetSurfaceOverrideMaterial(0, material);
        Basis rot = Basis.FromEuler(new Vector3(-Mathf.Pi / 2f, 0, 0));
        tip.Transform = new Transform3D(rot, rot * new Vector3(0, 0.045f, 0));
        controller.AddChild(tip);
    }

    private void UpdateNearMarkers()
    {
        Vector3 head = _camera.GlobalPosition;
        foreach (Node node in GetTree().GetNodesInGroup("teleport_marker"))
        {
            if (node is not TeleportMarker marker)
                continue;
            Vector3 d = marker.GlobalPosition - head;
            d.Y = 0f;
            marker.SetNear(d.Length() < MarkerNearDistance);
        }
    }

    private void UpdatePointer(XRController3D controller, Node3D laser)
    {
        Vector3 from = controller.GlobalPosition;
        Vector3 dir = -controller.GlobalTransform.Basis.Z;

        var query = PhysicsRayQueryParameters3D.Create(from, from + dir * RayLength);
        var hit = GetViewport().GetWorld3D().DirectSpaceState.IntersectRay(query);

        bool show = false;
        float dist = 0f;
        if (WorldPanel.TryGetLaserDistance(controller, out float panelDist))
        {
            // The menu decides touch vs laser itself (tip near the surface = touch, no beam).
            dist = panelDist;
            show = dist > LaserMinDistance;
        }
        else if (hit.TryGetValue("collider", out Variant collider) && collider.As<Node>() is Node node)
        {
            bool interactive;
            if (node.GetParent() is TeleportMarker marker)
                interactive = !marker.IsNear;
            else
                interactive = false; // panel hit but the menu isn't in laser mode for this hand
            if (interactive && hit.TryGetValue("position", out Variant posVariant))
            {
                dist = from.DistanceTo(posVariant.As<Vector3>());
                show = dist > LaserMinDistance;
            }
        }

        laser.Visible = show;
        if (show) // beam starts at the stylus tip (LaserStart ahead of the controller)
            laser.Scale = new Vector3(1, Mathf.Max(0.01f, dist - LaserStart), 1);
    }

    private void CheckTeleport(XRController3D controller, ref bool wasPressed)
    {
        bool pressed = controller.GetFloat("trigger") > TriggerThreshold;
        if (pressed && !wasPressed && !WorldPanel.IsBlocking(controller))
        {
            TeleportMarker marker = AimMarker(controller);
            if (marker is { IsNear: false })
                StartTeleport(marker);
        }
        wasPressed = pressed;
    }

    private void UpdateHighlight()
    {
        TeleportMarker marker = AimMarker(_left) ?? AimMarker(_right);
        if (marker is { IsNear: true })
            marker = null;
        if (marker == _highlighted)
            return;
        _highlighted?.SetHighlighted(false);
        _highlighted = marker;
        _highlighted?.SetHighlighted(true);
    }

    private TeleportMarker AimMarker(XRController3D controller)
    {
        Vector3 from = controller.GlobalPosition;
        Vector3 to = from - controller.GlobalTransform.Basis.Z * RayLength;
        var query = PhysicsRayQueryParameters3D.Create(from, to);
        var hit = GetViewport().GetWorld3D().DirectSpaceState.IntersectRay(query);
        if (hit.TryGetValue("collider", out Variant collider) && collider.As<Node>()?.GetParent() is TeleportMarker marker)
            return marker;
        return null;
    }

    private void StartTeleport(TeleportMarker marker)
    {
        _teleporting = true;
        Vector3 target = marker.GlobalPosition;
        float yaw = ForwardYaw(marker.GlobalTransform);

        var tween = CreateTween();
        tween.TweenProperty(_fade, "color:a", 1.0f, 0.12);
        tween.TweenCallback(Callable.From(() =>
        {
            PlacePlayer(target, yaw);
            GD.Print($"Locomotion: teleported to marker '{marker.MarkerLabel}'");
        }));
        tween.TweenProperty(_fade, "color:a", 0.0f, 0.25);
        tween.TweenCallback(Callable.From(() => _teleporting = false));
    }

    public void ReturnToSpawn()
    {
        MoveToSpawn();
        GD.Print("Locomotion: returned to spawn point");
    }

    private void OnFocusGained()
    {
        if (_placedAtSpawn)
            return;
        _placedAtSpawn = true;
        MoveToSpawn();
        GD.Print("Locomotion: player placed at spawn point");
    }

    private void OnPoseRecentered()
    {
        if (_loggedRecentered)
            return;
        _loggedRecentered = true;
        GD.Print("Locomotion: pose_recentered received — WiVRN fires this continuously (~90 Hz), placement stays unchanged and further occurrences are not logged");
    }

    private void MoveToSpawn() => PlacePlayer(_spawn.GlobalPosition, ForwardYaw(_spawn.GlobalTransform));

    private void PlacePlayer(Vector3 targetXz, float yaw)
    {
        Transform3D h = _camera.GlobalTransform;
        Transform3D hLocal = _origin.GlobalTransform.AffineInverse() * h;
        float hmdYaw = h.Basis.GetEuler().Y;
        Basis basis = Basis.FromEuler(new Vector3(0, yaw - hmdYaw, 0)) * h.Basis;
        Transform3D desired = new Transform3D(basis, new Vector3(targetXz.X, h.Origin.Y, targetXz.Z));
        _origin.GlobalTransform = desired * hLocal.AffineInverse();
    }

    private static float ForwardYaw(Transform3D transform)
    {
        Vector3 fwd = -transform.Basis.Z;
        fwd.Y = 0f;
        if (fwd.LengthSquared() < 0.0001f)
            return 0f;
        return Mathf.Atan2(-fwd.X, -fwd.Z);
    }
}