diff --git a/FishROV/Assets/Scripts/Benchmark/AvoidanceBenchmarkTypes.cs b/FishROV/Assets/Scripts/Benchmark/AvoidanceBenchmarkTypes.cs index c519dd6..dee495c 100644 --- a/FishROV/Assets/Scripts/Benchmark/AvoidanceBenchmarkTypes.cs +++ b/FishROV/Assets/Scripts/Benchmark/AvoidanceBenchmarkTypes.cs @@ -8,7 +8,8 @@ namespace FishROV.AvoidanceBenchmark NoAvoidance = 0, AstarRvo = 1, Rvo2 = 2, - UnityNavMesh = 3 + UnityNavMesh = 3, + SamplingLocal = 4 } public enum AvoidanceScenarioKind diff --git a/FishROV/Assets/Scripts/Benchmark/AvoidanceCollisionProbe.cs b/FishROV/Assets/Scripts/Benchmark/AvoidanceCollisionProbe.cs new file mode 100644 index 0000000..6f99abc --- /dev/null +++ b/FishROV/Assets/Scripts/Benchmark/AvoidanceCollisionProbe.cs @@ -0,0 +1,141 @@ +using System.Collections.Generic; +using UnityEngine; + +namespace FishROV.AvoidanceBenchmark +{ + public sealed class AvoidanceCollisionProbe + { + private const float RefreshInterval = 0.25f; + + private readonly Dictionary> buckets = + new Dictionary>(1024); + + private float refreshTimer; + + public int HorizontalOverlapPairs { get; private set; } + public int SpatialOverlapPairs { get; private set; } + public float MinimumClearance { get; private set; } + + public void Update(IReadOnlyList agents, AvoidanceDimensionMode dimensionMode, float deltaTime) + { + refreshTimer -= deltaTime; + if (refreshTimer > 0f) + { + return; + } + + refreshTimer = RefreshInterval; + Recalculate(agents, dimensionMode); + } + + private void Recalculate(IReadOnlyList agents, AvoidanceDimensionMode dimensionMode) + { + HorizontalOverlapPairs = 0; + SpatialOverlapPairs = 0; + MinimumClearance = float.PositiveInfinity; + + buckets.Clear(); + var cellSize = CalculateCellSize(agents); + + for (var i = 0; i < agents.Count; i++) + { + var agent = agents[i]; + if (agent == null) + { + continue; + } + + var cell = ToCell(agent.transform.position, cellSize); + if (!buckets.TryGetValue(cell, out var bucket)) + { + bucket = new List(8); + buckets[cell] = bucket; + } + + bucket.Add(agent); + } + + for (var i = 0; i < agents.Count; i++) + { + var agent = agents[i]; + if (agent == null) + { + continue; + } + + var center = ToCell(agent.transform.position, cellSize); + for (var x = -1; x <= 1; x++) + { + for (var y = -1; y <= 1; y++) + { + var key = new Vector2Int(center.x + x, center.y + y); + if (!buckets.TryGetValue(key, out var bucket)) + { + continue; + } + + CountPairs(agent, bucket, dimensionMode); + } + } + } + + if (float.IsPositiveInfinity(MinimumClearance)) + { + MinimumClearance = 0f; + } + } + + private void CountPairs(BenchmarkAgent agent, List bucket, AvoidanceDimensionMode dimensionMode) + { + for (var i = 0; i < bucket.Count; i++) + { + var other = bucket[i]; + if (other == null || other.GetInstanceID() <= agent.GetInstanceID()) + { + continue; + } + + var combinedRadius = agent.Radius + other.Radius; + var delta = other.transform.position - agent.transform.position; + var flatDistance = new Vector2(delta.x, delta.z).magnitude; + var flatClearance = flatDistance - combinedRadius; + MinimumClearance = Mathf.Min(MinimumClearance, flatClearance); + + if (flatClearance < 0f) + { + HorizontalOverlapPairs++; + } + + var spatialDistance = dimensionMode == AvoidanceDimensionMode.XYZ3D + ? delta.magnitude + : new Vector3(delta.x, delta.y, delta.z).magnitude; + if (spatialDistance < combinedRadius) + { + SpatialOverlapPairs++; + } + } + } + + private static float CalculateCellSize(IReadOnlyList agents) + { + var maxRadius = 0.5f; + for (var i = 0; i < agents.Count; i++) + { + var agent = agents[i]; + if (agent != null) + { + maxRadius = Mathf.Max(maxRadius, agent.Radius); + } + } + + return Mathf.Max(0.5f, maxRadius * 2.2f); + } + + private static Vector2Int ToCell(Vector3 position, float cellSize) + { + return new Vector2Int( + Mathf.FloorToInt(position.x / cellSize), + Mathf.FloorToInt(position.z / cellSize)); + } + } +} diff --git a/FishROV/Assets/Scripts/Benchmark/AvoidanceCollisionProbe.cs.meta b/FishROV/Assets/Scripts/Benchmark/AvoidanceCollisionProbe.cs.meta new file mode 100644 index 0000000..3ae0a98 --- /dev/null +++ b/FishROV/Assets/Scripts/Benchmark/AvoidanceCollisionProbe.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: d012290bb8804b8ab9e2e0d7de017ced +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/FishROV/Assets/Scripts/Benchmark/AvoidancePerfBootstrap.cs b/FishROV/Assets/Scripts/Benchmark/AvoidancePerfBootstrap.cs index 6f1b7a8..5ec6ab4 100644 --- a/FishROV/Assets/Scripts/Benchmark/AvoidancePerfBootstrap.cs +++ b/FishROV/Assets/Scripts/Benchmark/AvoidancePerfBootstrap.cs @@ -17,6 +17,7 @@ namespace FishROV.AvoidanceBenchmark private readonly List agents = new List(512); private readonly AvoidanceScenarioController scenarioController = new AvoidanceScenarioController(); private readonly AvoidanceMetricsRecorder metrics = new AvoidanceMetricsRecorder(); + private readonly AvoidanceCollisionProbe collisionProbe = new AvoidanceCollisionProbe(); private AvoidanceBenchmarkConfig config; private IAvoidanceAdapter adapter; private Transform agentRoot; @@ -57,6 +58,7 @@ namespace FishROV.AvoidanceBenchmark agent.Integrate(Time.deltaTime, config.DimensionMode, config.ArenaRadius); } + collisionProbe.Update(agents, config.DimensionMode, Time.unscaledDeltaTime); metrics.EndSimulation(); } @@ -69,6 +71,7 @@ namespace FishROV.AvoidanceBenchmark GUILayout.Label($"对象数量：小鱼 {fishCount}，鲨鱼 {sharkCount}，总数 {agents.Count}"); GUILayout.Label($"帧率：平均 {metrics.AverageFps:F1} | 1% Low {metrics.OnePercentLowFps:F1}"); GUILayout.Label($"模拟耗时：{metrics.SimulationMs:F2} ms | GC：{metrics.GcAllocBytes / 1024f:F1} KB"); + GUILayout.Label($"碰撞调试：XZ重叠 {collisionProbe.HorizontalOverlapPairs} 对 | 3D重叠 {collisionProbe.SpatialOverlapPairs} 对 | 最近间距 {collisionProbe.MinimumClearance:F2}"); GUILayout.Label($"帧率限制：不锁帧，vSync={QualitySettings.vSyncCount}，targetFrameRate={Application.targetFrameRate}"); GUILayout.Space(8); @@ -345,6 +348,8 @@ namespace FishROV.AvoidanceBenchmark return "RVO2"; case AvoidanceFrameworkKind.UnityNavMesh: return "NavMesh"; + case AvoidanceFrameworkKind.SamplingLocal: + return "采样避障"; case AvoidanceScenarioKind.FreeSwim: return "自由巡游"; case AvoidanceScenarioKind.CrossFlow: diff --git a/FishROV/Assets/Scripts/Benchmark/FrameworkAdapters/AvoidanceAdapterRegistry.cs b/FishROV/Assets/Scripts/Benchmark/FrameworkAdapters/AvoidanceAdapterRegistry.cs index c3b3577..9cd96fb 100644 --- a/FishROV/Assets/Scripts/Benchmark/FrameworkAdapters/AvoidanceAdapterRegistry.cs +++ b/FishROV/Assets/Scripts/Benchmark/FrameworkAdapters/AvoidanceAdapterRegistry.cs @@ -14,6 +14,8 @@ namespace FishROV.AvoidanceBenchmark return new Rvo2Adapter(); case AvoidanceFrameworkKind.UnityNavMesh: return new UnityNavMeshAdapter(); + case AvoidanceFrameworkKind.SamplingLocal: + return new SamplingLocalAvoidanceAdapter(); default: return new UnavailableAvoidanceAdapter(kind); } diff --git a/FishROV/Assets/Scripts/Benchmark/FrameworkAdapters/Rvo2Adapter.cs b/FishROV/Assets/Scripts/Benchmark/FrameworkAdapters/Rvo2Adapter.cs index 0945e67..a3db57f 100644 --- a/FishROV/Assets/Scripts/Benchmark/FrameworkAdapters/Rvo2Adapter.cs +++ b/FishROV/Assets/Scripts/Benchmark/FrameworkAdapters/Rvo2Adapter.cs @@ -1,41 +1,54 @@ using System; using System.Collections.Generic; -using System.Reflection; using UnityEngine; namespace FishROV.AvoidanceBenchmark { public sealed class Rvo2Adapter : IAvoidanceAdapter { - private readonly Dictionary agentIds = new Dictionary(); - private Rvo2ReflectionBridge bridge; + private const float TimeHorizon = 2.5f; + private const float TimeHorizonObstacles = 2.5f; + private const float NeighbourDistanceMultiplier = 8f; + + private readonly Dictionary agentIds = new Dictionary(512); private AvoidanceBenchmarkConfig config; - private bool initialized; + private RVO.Simulator simulator; public string Name => "RVO2 本地避障"; - public bool IsAvailable => bridge != null && bridge.IsAvailable; - public string Status => bridge != null - ? bridge.Status - : Rvo2ReflectionBridge.MissingPackageStatus; + public bool IsAvailable => simulator != null; + public string Status => config != null && config.DimensionMode == AvoidanceDimensionMode.XYZ3D + ? $"RVO2-CS 已直接接入，当前 {agentIds.Count} 个对象；RVO2-CS 是 2D 算法，3D 压测会投影到 XZ 平面。" + : $"RVO2-CS 已直接接入，当前 {agentIds.Count} 个对象。"; public void Initialize(AvoidanceBenchmarkConfig config) { this.config = config.Clone(); agentIds.Clear(); - bridge = Rvo2ReflectionBridge.Create(this.config); - initialized = true; + simulator = RVO.Simulator.Instance; + simulator.Clear(); + simulator.NumWorkers = Math.Max(1, Environment.ProcessorCount - 1); + simulator.TimeStep = Time.fixedDeltaTime; } public BenchmarkAgent CreateAgent(BenchmarkAgentSpawnInfo spawnInfo, Transform parent) { var agent = BenchmarkAgent.CreatePrimitive(spawnInfo, parent); - - if (!initialized || bridge == null || !bridge.IsAvailable) + if (simulator == null) { return agent; } - var id = bridge.AddAgent(spawnInfo); + var neighbourDistance = Mathf.Max(spawnInfo.Radius * NeighbourDistanceMultiplier, spawnInfo.Radius * 2f); + var id = simulator.AddAgent( + ToRvoVector(spawnInfo.Position), + neighbourDistance, + Math.Max(1, config.MaxNeighbours), + TimeHorizon, + TimeHorizonObstacles, + Mathf.Max(0.01f, spawnInfo.Radius), + Mathf.Max(0.01f, spawnInfo.MaxSpeed), + new RVO.Vector2(0f, 0f)); + if (id >= 0) { agentIds[agent] = id; @@ -46,14 +59,14 @@ namespace FishROV.AvoidanceBenchmark public void SetPreferredVelocity(BenchmarkAgent agent, Vector3 preferredVelocity) { - if (bridge == null || !bridge.IsAvailable || !agentIds.TryGetValue(agent, out var id)) + if (simulator == null || !agentIds.TryGetValue(agent, out var id)) { agent.ApplyVelocity(preferredVelocity); return; } - bridge.SetAgentPosition(id, agent.transform.position); - bridge.SetPreferredVelocity(id, preferredVelocity); + simulator.SetAgentPosition(id, ToRvoVector(agent.transform.position)); + simulator.SetAgentPrefVelocity(id, ToRvoVector(Vector3.ClampMagnitude(preferredVelocity, agent.MaxSpeed))); } public void SetTarget(BenchmarkAgent agent, Vector3 target) @@ -63,474 +76,35 @@ namespace FishROV.AvoidanceBenchmark public void Tick(float deltaTime) { - if (bridge == null || !bridge.IsAvailable) + if (simulator == null || deltaTime <= 0f) { return; } - bridge.Step(deltaTime); + simulator.TimeStep = Mathf.Max(0.001f, deltaTime); + simulator.DoStep(); foreach (var pair in agentIds) { - pair.Key.ApplyVelocity(bridge.GetVelocity(pair.Value, pair.Key.MaxSpeed)); + pair.Key.ApplyVelocity(FromRvoVector(simulator.GetAgentVelocity(pair.Value), pair.Key.MaxSpeed)); } } public void Dispose() { agentIds.Clear(); - bridge?.Dispose(); - bridge = null; - initialized = false; + simulator?.Clear(); + simulator = null; } - private sealed class Rvo2ReflectionBridge + private static RVO.Vector2 ToRvoVector(Vector3 value) { - public const string MissingPackageStatus = - "未检测到 RVO2/RVO2-3D 源码或包；请把 C# 版本 RVO2 放到 Assets 或 Packages，并提供类似 RVO.Simulator 的 Simulator 类型。当前退回基线移动。"; + return new RVO.Vector2(value.x, value.z); + } - private static readonly string[] SimulatorTypeNames = - { - "RVO.Simulator", - "RVO2.Simulator", - "RVO3D.Simulator", - "RVO.Simulator3D" - }; - - private readonly object simulator; - private readonly Type simulatorType; - private readonly Type vector2Type; - private readonly Type vector3Type; - private readonly bool use3D; - private readonly string status; - private readonly MethodInfo addAgent; - private readonly MethodInfo setAgentPrefVelocity; - private readonly MethodInfo setAgentPosition; - private readonly MethodInfo doStep; - private readonly MethodInfo getAgentVelocity; - private readonly MethodInfo setTimeStep; - private readonly MethodInfo setAgentDefaults; - private readonly MethodInfo clear; - - private Rvo2ReflectionBridge( - object simulator, - Type simulatorType, - Type vector2Type, - Type vector3Type, - bool use3D, - string status, - MethodInfo addAgent, - MethodInfo setAgentPrefVelocity, - MethodInfo setAgentPosition, - MethodInfo doStep, - MethodInfo getAgentVelocity, - MethodInfo setTimeStep, - MethodInfo setAgentDefaults, - MethodInfo clear) - { - this.simulator = simulator; - this.simulatorType = simulatorType; - this.vector2Type = vector2Type; - this.vector3Type = vector3Type; - this.use3D = use3D; - this.status = status; - this.addAgent = addAgent; - this.setAgentPrefVelocity = setAgentPrefVelocity; - this.setAgentPosition = setAgentPosition; - this.doStep = doStep; - this.getAgentVelocity = getAgentVelocity; - this.setTimeStep = setTimeStep; - this.setAgentDefaults = setAgentDefaults; - this.clear = clear; - } - - public bool IsAvailable => simulator != null; - public string Status => status; - - public static Rvo2ReflectionBridge Create(AvoidanceBenchmarkConfig config) - { - var simulatorType = FindType(SimulatorTypeNames); - if (simulatorType == null) - { - return Unavailable(); - } - - var simulator = CreateOrGetSimulator(simulatorType); - if (simulator == null) - { - return Unavailable("已找到 RVO2 Simulator 类型，但无法取得单例实例，也没有可用的公开无参构造函数。当前退回基线移动。"); - } - - var vector2Type = FindType(simulatorType, "RVO.Vector2", "RVO2.Vector2"); - var vector3Type = FindType(simulatorType, "RVO.Vector3", "RVO3D.Vector3", "RVO2.Vector3"); - var use3D = config.DimensionMode == AvoidanceDimensionMode.XYZ3D && vector3Type != null; - var agentVectorType = use3D ? vector3Type : vector2Type; - if (agentVectorType == null) - { - return Unavailable("已找到 RVO2 Simulator 类型，但未找到可支持的 RVO.Vector2 或 RVO.Vector3 类型。当前退回基线移动。"); - } - - var addAgent = FindMethod(simulatorType, "addAgent", "AddAgent", new[] { agentVectorType }); - var setAgentPrefVelocity = FindMethod(simulatorType, "setAgentPrefVelocity", "SetAgentPrefVelocity", new[] { typeof(int), agentVectorType }); - var setAgentPosition = FindMethod(simulatorType, "setAgentPosition", "SetAgentPosition", new[] { typeof(int), agentVectorType }); - var getAgentVelocity = FindMethod(simulatorType, "getAgentVelocity", "GetAgentVelocity", new[] { typeof(int) }); - var doStep = FindMethod(simulatorType, "doStep", "DoStep", Type.EmptyTypes); - var setTimeStep = FindMethod(simulatorType, "setTimeStep", "SetTimeStep", new[] { typeof(float) }); - var setAgentDefaults = FindMethodByName(simulatorType, "setAgentDefaults", "SetAgentDefaults"); - var clear = FindMethodByName(simulatorType, "clear", "Clear", "reset", "Reset"); - - if (addAgent == null || setAgentPrefVelocity == null || getAgentVelocity == null || doStep == null) - { - return Unavailable("已找到 RVO2 Simulator 类型，但缺少 addAgent、setAgentPrefVelocity、getAgentVelocity 或 doStep 等必要方法。当前退回基线移动。"); - } - - var bridge = new Rvo2ReflectionBridge( - simulator, - simulatorType, - vector2Type, - vector3Type, - use3D, - use3D ? "RVO2-3D 已通过反射接入。" : "RVO2 已通过反射接入。", - addAgent, - setAgentPrefVelocity, - setAgentPosition, - doStep, - getAgentVelocity, - setTimeStep, - setAgentDefaults, - clear); - - bridge.Configure(config); - return bridge; - } - - public int AddAgent(BenchmarkAgentSpawnInfo spawnInfo) - { - try - { - var result = addAgent.Invoke(simulator, new[] { ToRvoVector(spawnInfo.Position) }); - return Convert.ToInt32(result); - } - catch (Exception exception) - { - Debug.LogWarning($"RVO2 添加 Agent 失败：{exception.Message}"); - return -1; - } - } - - public void SetAgentPosition(int id, Vector3 position) - { - if (setAgentPosition == null) - { - return; - } - - try - { - setAgentPosition.Invoke(simulator, new[] { id, ToRvoVector(position) }); - } - catch (Exception exception) - { - Debug.LogWarning($"RVO2 同步 Agent 位置失败：{exception.Message}"); - } - } - - public void SetPreferredVelocity(int id, Vector3 preferredVelocity) - { - try - { - setAgentPrefVelocity.Invoke(simulator, new[] { id, ToRvoVector(preferredVelocity) }); - } - catch (Exception exception) - { - Debug.LogWarning($"RVO2 设置期望速度失败：{exception.Message}"); - } - } - - public void Step(float deltaTime) - { - try - { - InvokeSetTimeStep(deltaTime); - doStep.Invoke(simulator, null); - } - catch (Exception exception) - { - Debug.LogWarning($"RVO2 模拟步进失败：{exception.Message}"); - } - } - - public Vector3 GetVelocity(int id, float maxSpeed) - { - try - { - var value = getAgentVelocity.Invoke(simulator, new object[] { id }); - return Vector3.ClampMagnitude(FromRvoVector(value), maxSpeed); - } - catch (Exception exception) - { - Debug.LogWarning($"RVO2 获取 Agent 速度失败：{exception.Message}"); - return Vector3.zero; - } - } - - public void Dispose() - { - try - { - clear?.Invoke(simulator, null); - } - catch (Exception exception) - { - Debug.LogWarning($"RVO2 清理/重置失败：{exception.Message}"); - } - } - - private void Configure(AvoidanceBenchmarkConfig config) - { - InvokeSetTimeStep(Time.fixedDeltaTime); - InvokeAgentDefaults(config); - } - - private void InvokeSetTimeStep(float deltaTime) - { - if (setTimeStep == null) - { - return; - } - - setTimeStep.Invoke(simulator, new object[] { Mathf.Max(0.001f, deltaTime) }); - } - - private void InvokeAgentDefaults(AvoidanceBenchmarkConfig config) - { - if (setAgentDefaults == null) - { - return; - } - - var parameters = setAgentDefaults.GetParameters(); - var values = new object[parameters.Length]; - for (var i = 0; i < parameters.Length; i++) - { - var parameterType = parameters[i].ParameterType; - var name = parameters[i].Name ?? string.Empty; - - if (parameterType == typeof(int)) - { - values[i] = config.MaxNeighbours; - } - else if (parameterType == typeof(float)) - { - values[i] = GuessDefaultFloat(name, config); - } - else if (parameterType == vector2Type) - { - values[i] = CreateVector(vector2Type, 0f, 0f, 0f); - } - else if (parameterType == vector3Type) - { - values[i] = CreateVector(vector3Type, 0f, 0f, 0f); - } - else - { - values[i] = parameterType.IsValueType ? Activator.CreateInstance(parameterType) : null; - } - } - - setAgentDefaults.Invoke(simulator, values); - } - - private static float GuessDefaultFloat(string parameterName, AvoidanceBenchmarkConfig config) - { - var lower = parameterName.ToLowerInvariant(); - if (lower.Contains("radius")) - { - return config.FishRadius; - } - - if (lower.Contains("maxspeed") || lower.Contains("max_speed")) - { - return config.FishSpeed; - } - - if (lower.Contains("neigh") || lower.Contains("dist")) - { - return config.FishRadius * 8f; - } - - if (lower.Contains("horizon")) - { - return 2.5f; - } - - return 0f; - } - - private object ToRvoVector(Vector3 value) - { - if (use3D && vector3Type != null) - { - return CreateVector(vector3Type, value.x, value.y, value.z); - } - - return CreateVector(vector2Type, value.x, value.z, 0f); - } - - private Vector3 FromRvoVector(object value) - { - if (value == null) - { - return Vector3.zero; - } - - var type = value.GetType(); - if (use3D && type == vector3Type) - { - return new Vector3(GetFloatMember(value, "x", "X"), GetFloatMember(value, "y", "Y"), GetFloatMember(value, "z", "Z")); - } - - return new Vector3(GetFloatMember(value, "x", "X"), 0f, GetFloatMember(value, "y", "Y")); - } - - private static object CreateVector(Type type, float x, float y, float z) - { - var constructor = type.GetConstructor(new[] { typeof(float), typeof(float), typeof(float) }); - if (constructor != null) - { - return constructor.Invoke(new object[] { x, y, z }); - } - - constructor = type.GetConstructor(new[] { typeof(float), typeof(float) }); - if (constructor != null) - { - return constructor.Invoke(new object[] { x, y }); - } - - var value = Activator.CreateInstance(type); - SetFloatMember(value, x, "x", "X"); - SetFloatMember(value, y, "y", "Y"); - SetFloatMember(value, z, "z", "Z"); - return value; - } - - private static float GetFloatMember(object value, params string[] names) - { - var type = value.GetType(); - foreach (var name in names) - { - var field = type.GetField(name, BindingFlags.Instance | BindingFlags.Public | BindingFlags.NonPublic); - if (field != null) - { - return Convert.ToSingle(field.GetValue(value)); - } - - var property = type.GetProperty(name, BindingFlags.Instance | BindingFlags.Public | BindingFlags.NonPublic); - if (property != null) - { - return Convert.ToSingle(property.GetValue(value, null)); - } - } - - return 0f; - } - - private static void SetFloatMember(object value, float memberValue, params string[] names) - { - var type = value.GetType(); - foreach (var name in names) - { - var field = type.GetField(name, BindingFlags.Instance | BindingFlags.Public | BindingFlags.NonPublic); - if (field != null) - { - field.SetValue(value, memberValue); - return; - } - - var property = type.GetProperty(name, BindingFlags.Instance | BindingFlags.Public | BindingFlags.NonPublic); - if (property != null && property.CanWrite) - { - property.SetValue(value, memberValue, null); - return; - } - } - } - - private static Rvo2ReflectionBridge Unavailable(string status = MissingPackageStatus) - { - return new Rvo2ReflectionBridge(null, null, null, null, false, status, null, null, null, null, null, null, null, null); - } - - private static object CreateOrGetSimulator(Type type) - { - var instanceProperty = type.GetProperty("Instance", BindingFlags.Public | BindingFlags.Static); - if (instanceProperty != null) - { - return instanceProperty.GetValue(null, null); - } - - var instanceField = type.GetField("Instance", BindingFlags.Public | BindingFlags.Static); - if (instanceField != null) - { - return instanceField.GetValue(null); - } - - var constructor = type.GetConstructor(Type.EmptyTypes); - return constructor != null ? constructor.Invoke(null) : null; - } - - private static Type FindType(params string[] typeNames) - { - foreach (var assembly in AppDomain.CurrentDomain.GetAssemblies()) - { - foreach (var typeName in typeNames) - { - var type = assembly.GetType(typeName, false); - if (type != null) - { - return type; - } - } - } - - return null; - } - - private static Type FindType(Type anchorType, params string[] typeNames) - { - foreach (var typeName in typeNames) - { - var type = anchorType.Assembly.GetType(typeName, false); - if (type != null) - { - return type; - } - } - - return FindType(typeNames); - } - - private static MethodInfo FindMethod(Type type, string camelName, string pascalName, Type[] parameterTypes) - { - return type.GetMethod(camelName, BindingFlags.Instance | BindingFlags.Public, null, parameterTypes, null) - ?? type.GetMethod(pascalName, BindingFlags.Instance | BindingFlags.Public, null, parameterTypes, null); - } - - private static MethodInfo FindMethodByName(Type type, params string[] names) - { - foreach (var name in names) - { - foreach (var method in type.GetMethods(BindingFlags.Instance | BindingFlags.Public)) - { - if (method.Name == name) - { - return method; - } - } - } - - return null; - } + private static Vector3 FromRvoVector(RVO.Vector2 value, float maxSpeed) + { + return Vector3.ClampMagnitude(new Vector3(value.X, 0f, value.Y), maxSpeed); } } } diff --git a/FishROV/Assets/Scripts/Benchmark/FrameworkAdapters/SamplingLocalAvoidanceAdapter.cs b/FishROV/Assets/Scripts/Benchmark/FrameworkAdapters/SamplingLocalAvoidanceAdapter.cs new file mode 100644 index 0000000..760bca3 --- /dev/null +++ b/FishROV/Assets/Scripts/Benchmark/FrameworkAdapters/SamplingLocalAvoidanceAdapter.cs @@ -0,0 +1,326 @@ +using System.Collections.Generic; +using UnityEngine; + +namespace FishROV.AvoidanceBenchmark +{ + public sealed class SamplingLocalAvoidanceAdapter : IAvoidanceAdapter + { + private const float TimeHorizon = 1.25f; + private const float NeighbourRangeMultiplier = 5.5f; + private const float GoalWeight = 0.85f; + private const float SmoothWeight = 0.12f; + private const float CollisionWeight = 18f; + private const float CurrentOverlapWeight = 80f; + private const float BoundaryWeight = 12f; + private const int DirectionSamples = 16; + + private readonly Dictionary states = + new Dictionary(512); + + private readonly List orderedStates = new List(512); + private readonly Dictionary> buckets = + new Dictionary>(1024); + + private AvoidanceBenchmarkConfig config; + private float cellSize; + + public string Name => "采样避障（学习版）"; + public bool IsAvailable => true; + public string Status => "学习版：空间哈希找邻居，采样候选速度，用预测碰撞/平滑/目标偏差/边界惩罚打分。"; + + public void Initialize(AvoidanceBenchmarkConfig config) + { + this.config = config.Clone(); + states.Clear(); + orderedStates.Clear(); + buckets.Clear(); + cellSize = Mathf.Max(config.SharkRadius * 2.5f, config.FishRadius * NeighbourRangeMultiplier); + } + + public BenchmarkAgent CreateAgent(BenchmarkAgentSpawnInfo spawnInfo, Transform parent) + { + var agent = BenchmarkAgent.CreatePrimitive(spawnInfo, parent); + var state = new AgentState(agent); + states[agent] = state; + orderedStates.Add(state); + return agent; + } + + public void SetPreferredVelocity(BenchmarkAgent agent, Vector3 preferredVelocity) + { + if (!states.TryGetValue(agent, out var state)) + { + agent.ApplyVelocity(preferredVelocity); + return; + } + + state.PreferredVelocity = ClampForMode(preferredVelocity, agent.MaxSpeed); + } + + public void SetTarget(BenchmarkAgent agent, Vector3 target) + { + agent.Target = target; + } + + public void Tick(float deltaTime) + { + if (deltaTime <= 0f) + { + return; + } + + BuildBuckets(); + + for (var i = 0; i < orderedStates.Count; i++) + { + var state = orderedStates[i]; + if (state.Agent == null) + { + continue; + } + + var velocity = ChooseVelocity(state, deltaTime); + state.Agent.ApplyVelocity(velocity); + state.LastVelocity = velocity; + } + } + + public void Dispose() + { + states.Clear(); + orderedStates.Clear(); + buckets.Clear(); + } + + private Vector3 ChooseVelocity(AgentState state, float deltaTime) + { + var preferred = state.PreferredVelocity; + var bestVelocity = preferred; + var bestScore = EvaluateVelocity(state, preferred, deltaTime); + + TestCandidate(state, Vector3.zero, deltaTime, ref bestVelocity, ref bestScore); + + var flatPreferred = Flatten(preferred); + var baseDirection = flatPreferred.sqrMagnitude > 0.0001f + ? flatPreferred.normalized + : ForwardFromAgent(state.Agent); + + var maxSpeed = state.Agent.MaxSpeed; + for (var i = 0; i < DirectionSamples; i++) + { + var angle = i * 360f / DirectionSamples; + var direction = Quaternion.AngleAxis(angle, Vector3.up) * baseDirection; + TestCandidate(state, RestoreVertical(direction * maxSpeed, preferred), deltaTime, ref bestVelocity, ref bestScore); + TestCandidate(state, RestoreVertical(direction * maxSpeed * 0.55f, preferred), deltaTime, ref bestVelocity, ref bestScore); + } + + return ClampForMode(bestVelocity, maxSpeed); + } + + private void TestCandidate( + AgentState state, + Vector3 candidate, + float deltaTime, + ref Vector3 bestVelocity, + ref float bestScore) + { + var score = EvaluateVelocity(state, candidate, deltaTime); + if (score < bestScore) + { + bestScore = score; + bestVelocity = candidate; + } + } + + private float EvaluateVelocity(AgentState state, Vector3 candidate, float deltaTime) + { + candidate = ClampForMode(candidate, state.Agent.MaxSpeed); + var preferred = state.PreferredVelocity; + var score = GoalWeight * (candidate - preferred).sqrMagnitude; + score += SmoothWeight * (candidate - state.LastVelocity).sqrMagnitude; + score += BoundaryPenalty(state.Agent.transform.position, candidate, deltaTime); + + var position = state.Agent.transform.position; + var centerCell = ToCell(position); + var queryRange = Mathf.CeilToInt( + Mathf.Max(state.Agent.Radius * NeighbourRangeMultiplier, state.Agent.Radius + config.SharkRadius) / cellSize); + queryRange = Mathf.Clamp(queryRange, 1, 4); + + for (var x = -queryRange; x <= queryRange; x++) + { + for (var z = -queryRange; z <= queryRange; z++) + { + var cell = new Vector2Int(centerCell.x + x, centerCell.y + z); + if (!buckets.TryGetValue(cell, out var bucket)) + { + continue; + } + + for (var i = 0; i < bucket.Count; i++) + { + var other = bucket[i]; + if (other == state || other.Agent == null) + { + continue; + } + + score += CollisionPenalty(state, other, candidate); + } + } + } + + return score; + } + + private float CollisionPenalty(AgentState state, AgentState other, Vector3 candidate) + { + var position = state.Agent.transform.position; + var otherPosition = other.Agent.transform.position; + var relativePosition = otherPosition - position; + var combinedRadius = state.Agent.Radius + other.Agent.Radius; + var neighbourRange = Mathf.Max(combinedRadius * 2f, state.Agent.Radius * NeighbourRangeMultiplier); + + if (config.DimensionMode != AvoidanceDimensionMode.XYZ3D) + { + relativePosition.y = 0f; + } + + var distanceSq = relativePosition.sqrMagnitude; + if (distanceSq > neighbourRange * neighbourRange) + { + return 0f; + } + + var relativeVelocity = candidate - other.LastVelocity; + if (config.DimensionMode != AvoidanceDimensionMode.XYZ3D) + { + relativeVelocity.y = 0f; + } + + var clearanceRadius = combinedRadius * 1.08f; + var currentDistance = Mathf.Sqrt(Mathf.Max(0.0001f, distanceSq)); + var currentClearance = currentDistance - clearanceRadius; + var penalty = 0f; + + if (currentClearance < 0f) + { + var overlap = -currentClearance / clearanceRadius; + penalty += CurrentOverlapWeight * overlap * overlap; + } + + var relativeSpeedSq = relativeVelocity.sqrMagnitude; + if (relativeSpeedSq < 0.0001f) + { + return penalty; + } + + var closestTime = Mathf.Clamp( + -Vector3.Dot(relativePosition, relativeVelocity) / relativeSpeedSq, + 0f, + TimeHorizon); + var closest = relativePosition + relativeVelocity * closestTime; + var closestDistance = closest.magnitude; + var predictedClearance = closestDistance - clearanceRadius; + if (predictedClearance < 0f) + { + var risk = -predictedClearance / clearanceRadius; + var urgency = 1f + (TimeHorizon - closestTime) / TimeHorizon; + penalty += CollisionWeight * risk * risk * urgency; + } + + return penalty; + } + + private float BoundaryPenalty(Vector3 position, Vector3 velocity, float deltaTime) + { + var next = position + velocity * deltaTime * TimeHorizon; + var flat = new Vector2(next.x, next.z); + var softRadius = config.ArenaRadius * 0.92f; + if (flat.magnitude <= softRadius) + { + return 0f; + } + + var overflow = (flat.magnitude - softRadius) / Mathf.Max(0.001f, config.ArenaRadius - softRadius); + return BoundaryWeight * overflow * overflow; + } + + private void BuildBuckets() + { + buckets.Clear(); + for (var i = 0; i < orderedStates.Count; i++) + { + var state = orderedStates[i]; + if (state.Agent == null) + { + continue; + } + + var cell = ToCell(state.Agent.transform.position); + if (!buckets.TryGetValue(cell, out var bucket)) + { + bucket = new List(8); + buckets[cell] = bucket; + } + + bucket.Add(state); + } + } + + private Vector2Int ToCell(Vector3 position) + { + return new Vector2Int( + Mathf.FloorToInt(position.x / cellSize), + Mathf.FloorToInt(position.z / cellSize)); + } + + private Vector3 ClampForMode(Vector3 velocity, float maxSpeed) + { + if (config.DimensionMode != AvoidanceDimensionMode.XYZ3D) + { + velocity.y = 0f; + } + + return Vector3.ClampMagnitude(velocity, maxSpeed); + } + + private Vector3 RestoreVertical(Vector3 flatVelocity, Vector3 preferred) + { + if (config.DimensionMode == AvoidanceDimensionMode.XYZ3D) + { + flatVelocity.y = preferred.y; + } + else + { + flatVelocity.y = 0f; + } + + return flatVelocity; + } + + private static Vector3 Flatten(Vector3 value) + { + value.y = 0f; + return value; + } + + private static Vector3 ForwardFromAgent(BenchmarkAgent agent) + { + var forward = agent != null ? agent.transform.forward : Vector3.forward; + forward.y = 0f; + return forward.sqrMagnitude > 0.0001f ? forward.normalized : Vector3.forward; + } + + private sealed class AgentState + { + public AgentState(BenchmarkAgent agent) + { + Agent = agent; + } + + public BenchmarkAgent Agent { get; } + public Vector3 PreferredVelocity { get; set; } + public Vector3 LastVelocity { get; set; } + } + } +} diff --git a/FishROV/Assets/Scripts/Benchmark/FrameworkAdapters/SamplingLocalAvoidanceAdapter.cs.meta b/FishROV/Assets/Scripts/Benchmark/FrameworkAdapters/SamplingLocalAvoidanceAdapter.cs.meta new file mode 100644 index 0000000..e9a077b --- /dev/null +++ b/FishROV/Assets/Scripts/Benchmark/FrameworkAdapters/SamplingLocalAvoidanceAdapter.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 41e3ccd900cb4a37bf60c67fa49c906c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/FishROV/Assets/Scripts/Benchmark/FrameworkAdapters/UnityNavMeshAdapter.cs b/FishROV/Assets/Scripts/Benchmark/FrameworkAdapters/UnityNavMeshAdapter.cs index 040838c..9820d9f 100644 --- a/FishROV/Assets/Scripts/Benchmark/FrameworkAdapters/UnityNavMeshAdapter.cs +++ b/FishROV/Assets/Scripts/Benchmark/FrameworkAdapters/UnityNavMeshAdapter.cs @@ -104,7 +104,11 @@ namespace FishROV.AvoidanceBenchmark continue; } - benchmarkAgent.ApplyVelocity(navAgent.velocity); + var smoothedVelocity = Vector3.MoveTowards( + benchmarkAgent.Velocity, + navAgent.velocity, + navAgent.acceleration * deltaTime); + benchmarkAgent.ApplyVelocity(smoothedVelocity); navAgent.nextPosition = benchmarkAgent.transform.position; } } diff --git a/FishROV/Docs/local-avoidance-benchmark-notes.md b/FishROV/Docs/local-avoidance-benchmark-notes.md index 7d362b7..4566912 100644 --- a/FishROV/Docs/local-avoidance-benchmark-notes.md +++ b/FishROV/Docs/local-avoidance-benchmark-notes.md @@ -16,6 +16,7 @@ - 小鱼数量预设和滑条 - 暂停 / 继续 / 重置 - 平均 FPS、1% Low FPS、模拟耗时、GC 分配 + - 碰撞调试：XZ 平面重叠对数、3D 空间重叠对数、最近清空间距 - 表现对象：鱼群为运行时圆锥，圆锥尖头表示朝向；鲨鱼为 Capsule。 - 共享场景： - 自由巡游 @@ -45,7 +46,7 @@ adapter 可以给运行时创建的对象追加框架自己的组件，但不应 `NoAvoidanceAdapter` 是对照基线：只按期望速度移动，不做避障。 -A* RVO、RVO2、Unity NavMesh 都通过同一个调试面板选择，用同一套场景、数量、相机和指标进行对比。 +A* RVO、RVO2、Unity NavMesh、采样避障学习版都通过同一个调试面板选择，用同一套场景、数量、相机和指标进行对比。 ## A* Pathfinding Project Pro RVO Adapter @@ -58,14 +59,11 @@ A* RVO、RVO2、Unity NavMesh 都通过同一个调试面板选择，用同一 ## RVO2 Adapter -`Rvo2Adapter` 是反射 adapter。当前已在 `Assets/ThirdParty/RVO2-CS/` 导入 `snape/RVO2-CS` 的核心 `RVOCS` 源码，并保留 Apache-2.0 许可证说明。 +当前已在 `Assets/ThirdParty/RVO2-CS/` 导入 `snape/RVO2-CS` 的核心 `RVOCS` 源码，并保留 Apache-2.0 许可证说明。 -- 选择 `RVO2` 时，adapter 会优先命中导入后的 `RVO.Simulator`。 -- 如果之后移除 RVO2 源码，工程仍可编译；Game 视口状态会提示缺包，并退回基线移动。 -- adapter 会查找类似 `RVO.Simulator`、`RVO2.Simulator`、`RVO3D.Simulator`、`RVO.Simulator3D` 的 Simulator 类型。 -- 必要方法包括 `addAgent` / `AddAgent`、`setAgentPrefVelocity` / `SetAgentPrefVelocity`、`getAgentVelocity` / `GetAgentVelocity`、`doStep` / `DoStep`。 -- 如果存在 `setAgentPosition` / `SetAgentPosition`，adapter 每次设置期望速度前会同步 Unity Transform 位置，避免 RVO 内部位置和 benchmark 边界夹取结果漂移。 -- 2D 和 2.5D 使用 RVO 的 Vector2 风格类型，把 Unity `x/z` 映射到 RVO `x/y`；3D 模式在可用时绑定 Vector3 风格类型。 +- `Rvo2Adapter` 已从反射桥改为直接调用 `RVO.Simulator`，避免每个 agent 每帧多次反射调用污染性能对比。 +- 每帧设置期望速度前会同步 Unity Transform 位置，避免 RVO 内部位置和 benchmark 边界夹取结果漂移。 +- RVO2-CS 是 2D 算法，2D 和 2.5D 使用 Unity `x/z` 到 RVO `x/y` 的映射；3D 模式会投影到 XZ 平面。 ## Unity NavMesh Adapter @@ -73,4 +71,14 @@ A* RVO、RVO2、Unity NavMesh 都通过同一个调试面板选择，用同一 - adapter 会使用 `NavMeshBuilder.BuildNavMeshData` 为 benchmark 场景运行时生成一张平面 NavMesh，不需要手动烘焙场景。 - 如果运行时 NavMesh 构建失败，Game 视口状态会提示，并退回基线移动，不会添加无效 `NavMeshAgent`。 +- NavMeshAgent 计算出的速度会按自身 `acceleration` 平滑后再交给 benchmark 可视对象，减少高密度场景的一帧速度尖峰抖动。 - 因为 NavMesh 是平面数据，3D 压测更适合作为 2D / 2.5D 对照；完整 3D 体积避障仍需要专门方案。 + +## 采样避障学习版 + +`SamplingLocalAvoidanceAdapter` 是项目内自写的学习算法，用来和成熟框架做横向对比。 + +- 使用空间哈希查找近邻，避免全量 O(n²) 扫描。 +- 对每个 agent 采样多组候选速度，并用目标偏差、速度平滑、预测碰撞、当前重叠、边界越界惩罚打分。 +- 它不是完整 ORCA，也不保证无碰撞；目的在于提供一个可读、可改、可调参的学习基线。 +- 如果它在某些场景比 RVO2 或 NavMesh 更稳，只能说明当前启发式适合该场景，不代表算法理论上更强。