FishROV/FishROV/Assets/Scripts/Benchmark/FrameworkAdapters/AstarRvoAdapter.cs

using System;
using System.Collections.Generic;
using System.Reflection;
using UnityEngine;

namespace FishROV.AvoidanceBenchmark
{
    public sealed class AstarRvoAdapter : IAvoidanceAdapter
    {
        private const BindingFlags MemberFlags = BindingFlags.Instance | BindingFlags.Public | BindingFlags.NonPublic;

        private readonly Dictionary<BenchmarkAgent, RvoAgentBinding> bindings =
            new Dictionary<BenchmarkAgent, RvoAgentBinding>(512);

        private Type rvoControllerType;
        private Type rvoSimulatorType;
        private MethodInfo setTargetMethod;
        private MethodInfo calculateMovementDeltaMethod;
        private GameObject simulatorObject;
        private AvoidanceBenchmarkConfig config;
        private string status = "Not initialized";

        public string Name => "A* RVO 本地避障";
        public bool IsAvailable => rvoControllerType != null && rvoSimulatorType != null;
        public string Status => status;

        public void Initialize(AvoidanceBenchmarkConfig config)
        {
            this.config = config;
            ResolveAstarTypes();

            if (!IsAvailable)
            {
                status = "未检测到 A* Pathfinding Project Pro 的 RVO 模块；请导入 A* Pro，确保存在 Pathfinding.RVO.RVOController 和 RVOSimulator。当前退回基线移动。";
                return;
            }

            simulatorObject = new GameObject("A* RVO 模拟器");
            var simulator = simulatorObject.AddComponent(rvoSimulatorType);
            ConfigureSimulator(simulator);

            var dimensionNote = config.DimensionMode == AvoidanceDimensionMode.XYZ3D
                ? "3D 压测会投影到 A* RVO 的 XZ 平面"
                : "XZ 平面";
            status = $"A* Pro RVO 已通过反射接入（{dimensionNote}）。";
        }

        public BenchmarkAgent CreateAgent(BenchmarkAgentSpawnInfo spawnInfo, Transform parent)
        {
            var agent = BenchmarkAgent.CreatePrimitive(spawnInfo, parent);
            if (!IsAvailable)
            {
                return agent;
            }

            var controller = agent.gameObject.AddComponent(rvoControllerType);
            ConfigureController(controller, spawnInfo);
            bindings[agent] = new RvoAgentBinding(agent, controller);
            return agent;
        }

        public void SetPreferredVelocity(BenchmarkAgent agent, Vector3 preferredVelocity)
        {
            if (!IsAvailable || !bindings.TryGetValue(agent, out var binding))
            {
                agent.ApplyVelocity(preferredVelocity);
                return;
            }

            binding.PreferredVelocity = Vector3.ClampMagnitude(preferredVelocity, agent.MaxSpeed);
        }

        public void SetTarget(BenchmarkAgent agent, Vector3 target)
        {
            agent.Target = target;
        }

        public void Tick(float deltaTime)
        {
            if (!IsAvailable || deltaTime <= 0f)
            {
                return;
            }

            foreach (var binding in bindings.Values)
            {
                var velocity = CalculateAvoidedVelocity(binding, deltaTime);
                binding.Agent.ApplyVelocity(velocity);
            }
        }

        public void Dispose()
        {
            bindings.Clear();
            if (simulatorObject != null)
            {
                UnityEngine.Object.Destroy(simulatorObject);
                simulatorObject = null;
            }
        }

        private void ResolveAstarTypes()
        {
            rvoControllerType = FindType("Pathfinding.RVO.RVOController");
            rvoSimulatorType = FindType("Pathfinding.RVO.RVOSimulator");

            if (rvoControllerType == null)
            {
                return;
            }

            setTargetMethod = rvoControllerType.GetMethod(
                "SetTarget",
                MemberFlags,
                null,
                new[] { typeof(Vector3), typeof(float), typeof(float), typeof(Vector3) },
                null);
            calculateMovementDeltaMethod = rvoControllerType.GetMethod(
                "CalculateMovementDelta",
                MemberFlags,
                null,
                new[] { typeof(Vector3), typeof(float) },
                null);
        }

