cryptonhym-thunder/Code/Services/GodotWorldQuery.cs

using System;
using System.Collections.Generic;
using System.Linq;
using CryptonymThunder.Code.Autoloads;
using CryptonymThunder.Code.Extensions;
using CryptonymThunder.Code.Presenters;
using GameCore.ECS;
using GameCore.ECS.Interfaces;
using Godot;
using GodotWorld = Godot.World3D;
using Vector3 = GameCore.Math.Vector3;

namespace CryptonymThunder.Code.Services;

public class GodotWorldQuery(GamePresenter ownerNode) : IWorldQuery
{
    private readonly PresenterRegistry _presenterRegistry = ownerNode.GetNode<PresenterRegistry>("/root/PresenterRegistry");
    private readonly GodotWorld _godotWorld = ((SceneTree)Engine.GetMainLoop()).Root.World3D;

    public HitResult Raycast(Vector3 from, Vector3 to, Entity? ownerToExclude = null)
    {
        var spaceState = _godotWorld.DirectSpaceState;

        var query = PhysicsRayQueryParameters3D.Create(from.ToGodot(), to.ToGodot());
        query.CollisionMask = 1;
        query.CollideWithBodies = true;
        query.CollideWithAreas = false;

        if (ownerToExclude.HasValue)
        {
            var ownerPresenter = ownerNode.GetPresenterNode(ownerToExclude.Value);
            if (ownerPresenter != null)
            {
                query.Exclude = [((PhysicsBody3D)ownerPresenter).GetRid()];
            }
        }
        
        var result = spaceState.IntersectRay(query);

        if (result.Count > 0 && result["collider"].Obj is Node3D collider)
        {
            if (_presenterRegistry.TryGetEntity(collider.GetInstanceId(), out var hitEntity))
            {
                return new HitResult
                {
                    DidHit = true,
                    HitEntity = hitEntity,
                    Position = result["position"].AsVector3().ToGameCore(),
                    Normal = result["normal"].AsVector3().ToGameCore()
                };
            }
        }
        
        return new HitResult { DidHit = false };
    }

    public Vector3 RotateVectorByYaw(Vector3 vector, float yawRadians)
    {
        var rotatedX = vector.X * (float)Math.Cos(yawRadians) + vector.Z * (float)Math.Sin(yawRadians);
        var rotatedZ = -vector.X * (float)Math.Sin(yawRadians) + vector.Z * (float)Math.Cos(yawRadians);
        return new Vector3(rotatedX, vector.Y, rotatedZ);
    }

    public IEnumerable<Entity> OverlapSphere(Vector3 position, float radius, Entity? ownerToExclude = null)
    {
        var spaceState = _godotWorld.DirectSpaceState;
        var query = new PhysicsShapeQueryParameters3D();

        var sphere = new SphereShape3D();
        sphere.Radius = radius;
        query.Shape = sphere;
        
        query.Transform = new Transform3D(Basis.Identity, position.ToGodot());
        
        query.CollisionMask = 1;
        query.CollideWithBodies = true;
        query.CollideWithAreas = false;

        if (ownerToExclude.HasValue)
        {
            var ownerPresenter = ownerNode.GetPresenterNode(ownerToExclude.Value);
            if (ownerPresenter != null)
            {
                query.Exclude = [((PhysicsBody3D)ownerPresenter).GetRid()];
            }
        }

        var results = spaceState.IntersectShape(query);
        if (results.Count == 0)
        {
            yield break;
        }

        foreach (var result in results)
        {
            if (result["collider"].Obj is Node3D collider)
            {
                if (_presenterRegistry.TryGetEntity(collider.GetInstanceId(), out var hitEntity))
                {
                    yield return hitEntity;
                }
            }
        }
    }

    public List<Vector3> GetPath(Vector3 start, Vector3 end)
    {
        var map = _godotWorld.NavigationMap;
        var pathPoints = NavigationServer3D.MapGetPath(map, start.ToGodot(), end.ToGodot(), true);

        return pathPoints.Select(p => p.ToGameCore()).ToList();
    }
}