356 lines
14 KiB
C#
356 lines
14 KiB
C#
using DotRecast.Core;
|
||
using DotRecast.Core.Numerics;
|
||
using DotRecast.Detour;
|
||
using DotRecast.Detour.Crowd;
|
||
using System.Collections.Concurrent;
|
||
using System.Numerics;
|
||
using XNet.Business.Dto;
|
||
|
||
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弧度）才同步
|
||
private readonly ConcurrentDictionary<string, bool> _instanceIds = new ConcurrentDictionary<string, bool>();
|
||
|
||
public ConcurrentDictionary<string, bool> InstanceIds => _instanceIds;
|
||
|
||
// Agent状态结构体
|
||
private struct AgentState
|
||
{
|
||
public Vector3 Position;
|
||
public Vector3 Rotation; // 绕Y轴旋转（弧度）
|
||
}
|
||
|
||
|
||
public SceneAgent(NavMeshManager navMeshManager)
|
||
{
|
||
_navMeshManager = navMeshManager;
|
||
}
|
||
|
||
|
||
|
||
public void CreateInstance(string roomId, string mapId)
|
||
{
|
||
_navMeshManager.CreateInstance(roomId, mapId);
|
||
InstanceIds.TryAdd(roomId, true);
|
||
}
|
||
|
||
public void RemoveInstance(string roomId)
|
||
{
|
||
_navMeshManager.RemoveInstance(roomId);
|
||
InstanceIds.TryRemove(roomId, out _);
|
||
}
|
||
|
||
|
||
|
||
// ==========================================
|
||
// 修复4：GetAgentsNeedSync 遍历 AgentIndices
|
||
// ==========================================
|
||
public List<RoomMsg<AgentLocationSyncMsg>> GetAgentsNeedSync(string instanceId)
|
||
{
|
||
List<RoomMsg<AgentLocationSyncMsg>> 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);
|
||
Vector3 currRot = GetAgentRotation(instanceId, agentIdx);
|
||
|
||
// 对比上一帧状态
|
||
if (lastStates.TryGetValue(agentIdx, out var lastState))
|
||
{
|
||
// 判断是否超过阈值
|
||
bool posChanged = Vector3.Distance(currPos, lastState.Position) > POSITION_THRESHOLD;
|
||
bool rotChanged = Vector3.Distance(currRot, lastState.Rotation) > ROTATION_THRESHOLD;
|
||
|
||
if (posChanged || rotChanged)
|
||
{
|
||
// 加入同步列表
|
||
syncList.Add(new RoomMsg<AgentLocationSyncMsg>
|
||
{
|
||
RoomId = instanceId,
|
||
Data = new AgentLocationSyncMsg
|
||
{
|
||
AgentIdx = agentIdx,
|
||
Position = new Vec3 { X = currPos.X, Y = currPos.Y, Z = currPos.Z },
|
||
Rotation = new Vec3 { X = currRot.X, Y = currRot.Y, Z = currRot.Z },
|
||
}
|
||
});
|
||
// 更新缓存
|
||
lastStates[agentIdx] = new AgentState { Position = currPos, Rotation = currRot };
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// 首次同步，直接加入并缓存
|
||
syncList.Add(new RoomMsg<AgentLocationSyncMsg>
|
||
{
|
||
RoomId = instanceId,
|
||
Data = new AgentLocationSyncMsg
|
||
{
|
||
AgentIdx = agentIdx,
|
||
Position = new Vec3 { X = currPos.X, Y = currPos.Y, Z = currPos.Z },
|
||
Rotation = new Vec3 { X = currRot.X, Y = currRot.Y, Z = currRot.Z },
|
||
}
|
||
});
|
||
lastStates.TryAdd(agentIdx, new AgentState { Position = currPos, Rotation = currRot });
|
||
}
|
||
}
|
||
//}
|
||
|
||
return syncList;
|
||
}
|
||
|
||
// ===============================================
|
||
// 完整版：通过速度计算【完整三维欧拉角】X=俯仰 Y=偏航 Z=翻滚 全部齐全
|
||
// ===============================================
|
||
public Vector3 GetAgentRotation(string instanceId, int agentIdx)
|
||
{
|
||
if (!_crowdInstances.TryGetValue(instanceId, out var ci))
|
||
{
|
||
return new Vector3(0, 0, 0);
|
||
}
|
||
|
||
lock (ci.SyncRoot)
|
||
{
|
||
var agent = ci.Crowd.GetAgent(agentIdx);
|
||
if (agent == null || agent.state == DtCrowdAgentState.DT_CROWDAGENT_STATE_INVALID)
|
||
{
|
||
return new Vector3(0, 0, 0);
|
||
}
|
||
|
||
float vx = agent.vel.X;
|
||
float vy = agent.vel.Y; // 垂直速度-上下移动
|
||
float vz = -agent.vel.Z; // 坐标转换保留，正确无需修改
|
||
float pitch = 0f; // X轴：俯仰角-抬头低头
|
||
float yaw = 0f; // Y轴：偏航角-左右转向
|
||
float roll = 0f; // ✅ 新增：Z轴 翻滚角-左侧倾/右侧倾
|
||
const float VelocityThreshold = 0.001f;
|
||
|
||
// 三维速度防抖：任意轴速度达标，才计算所有旋转角
|
||
if (Math.Abs(vx) > VelocityThreshold || Math.Abs(vy) > VelocityThreshold || Math.Abs(vz) > VelocityThreshold)
|
||
{
|
||
// 1. 计算 Y轴 偏航角 (左右转向) - 原有逻辑不变
|
||
yaw = (float)Math.Atan2(vz, vx);
|
||
|
||
// 2. 计算 X轴 俯仰角 (抬头低头) - 原有逻辑不变
|
||
float horizontalSpeed = (float)Math.Sqrt(vx * vx + vz * vz);
|
||
pitch = (float)Math.Atan2(vy, horizontalSpeed);
|
||
|
||
// ✅ ✅ ✅ 核心新增：计算 Z轴 翻滚角 (侧身翻滚) 【推荐公式】
|
||
roll = (float)Math.Atan2(vx, vz);
|
||
}
|
||
|
||
// ✅ 完整三维欧拉角返回：X俯仰、Y偏航、Z翻滚 全部赋值
|
||
return new Vector3(pitch, yaw, roll);
|
||
}
|
||
}
|
||
|
||
|
||
// 初始化实例的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 = new Vector3(0, 0, 0) });
|
||
|
||
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);
|
||
}
|
||
});
|
||
}
|
||
}
|
||
} |