319 lines
12 KiB
C#
319 lines
12 KiB
C#
using DotRecast.Core;
|
||
using DotRecast.Core.Numerics;
|
||
using DotRecast.Detour;
|
||
using DotRecast.Detour.Crowd;
|
||
using System.Collections.Concurrent;
|
||
using System.Numerics;
|
||
|
||
namespace XNet.Business.PathNavigation
|
||
{
|
||
public class SceneAgent
|
||
{
|
||
private readonly NavMeshManager _navMeshManager;
|
||
|
||
private class CrowdInstance
|
||
{
|
||
public string InstanceId { get; }
|
||
public DtCrowd Crowd { get; set; }
|
||
//public object SyncRoot { get; } = new object(); // 线程锁
|
||
|
||
// 新增：存储该实例下所有 Agent 的索引（关键修复）
|
||
public ConcurrentDictionary<int, bool> AgentIndices { get; } = new ConcurrentDictionary<int, bool>();
|
||
|
||
// 构造函数
|
||
public CrowdInstance(string instanceId, DtCrowd crowd)
|
||
{
|
||
InstanceId = instanceId;
|
||
Crowd = crowd;
|
||
}
|
||
|
||
// 辅助：添加 Agent 时自动记录索引
|
||
public void AddAgentIndex(int agentIdx)
|
||
{
|
||
//lock (SyncRoot)
|
||
//{
|
||
//if (!AgentIndices.ContainsKey(agentIdx))
|
||
//{
|
||
AgentIndices.TryAdd(agentIdx, true);
|
||
//}
|
||
//}
|
||
}
|
||
|
||
// 辅助：移除 Agent 时清理索引
|
||
public void RemoveAgentIndex(int agentIdx)
|
||
{
|
||
//lock (SyncRoot)
|
||
//{
|
||
AgentIndices.TryRemove(agentIdx, out _);
|
||
//}
|
||
}
|
||
}
|
||
|
||
private readonly ConcurrentDictionary<string, CrowdInstance> _crowdInstances = new();
|
||
|
||
//private const float AGENT_RADIUS = 0.5f;
|
||
//private const float AGENT_HEIGHT = 2.0f;
|
||
|
||
// 新增：Agent状态缓存（实例ID -> AgentIdx -> 上一帧状态）
|
||
private readonly ConcurrentDictionary<string, ConcurrentDictionary<int, AgentState>> _agentLastState = new();
|
||
// 同步阈值（可配置）
|
||
private const float POSITION_THRESHOLD = 0.01f; // 位置变化超过0.01米才同步
|
||
private const float ROTATION_THRESHOLD = 0.017f; // 旋转变化超过1度（0.017弧度）才同步
|
||
|
||
// Agent状态结构体
|
||
private struct AgentState
|
||
{
|
||
public Vector3 Position;
|
||
public float Rotation; // 绕Y轴旋转（弧度）
|
||
}
|
||
|
||
|
||
public SceneAgent(NavMeshManager navMeshManager)
|
||
{
|
||
_navMeshManager = navMeshManager;
|
||
}
|
||
|
||
// ==========================================
|
||
// 修复4：GetAgentsNeedSync 遍历 AgentIndices
|
||
// ==========================================
|
||
public List<AgentPositionSyncMsg> GetAgentsNeedSync(string instanceId)
|
||
{
|
||
List<AgentPositionSyncMsg> syncList = new();
|
||
if (!_crowdInstances.TryGetValue(instanceId, out var ci) || !_agentLastState.TryGetValue(instanceId, out var lastStates))
|
||
{
|
||
return syncList;
|
||
}
|
||
|
||
//lock (ci.SyncRoot)
|
||
//{
|
||
// 遍历实例内所有Agent索引（修复AgentIndices报错）
|
||
foreach (var kv in ci.AgentIndices)
|
||
{
|
||
int agentIdx = kv.Key;
|
||
var agent = ci.Crowd.GetAgent(agentIdx);
|
||
if (agent == null || agent.state == DtCrowdAgentState.DT_CROWDAGENT_STATE_INVALID)
|
||
{
|
||
continue;
|
||
}
|
||
|
||
// 获取当前状态
|
||
Vector3 currPos = GetAgentPosition(instanceId, agentIdx);
|
||
float currRot = GetAgentRotation(instanceId, agentIdx);
|
||
|
||
// 对比上一帧状态
|
||
if (lastStates.TryGetValue(agentIdx, out var lastState))
|
||
{
|
||
// 判断是否超过阈值
|
||
bool posChanged = Vector3.Distance(currPos, lastState.Position) > POSITION_THRESHOLD;
|
||
bool rotChanged = Math.Abs(currRot - lastState.Rotation) > ROTATION_THRESHOLD;
|
||
|
||
if (posChanged || rotChanged)
|
||
{
|
||
// 加入同步列表
|
||
syncList.Add(new AgentPositionSyncMsg
|
||
{
|
||
InstanceId = instanceId,
|
||
AgentIdx = agentIdx,
|
||
Position = new Vector3Msg { X = currPos.X, Y = currPos.Y, Z = currPos.Z },
|
||
Rotation = currRot
|
||
});
|
||
// 更新缓存
|
||
lastStates[agentIdx] = new AgentState { Position = currPos, Rotation = currRot };
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// 首次同步，直接加入并缓存
|
||
syncList.Add(new AgentPositionSyncMsg
|
||
{
|
||
InstanceId = instanceId,
|
||
AgentIdx = agentIdx,
|
||
Position = new Vector3Msg { X = currPos.X, Y = currPos.Y, Z = currPos.Z },
|
||
Rotation = currRot
|
||
});
|
||
lastStates.TryAdd(agentIdx, new AgentState { Position = currPos, Rotation = currRot });
|
||
}
|
||
}
|
||
//}
|
||
|
||
return syncList;
|
||
}
|
||
|
||
// ===============================================
|
||
// 修复2：重写 GetAgentRotation（通过速度计算朝向）
|
||
// ===============================================
|
||
public float GetAgentRotation(string instanceId, int agentIdx)
|
||
{
|
||
if (!_crowdInstances.TryGetValue(instanceId, out var ci))
|
||
{
|
||
return 0f;
|
||
}
|
||
|
||
//lock (ci.SyncRoot)
|
||
//{
|
||
var agent = ci.Crowd.GetAgent(agentIdx);
|
||
if (agent == null || agent.state == DtCrowdAgentState.DT_CROWDAGENT_STATE_INVALID)
|
||
{
|
||
return 0f;
|
||
}
|
||
|
||
// 核心逻辑：通过Agent的速度向量计算绕Y轴的旋转角度（弧度）
|
||
// 速度向量 (vx, vz) -> 朝向角度 = atan2(vz, vx)
|
||
float vx = agent.vel.X;
|
||
float vz = -agent.vel.Z; // 和坐标转换一致，Z轴取反
|
||
float rotation = 0f;
|
||
|
||
// 只有速度大于阈值时才计算朝向（避免静止时角度抖动）
|
||
if (Math.Abs(vx) > 0.001f || Math.Abs(vz) > 0.001f)
|
||
{
|
||
rotation = (float)Math.Atan2(vz, vx); // 计算弧度（范围 -π ~ π）
|
||
}
|
||
|
||
return rotation;
|
||
//}
|
||
}
|
||
|
||
|
||
// 初始化实例的Agent状态缓存
|
||
|
||
public bool CreateCrowdForInstance(string instanceId, float maxAgentRadius = 0.1f)
|
||
{
|
||
// 使用 NavMeshManager 新增的 Helper 方法
|
||
if (!_navMeshManager.GetNavMeshAndQuery(instanceId, out var navMesh, out _))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
var config = new DtCrowdConfig(maxAgentRadius);
|
||
var crowd = new DtCrowd(config, navMesh);
|
||
|
||
var paramsData = crowd.GetObstacleAvoidanceParams(0);
|
||
paramsData.velBias = 0.4f;
|
||
paramsData.adaptiveDivs = 5;
|
||
paramsData.adaptiveRings = 2;
|
||
paramsData.adaptiveDepth = 2;
|
||
crowd.SetObstacleAvoidanceParams(0, paramsData);
|
||
|
||
|
||
// 添加到实例字典
|
||
bool isAddCrowdOK = _crowdInstances.TryAdd(instanceId, new CrowdInstance(instanceId, crowd));
|
||
// 初始化状态缓存
|
||
bool isAddStateOK = _agentLastState.TryAdd(instanceId, new ConcurrentDictionary<int, AgentState>());
|
||
|
||
return isAddCrowdOK && isAddStateOK;
|
||
}
|
||
|
||
public void RemoveCrowdInstance(string instanceId)
|
||
{
|
||
_crowdInstances.TryRemove(instanceId, out _);
|
||
}
|
||
|
||
public int AddAgent(string instanceId, Vector3 position, float radius, float height, float maxAcceleration = 8.0f, float maxSpeed = 3.5f)
|
||
{
|
||
if (!_crowdInstances.TryGetValue(instanceId, out var ci))
|
||
{
|
||
throw new ArgumentException($"实例 {instanceId} 不存在");
|
||
}
|
||
|
||
//lock (ci.SyncRoot)
|
||
//{
|
||
// 创建Agent（原有逻辑）
|
||
var agentParams = new DtCrowdAgentParams();
|
||
agentParams.radius = radius;
|
||
agentParams.height = height;
|
||
agentParams.maxAcceleration = 2.0f;
|
||
agentParams.maxSpeed = 3.0f;
|
||
agentParams.collisionQueryRange = radius * 8.0f;
|
||
agentParams.pathOptimizationRange = radius * 32.0f;
|
||
agentParams.separationWeight = 1.0f;
|
||
|
||
var rcPos = new RcVec3f(position.X, position.Y, -position.Z); // 坐标转换（和之前一致）
|
||
int agentIdx = ci.Crowd.AddAgent(rcPos, agentParams).idx;
|
||
|
||
// 关键：记录Agent索引到实例的AgentIndices
|
||
ci.AddAgentIndex(agentIdx);
|
||
|
||
// 初始化状态缓存
|
||
_agentLastState.GetOrAdd(instanceId, _ => new ConcurrentDictionary<int, AgentState>())
|
||
.TryAdd(agentIdx, new AgentState { Position = position, Rotation = 0f });
|
||
|
||
return agentIdx;
|
||
//}
|
||
}
|
||
|
||
public void RemoveAgent(string instanceId, int agentIdx)
|
||
{
|
||
if (!_crowdInstances.TryGetValue(instanceId, out var ci)) return;
|
||
|
||
//lock (ci.SyncRoot)
|
||
//{
|
||
var agent = ci.Crowd.GetAgent(agentIdx);
|
||
// 【修复】判断 Agent 是否有效，需要检查 state
|
||
if (agent != null && agent.state != DtCrowdAgentState.DT_CROWDAGENT_STATE_INVALID)
|
||
{
|
||
ci.Crowd.RemoveAgent(agent);
|
||
}
|
||
//}
|
||
}
|
||
|
||
public bool AgentGoto(string instanceId, int agentIdx, Vector3 destination)
|
||
{
|
||
// 原有逻辑（无需修改）
|
||
if (!_crowdInstances.TryGetValue(instanceId, out var ci)) return false;
|
||
if (!_navMeshManager.GetNavMeshAndQuery(instanceId, out _, out var query)) return false;
|
||
|
||
var targetPos = new RcVec3f(destination.X, destination.Y, -destination.Z);
|
||
query.FindNearestPoly(targetPos, _navMeshManager.Extents, _navMeshManager.Filter,
|
||
out long targetRef, out var realTargetPos, out var _);
|
||
|
||
if (targetRef == 0) return false;
|
||
|
||
//lock (ci.SyncRoot)
|
||
//{
|
||
var agent = ci.Crowd.GetAgent(agentIdx);
|
||
if (agent != null && agent.state != DtCrowdAgentState.DT_CROWDAGENT_STATE_INVALID)
|
||
{
|
||
return ci.Crowd.RequestMoveTarget(agent, targetRef, realTargetPos);
|
||
}
|
||
//}
|
||
return false;
|
||
}
|
||
|
||
// ==========================================
|
||
// 修复3：GetAgentPosition 确保坐标转换一致
|
||
// ==========================================
|
||
public Vector3 GetAgentPosition(string instanceId, int agentIdx)
|
||
{
|
||
if (!_crowdInstances.TryGetValue(instanceId, out var ci))
|
||
{
|
||
return Vector3.Zero;
|
||
}
|
||
|
||
//lock (ci.SyncRoot)
|
||
//{
|
||
var agent = ci.Crowd.GetAgent(agentIdx);
|
||
if (agent == null || agent.state == DtCrowdAgentState.DT_CROWDAGENT_STATE_INVALID)
|
||
{
|
||
return Vector3.Zero;
|
||
}
|
||
|
||
// 坐标转换（和AddAgent时一致：Z轴取反）
|
||
return new Vector3(agent.npos.X, agent.npos.Y, -agent.npos.Z);
|
||
//}
|
||
}
|
||
|
||
public void UpdateAll(float deltaTime)
|
||
{
|
||
Parallel.ForEach(_crowdInstances.Values, new ParallelOptions
|
||
{
|
||
MaxDegreeOfParallelism = Environment.ProcessorCount // 限制并行数=CPU核心数，避免过载
|
||
}, ci =>
|
||
{
|
||
//lock (ci.SyncRoot)
|
||
//{
|
||
ci.Crowd.Update(deltaTime, null);
|
||
//}
|
||
});
|
||
}
|
||
}
|
||
} |