        private void ConfigureSimulator(object simulator)
        {
            SetMember(simulator, "desiredSimulationFPS", 60);
            SetMember(simulator, "movementPlane", "XZ");
            SetMember(simulator, "doubleBuffering", true);
            SetMember(simulator, "workerThreads", "None");
            SetMember(simulator, "symmetryBreakingBias", 0.1f);
        }

        private void ConfigureController(object controller, BenchmarkAgentSpawnInfo spawnInfo)
        {
            SetMember(controller, "radius", spawnInfo.Radius);
            SetMember(controller, "height", Mathf.Max(spawnInfo.Height, spawnInfo.Radius * 2f));
            SetMember(controller, "center", Vector3.up * spawnInfo.Height * 0.5f);
            SetMember(controller, "maxSpeed", spawnInfo.MaxSpeed);
            SetMember(controller, "priority", Mathf.Clamp01(spawnInfo.Priority));
            SetMember(controller, "layer", spawnInfo.Layer);
            SetMember(controller, "collidesWith", spawnInfo.CollidesWith);
            SetMember(controller, "locked", spawnInfo.IsKinematicObstacle);
            SetMember(controller, "lockWhenNotMoving", spawnInfo.IsKinematicObstacle);
        }

        private Vector3 CalculateAvoidedVelocity(RvoAgentBinding binding, float deltaTime)
        {
            var agent = binding.Agent;
            var preferredVelocity = binding.PreferredVelocity;
            var position = agent.transform.position;
            var target = position + preferredVelocity;
            var speed = preferredVelocity.magnitude;

            if (setTargetMethod != null)
            {
                TryInvoke(
                    setTargetMethod,
                    binding.Controller,
                    new object[] { target, speed, agent.MaxSpeed, target });
            }

            if (calculateMovementDeltaMethod == null)
            {
                return preferredVelocity;
            }

            try
            {
                var delta = calculateMovementDeltaMethod.Invoke(
                    binding.Controller,
                    new object[] { position, deltaTime });
                if (delta is Vector3 movementDelta)
                {
                    return movementDelta / deltaTime;
                }
            }
            catch (Exception exception)
            {
                status = $"A* RVO 反射调用失败：{exception.GetBaseException().Message}";
            }

            return preferredVelocity;
        }

        private static void TryInvoke(MethodInfo method, object target, object[] parameters)
        {
            try
            {
                method.Invoke(target, parameters);
            }
            catch
            {
                // Keep the benchmark running even when an A* version changes a reflected member.
            }
        }

        private static void SetMember(object target, string name, object value)
        {
            if (target == null)
            {
                return;
            }

            var type = target.GetType();
            var property = type.GetProperty(name, MemberFlags);
            if (property != null && property.CanWrite)
            {
                TrySetValue(property.PropertyType, value, converted => property.SetValue(target, converted, null));
                return;
            }

            var field = type.GetField(name, MemberFlags);
            if (field != null)
            {
                TrySetValue(field.FieldType, value, converted => field.SetValue(target, converted));
            }
        }

        private static void TrySetValue(Type targetType, object value, Action<object> setter)
        {
            try
            {
                setter(ConvertValue(targetType, value));
            }
            catch
            {
                // Optional reflected configuration only; defaults are acceptable across A* versions.
            }
        }

        private static object ConvertValue(Type targetType, object value)
        {
            if (value == null || targetType.IsInstanceOfType(value))
            {
                return value;
            }

            if (targetType.IsEnum)
            {
                if (value is string enumName)
                {
                    return Enum.Parse(targetType, enumName);
                }

                return Enum.ToObject(targetType, value);
            }

            return Convert.ChangeType(value, targetType);
        }

        private static Type FindType(string fullName)
        {
            var direct = Type.GetType(fullName);
            if (direct != null)
            {
                return direct;
            }

            foreach (var assembly in AppDomain.CurrentDomain.GetAssemblies())
            {
                var type = assembly.GetType(fullName);
                if (type != null)
                {
                    return type;
                }
            }

            return null;
        }

        private sealed class RvoAgentBinding
        {
            public RvoAgentBinding(BenchmarkAgent agent, object controller)
            {
                Agent = agent;
                Controller = controller;
            }

            public BenchmarkAgent Agent { get; }
            public object Controller { get; }
            public Vector3 PreferredVelocity { get; set; }
        }
    }
}