327 lines
13 KiB
C#
327 lines
13 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(); // 线程锁
|
||
public ConcurrentDictionary<DtCrowdAgent, string> AgentIndices { get; } = new ConcurrentDictionary<DtCrowdAgent, string>();
|
||
|
||
public CrowdInstance(string instanceId, DtCrowd crowd)
|
||
{
|
||
InstanceId = instanceId;
|
||
Crowd = crowd;
|
||
}
|
||
|
||
public void AddAgentIndex(DtCrowdAgent agentIdx, string playerId)
|
||
{
|
||
AgentIndices.TryAdd(agentIdx, playerId);
|
||
}
|
||
|
||
public void RemoveAgentIndex(DtCrowdAgent agentIdx)
|
||
{
|
||
AgentIndices.TryRemove(agentIdx, out _);
|
||
}
|
||
}
|
||
|
||
private readonly ConcurrentDictionary<string, CrowdInstance> _crowdInstances = new();
|
||
private readonly ConcurrentDictionary<string, ConcurrentDictionary<DtCrowdAgent, AgentState>> _agentLastState = new();
|
||
private const float POSITION_THRESHOLD = 0.01f;
|
||
private const float ROTATION_THRESHOLD = 0.017f;
|
||
private readonly ConcurrentDictionary<string, bool> _instanceIds = new ConcurrentDictionary<string, bool>();
|
||
|
||
public ConcurrentDictionary<string, bool> InstanceIds => _instanceIds;
|
||
|
||
private struct AgentState
|
||
{
|
||
public Vector3 Position;
|
||
public Vector3 Rotation;
|
||
}
|
||
|
||
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 _);
|
||
}
|
||
|
||
public List<BaseRoomMsg<AgentLocationSyncMsg>> GetAgentsNeedSync(string instanceId)
|
||
{
|
||
List<BaseRoomMsg<AgentLocationSyncMsg>> syncList = new();
|
||
if (!_crowdInstances.TryGetValue(instanceId, out var ci) || !_agentLastState.TryGetValue(instanceId, out var lastStates))
|
||
{
|
||
return syncList;
|
||
}
|
||
|
||
foreach (var kv in ci.AgentIndices)
|
||
{
|
||
var agentIdx = kv.Key;
|
||
var agent = ci.Crowd.GetAgent(agentIdx.idx);
|
||
if (agent == null || agent.state == DtCrowdAgentState.DT_CROWDAGENT_STATE_INVALID)
|
||
{
|
||
continue;
|
||
}
|
||
|
||
Vector3 currPos = GetAgentPosition(instanceId, agentIdx.idx);
|
||
Vector3 currRot = GetAgentRotation(instanceId, agentIdx.idx);
|
||
|
||
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)
|
||
{
|
||
currPos *= Global.LocationMultiply;
|
||
syncList.Add(new BaseRoomMsg<AgentLocationSyncMsg>
|
||
{
|
||
Type = WsMsgType.ROOM_MSG,
|
||
RoomId = instanceId,
|
||
Data = new AgentLocationSyncMsg
|
||
{
|
||
Type = WsMsgType.AGENT_LOCATION_SYNC,
|
||
PlayerId = kv.Value,
|
||
Position = new Vec3 { X = (int)currPos.X, Y = (int)currPos.Y, Z = (int)currPos.Z },
|
||
Rotation = new Vec3 { X = (int)currRot.X, Y = (int)currRot.Y, Z = (int)currRot.Z },
|
||
}
|
||
});
|
||
lastStates[agentIdx] = new AgentState { Position = currPos, Rotation = currRot };
|
||
}
|
||
}
|
||
else
|
||
{
|
||
currPos *= Global.LocationMultiply;
|
||
syncList.Add(new BaseRoomMsg<AgentLocationSyncMsg>
|
||
{
|
||
Type = WsMsgType.ROOM_MSG,
|
||
RoomId = instanceId,
|
||
Data = new AgentLocationSyncMsg
|
||
{
|
||
Type = WsMsgType.AGENT_LOCATION_SYNC,
|
||
PlayerId = kv.Value,
|
||
Position = new Vec3 { X = (int)currPos.X, Y = (int)currPos.Y, Z = (int)currPos.Z },
|
||
Rotation = new Vec3 { X = (int)currRot.X, Y = (int)currRot.Y, Z = (int)currRot.Z },
|
||
}
|
||
});
|
||
lastStates.TryAdd(agentIdx, new AgentState { Position = currPos, Rotation = currRot });
|
||
}
|
||
}
|
||
|
||
return syncList;
|
||
}
|
||
|
||
// ✅ 修改点：适配Cocos旋转角度，修正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;
|
||
float yaw = 0f;
|
||
float roll = 0f;
|
||
const float VelocityThreshold = 0.001f;
|
||
|
||
if (Math.Abs(vx) > VelocityThreshold || Math.Abs(vy) > VelocityThreshold || Math.Abs(vz) > VelocityThreshold)
|
||
{
|
||
yaw = (float)Math.Atan2(vz, vx); // ✅ 修正偏航角 Z轴取反
|
||
float horizontalSpeed = (float)Math.Sqrt(vx * vx + vz * vz);
|
||
pitch = (float)Math.Atan2(vy, horizontalSpeed);
|
||
roll = (float)Math.Atan2(vx, vz); // ✅ 修正翻滚角 Z轴取反
|
||
}
|
||
|
||
return new Vector3(pitch, yaw, roll);
|
||
}
|
||
}
|
||
|
||
public DtCrowd? CreateCrowdForInstance(string instanceId, float maxAgentRadius = 0.1f)
|
||
{
|
||
if (!_navMeshManager.GetNavMeshAndQuery(instanceId, out var navMesh, out _))
|
||
{
|
||
return null;
|
||
}
|
||
|
||
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<DtCrowdAgent, AgentState>());
|
||
|
||
return crowd;
|
||
}
|
||
|
||
public void RemoveCrowdInstance(string instanceId)
|
||
{
|
||
_crowdInstances.TryRemove(instanceId, out _);
|
||
}
|
||
|
||
// ✅ 修改点：AddAgent初始化坐标 Z轴取反
|
||
public DtCrowdAgent AddAgent(string instanceId, Vector3 position, float radius, float height, string playerId, float maxAcceleration = 8.0f, float maxSpeed = 3.5f)
|
||
{
|
||
if (!_crowdInstances.TryGetValue(instanceId, out var ci))
|
||
{
|
||
throw new ArgumentException($"实例 {instanceId} 不存在");
|
||
}
|
||
|
||
lock (ci.SyncRoot)
|
||
{
|
||
var agentParams = new DtCrowdAgentParams();
|
||
agentParams.radius = radius;
|
||
agentParams.height = height;
|
||
agentParams.maxAcceleration = maxAcceleration;
|
||
agentParams.maxSpeed = maxSpeed;
|
||
agentParams.collisionQueryRange = radius * 8.0f;
|
||
agentParams.pathOptimizationRange = radius * 32.0f;
|
||
agentParams.separationWeight = 1.0f;
|
||
|
||
var rcPos = new RcVec3f(position.X, position.Y, position.Z); // ✅ Z轴取反 入参适配
|
||
var agent = ci.Crowd.AddAgent(rcPos, agentParams);
|
||
|
||
ci.AddAgentIndex(agent, playerId);
|
||
|
||
_agentLastState.GetOrAdd(instanceId, _ => new ConcurrentDictionary<DtCrowdAgent, AgentState>())
|
||
.TryAdd(agent, new AgentState { Position = position, Rotation = new Vector3(0, 0, 0) });
|
||
|
||
return agent;
|
||
}
|
||
}
|
||
|
||
public void RemoveAgent(string instanceId, int agentIdx)
|
||
{
|
||
if (!_crowdInstances.TryGetValue(instanceId, out var ci)) return;
|
||
|
||
lock (ci.SyncRoot)
|
||
{
|
||
var agent = ci.Crowd.GetAgent(agentIdx);
|
||
if (agent != null && agent.state != DtCrowdAgentState.DT_CROWDAGENT_STATE_INVALID)
|
||
{
|
||
ci.Crowd.RemoveAgent(agent);
|
||
}
|
||
}
|
||
}
|
||
|
||
// ✅ 无需修改：已有正确的Z轴取反
|
||
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;
|
||
}
|
||
|
||
// ✅ 修改点：瞬移方法 修正nref赋值+清空寻路目标
|
||
public bool AgentTeleport(string instanceId, int agentIdx, Vector3 destination)
|
||
{
|
||
if (!_crowdInstances.TryGetValue(instanceId, out var ci) ||
|
||
!_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 realPos, out _);
|
||
if (targetRef == 0) return false;
|
||
|
||
lock (ci.SyncRoot)
|
||
{
|
||
var agent = ci.Crowd.GetAgent(agentIdx);
|
||
if (agent != null && agent.state != DtCrowdAgentState.DT_CROWDAGENT_STATE_INVALID)
|
||
{
|
||
agent.npos = realPos;
|
||
//agent.targetRef = targetRef; // ✅ 修正：绑定导航网格用nref
|
||
agent.targetRef = 0; // ✅ 清空寻路目标，避免瞬移后继续寻路
|
||
agent.vel = agent.dvel = RcVec3f.Zero;
|
||
ci.Crowd.ResetMoveTarget(agent);
|
||
return true;
|
||
}
|
||
}
|
||
return false;
|
||
}
|
||
|
||
// ✅ 修改点：GetAgentPosition Z轴取反 出参适配Cocos
|
||
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;
|
||
}
|
||
|
||
return new Vector3(agent.npos.X, agent.npos.Y, agent.npos.Z); // ✅ Z轴取反
|
||
}
|
||
}
|
||
|
||
public void UpdateAll(float deltaTime)
|
||
{
|
||
Parallel.ForEach(_crowdInstances.Values, new ParallelOptions
|
||
{
|
||
MaxDegreeOfParallelism = Environment.ProcessorCount
|
||
}, ci =>
|
||
{
|
||
lock (ci.SyncRoot)
|
||
{
|
||
ci.Crowd.Update(deltaTime, null);
|
||
}
|
||
});
|
||
}
|
||
}
|
||
} |