a.samsonkin
/
DotRecastNetSim
forked from bit/DotRecastNetSim
1
0
Fork 0
Code Pull Requests Activity

Crowd Sample Tool Model + View

This commit is contained in:
ikpil 2023-09-09 23:20:47 +09:00
parent 303f194fe7
commit 95af26e884
5 changed files with 665 additions and 326 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/DotRecast.Detour.Crowd/DtCrowd.cs
View File

@ -185,8 +185,7 @@ namespace DotRecast.Detour.Crowd
_agents = new List<DtCrowdAgent>();
// The navQuery is mostly used for local searches, no need for large node pool.
_navMesh = nav;
_navQuery = new DtNavMeshQuery(nav);
SetNavMesh(nav);
}
public void SetNavMesh(DtNavMesh nav)
@ -195,6 +194,16 @@ namespace DotRecast.Detour.Crowd
_navQuery = new DtNavMeshQuery(nav);
}
public DtNavMesh GetNavMesh()
{
return _navMesh;
}
public DtNavMeshQuery GetNavMeshQuery()
{
return _navQuery;
}
/// Sets the shared avoidance configuration for the specified index.
/// @param[in] idx The index. [Limits: 0 <= value <
/// #DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS]

2
src/DotRecast.Recast.Demo/Tools/CrowdProfilingSampleTool.cs
View File

@ -40,7 +40,7 @@ public class CrowdProfilingSampleTool : ISampleTool
private DemoSample _sample;
private DtNavMesh m_nav;
private readonly CrowdOption _option = new CrowdOption();
private readonly CrowdConfig _option = new CrowdConfig();
private RcCrowdProfilingTool _tool;
private int expandSimOptions = 1;

669
src/DotRecast.Recast.Demo/Tools/CrowdSampleTool.cs
View File

@ -41,36 +41,29 @@ public class CrowdSampleTool : ISampleTool
private DemoSample _sample;
private readonly RcCrowdTool _tool;
private readonly CrowdOption _option = new CrowdOption();
private DtNavMesh m_nav;
private DtCrowd crowd;
private readonly DtCrowdAgentDebugInfo _agentDebug = new DtCrowdAgentDebugInfo();
private readonly Dictionary<long, CrowdAgentTrail> m_trails = new();
private RcVec3f m_targetPos;
private long m_targetRef;
private DtNavMesh m_nav;
private CrowdToolMode m_mode = CrowdToolMode.CREATE;
private int m_modeIdx = CrowdToolMode.CREATE.Idx;
private long crowdUpdateTime;
private int _expandSelectedDebugDraw = 1;
private bool _showCorners;
private bool _showCollisionSegments;
private bool _showPath;
private bool _showVO;
private bool _showOpt;
private bool _showNeis;
private bool _showCorners = true;
private bool _showCollisionSegments = true;
private bool _showPath = true;
private bool _showVO = true;
private bool _showOpt = true;
private bool _showNeis = true;
private int _expandDebugDraw = 0;
private bool _showLabels;
private bool _showGrid;
private bool _showNodes;
private bool _showPerfGraph;
private bool _showDetailAll;
private bool _showLabels = true;
private bool _showGrid = false;
private bool _showNodes = true;
private bool _showPerfGraph = true;
private bool _showDetailAll = true;
public CrowdSampleTool()
{
_agentDebug.vod = new DtObstacleAvoidanceDebugData(2048);
_tool = new();
}
@ -90,33 +83,34 @@ public class CrowdSampleTool : ISampleTool
m_mode = CrowdToolMode.Values[m_modeIdx];
}
var prevOptimizeVis = _option.optimizeVis;
var prevOptimizeTopo = _option.optimizeTopo;
var prevAnticipateTurns = _option.anticipateTurns;
var prevObstacleAvoidance = _option.obstacleAvoidance;
var prevSeparation = _option.separation;
var prevObstacleAvoidanceType = _option.obstacleAvoidanceType;
var prevSeparationWeight = _option.separationWeight;
var crowdCfg = _tool.GetCrowdConfig();
var prevOptimizeVis = crowdCfg.optimizeVis;
var prevOptimizeTopo = crowdCfg.optimizeTopo;
var prevAnticipateTurns = crowdCfg.anticipateTurns;
var prevObstacleAvoidance = crowdCfg.obstacleAvoidance;
var prevSeparation = crowdCfg.separation;
var prevObstacleAvoidanceType = crowdCfg.obstacleAvoidanceType;
var prevSeparationWeight = crowdCfg.separationWeight;
ImGui.Text("Options");
ImGui.Separator();
ImGui.Checkbox("Optimize Visibility", ref _option.optimizeVis);
ImGui.Checkbox("Optimize Topology", ref _option.optimizeTopo);
ImGui.Checkbox("Anticipate Turns", ref _option.anticipateTurns);
ImGui.Checkbox("Obstacle Avoidance", ref _option.obstacleAvoidance);
ImGui.SliderInt("Avoidance Quality", ref _option.obstacleAvoidanceType, 0, 3);
ImGui.Checkbox("Separation", ref _option.separation);
ImGui.SliderFloat("Separation Weight", ref _option.separationWeight, 0f, 20f, "%.2f");
ImGui.Checkbox("Optimize Visibility", ref crowdCfg.optimizeVis);
ImGui.Checkbox("Optimize Topology", ref crowdCfg.optimizeTopo);
ImGui.Checkbox("Anticipate Turns", ref crowdCfg.anticipateTurns);
ImGui.Checkbox("Obstacle Avoidance", ref crowdCfg.obstacleAvoidance);
ImGui.SliderInt("Avoidance Quality", ref crowdCfg.obstacleAvoidanceType, 0, 3);
ImGui.Checkbox("Separation", ref crowdCfg.separation);
ImGui.SliderFloat("Separation Weight", ref crowdCfg.separationWeight, 0f, 20f, "%.2f");
ImGui.NewLine();
if (prevOptimizeVis != _option.optimizeVis || prevOptimizeTopo != _option.optimizeTopo
|| prevAnticipateTurns != _option.anticipateTurns
|| prevObstacleAvoidance != _option.obstacleAvoidance
|| prevSeparation != _option.separation
|| prevObstacleAvoidanceType != _option.obstacleAvoidanceType
|| !prevSeparationWeight.Equals(_option.separationWeight))
if (prevOptimizeVis != crowdCfg.optimizeVis || prevOptimizeTopo != crowdCfg.optimizeTopo
|| prevAnticipateTurns != crowdCfg.anticipateTurns
|| prevObstacleAvoidance != crowdCfg.obstacleAvoidance
|| prevSeparation != crowdCfg.separation
|| prevObstacleAvoidanceType != crowdCfg.obstacleAvoidanceType
|| !prevSeparationWeight.Equals(crowdCfg.separationWeight))
{
UpdateAgentParams();
_tool.UpdateAgentParams();
}
@ -134,54 +128,322 @@ public class CrowdSampleTool : ISampleTool
ImGui.Separator();
ImGui.Checkbox("Show Proximity Grid", ref _showGrid);
ImGui.Checkbox("Show Nodes", ref _showNodes);
ImGui.Text($"Update Time: {crowdUpdateTime} ms");
ImGui.Text($"Update Time: {_tool.GetCrowdUpdateTime()} ms");
}
public void HandleRender(NavMeshRenderer renderer)
{
RecastDebugDraw dd = renderer.GetDebugDraw();
var settings = _sample.GetSettings();
float rad = settings.agentRadius;
var crowd = _tool.GetCrowd();
if (crowd == null)
return;
var nav = crowd.GetNavMesh();
if (nav == null)
return;
var cfg = _tool.GetCrowdConfig();
var agentDebug = _tool.GetCrowdAgentDebugInfo();
var agentTrails = _tool.GetCrowdAgentTrails();
var moveTargetRef = _tool.GetMoveTargetRef();
var moveTargetPos = _tool.GetMoveTargetPos();
if (_showNodes && crowd.GetPathQueue() != null)
{
var navquery = crowd.GetNavMeshQuery();
if (navquery != null)
{
dd.DebugDrawNavMeshNodes(navquery);
}
}
dd.DepthMask(false);
if (crowd != null)
// Draw paths
if (_showPath)
{
foreach (DtCrowdAgent ag in crowd.GetActiveAgents())
{
float radius = ag.option.radius;
RcVec3f pos = ag.npos;
dd.DebugDrawCircle(pos.x, pos.y, pos.z, radius, DuRGBA(0, 0, 0, 32), 2.0f);
}
if (!_showDetailAll && ag != agentDebug.agent)
continue;
List<long> path = ag.corridor.GetPath();
int npath = ag.corridor.GetPathCount();
for (int j = 0; j < npath; ++j)
{
dd.DebugDrawNavMeshPoly(nav, path[j], DuRGBA(255, 255, 255, 24));
}
}
}
if (moveTargetRef != 0)
dd.DebugDrawCross(moveTargetPos.x, moveTargetPos.y + 0.1f, moveTargetPos.z, rad, DuRGBA(255, 255, 255, 192), 2.0f);
// Occupancy grid.
if (_showGrid)
{
float gridy = -float.MaxValue;
foreach (DtCrowdAgent ag in crowd.GetActiveAgents())
{
CrowdAgentData crowAgentData = (CrowdAgentData)ag.option.userData;
float height = ag.option.height;
float radius = ag.option.radius;
RcVec3f pos = ag.npos;
int col = DuRGBA(220, 220, 220, 128);
if (crowAgentData.type == CrowdAgentType.TRAVELLER)
{
col = DuRGBA(100, 160, 100, 128);
}
if (crowAgentData.type == CrowdAgentType.VILLAGER)
{
col = DuRGBA(120, 80, 160, 128);
}
if (ag.targetState == DtMoveRequestState.DT_CROWDAGENT_TARGET_REQUESTING
|| ag.targetState == DtMoveRequestState.DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE)
col = DuLerpCol(col, DuRGBA(255, 255, 32, 128), 128);
else if (ag.targetState == DtMoveRequestState.DT_CROWDAGENT_TARGET_WAITING_FOR_PATH)
col = DuLerpCol(col, DuRGBA(255, 64, 32, 128), 128);
else if (ag.targetState == DtMoveRequestState.DT_CROWDAGENT_TARGET_FAILED)
col = DuRGBA(255, 32, 16, 128);
else if (ag.targetState == DtMoveRequestState.DT_CROWDAGENT_TARGET_VELOCITY)
col = DuLerpCol(col, DuRGBA(64, 255, 0, 128), 128);
dd.DebugDrawCylinder(pos.x - radius, pos.y + radius * 0.1f, pos.z - radius, pos.x + radius, pos.y + height,
pos.z + radius, col);
RcVec3f pos = ag.corridor.GetPos();
gridy = Math.Max(gridy, pos.y);
}
gridy += 1.0f;
DtProximityGrid grid = crowd.GetGrid();
if (null != grid)
{
dd.Begin(QUADS);
float cs = grid.GetCellSize();
foreach (var (combinedKey, count) in grid.GetItemCounts())
{
DtProximityGrid.DecomposeKey(combinedKey, out var x, out var y);
if (count != 0)
{
int col = DuRGBA(128, 0, 0, Math.Min(count * 40, 255));
dd.Vertex(x * cs, gridy, y * cs, col);
dd.Vertex(x * cs, gridy, y * cs + cs, col);
dd.Vertex(x * cs + cs, gridy, y * cs + cs, col);
dd.Vertex(x * cs + cs, gridy, y * cs, col);
}
}
dd.End();
}
}
// Trail
foreach (DtCrowdAgent ag in crowd.GetActiveAgents())
{
CrowdAgentTrail trail = agentTrails[ag.idx];
RcVec3f pos = ag.npos;
dd.Begin(LINES, 3.0f);
RcVec3f prev = new RcVec3f();
float preva = 1;
prev = pos;
for (int j = 0; j < CrowdAgentTrail.AGENT_MAX_TRAIL - 1; ++j)
{
int idx = (trail.htrail + CrowdAgentTrail.AGENT_MAX_TRAIL - j) % CrowdAgentTrail.AGENT_MAX_TRAIL;
int v = idx * 3;
float a = 1 - j / (float)CrowdAgentTrail.AGENT_MAX_TRAIL;
dd.Vertex(prev.x, prev.y + 0.1f, prev.z, DuRGBA(0, 0, 0, (int)(128 * preva)));
dd.Vertex(trail.trail[v], trail.trail[v + 1] + 0.1f, trail.trail[v + 2], DuRGBA(0, 0, 0, (int)(128 * a)));
preva = a;
prev.Set(trail.trail, v);
}
dd.End();
}
// Corners & co
foreach (DtCrowdAgent ag in crowd.GetActiveAgents())
{
if (_showDetailAll == false && ag != agentDebug.agent)
continue;
float radius = ag.option.radius;
RcVec3f pos = ag.npos;
if (_showCorners)
{
if (0 < ag.corners.Count)
{
dd.Begin(LINES, 2.0f);
for (int j = 0; j < ag.corners.Count; ++j)
{
RcVec3f va = j == 0 ? pos : ag.corners[j - 1].pos;
RcVec3f vb = ag.corners[j].pos;
dd.Vertex(va.x, va.y + radius, va.z, DuRGBA(128, 0, 0, 192));
dd.Vertex(vb.x, vb.y + radius, vb.z, DuRGBA(128, 0, 0, 192));
}
if ((ag.corners[ag.corners.Count - 1].flags
& DtNavMeshQuery.DT_STRAIGHTPATH_OFFMESH_CONNECTION) != 0)
{
RcVec3f v = ag.corners[ag.corners.Count - 1].pos;
dd.Vertex(v.x, v.y, v.z, DuRGBA(192, 0, 0, 192));
dd.Vertex(v.x, v.y + radius * 2, v.z, DuRGBA(192, 0, 0, 192));
}
dd.End();
if (cfg.anticipateTurns)
{
/* float dvel[3], pos[3];
CalcSmoothSteerDirection(ag.pos, ag.cornerVerts, ag.ncorners, dvel);
pos.x = ag.pos.x + dvel.x;
pos.y = ag.pos.y + dvel.y;
pos.z = ag.pos.z + dvel.z;
float off = ag.radius+0.1f;
float[] tgt = &ag.cornerVerts.x;
float y = ag.pos.y+off;
dd.Begin(DU_DRAW_LINES, 2.0f);
dd.Vertex(ag.pos.x,y,ag.pos.z, DuRGBA(255,0,0,192));
dd.Vertex(pos.x,y,pos.z, DuRGBA(255,0,0,192));
dd.Vertex(pos.x,y,pos.z, DuRGBA(255,0,0,192));
dd.Vertex(tgt.x,y,tgt.z, DuRGBA(255,0,0,192));
dd.End();*/
}
}
}
if (_showCollisionSegments)
{
RcVec3f center = ag.boundary.GetCenter();
dd.DebugDrawCross(center.x, center.y + radius, center.z, 0.2f, DuRGBA(192, 0, 128, 255), 2.0f);
dd.DebugDrawCircle(center.x, center.y + radius, center.z, ag.option.collisionQueryRange, DuRGBA(192, 0, 128, 128), 2.0f);
dd.Begin(LINES, 3.0f);
for (int j = 0; j < ag.boundary.GetSegmentCount(); ++j)
{
int col = DuRGBA(192, 0, 128, 192);
RcVec3f[] s = ag.boundary.GetSegment(j);
RcVec3f s0 = s[0];
RcVec3f s3 = s[1];
if (DtUtils.TriArea2D(pos, s0, s3) < 0.0f)
col = DuDarkenCol(col);
dd.AppendArrow(s[0].x, s[0].y + 0.2f, s[0].z, s[1].x, s[1].z + 0.2f, s[1].z, 0.0f, 0.3f, col);
}
dd.End();
}
if (_showNeis)
{
dd.DebugDrawCircle(pos.x, pos.y + radius, pos.z, ag.option.collisionQueryRange, DuRGBA(0, 192, 128, 128),
2.0f);
dd.Begin(LINES, 2.0f);
for (int j = 0; j < ag.neis.Count; ++j)
{
DtCrowdAgent nei = ag.neis[j].agent;
if (nei != null)
{
dd.Vertex(pos.x, pos.y + radius, pos.z, DuRGBA(0, 192, 128, 128));
dd.Vertex(nei.npos.x, nei.npos.y + radius, nei.npos.z, DuRGBA(0, 192, 128, 128));
}
}
dd.End();
}
if (_showOpt)
{
dd.Begin(LINES, 2.0f);
dd.Vertex(agentDebug.optStart.x, agentDebug.optStart.y + 0.3f, agentDebug.optStart.z,
DuRGBA(0, 128, 0, 192));
dd.Vertex(agentDebug.optEnd.x, agentDebug.optEnd.y + 0.3f, agentDebug.optEnd.z, DuRGBA(0, 128, 0, 192));
dd.End();
}
}
// Agent cylinders.
foreach (DtCrowdAgent ag in crowd.GetActiveAgents())
{
float radius = ag.option.radius;
RcVec3f pos = ag.npos;
int col = DuRGBA(0, 0, 0, 32);
if (agentDebug.agent == ag)
col = DuRGBA(255, 0, 0, 128);
dd.DebugDrawCircle(pos.x, pos.y, pos.z, radius, col, 2.0f);
}
foreach (DtCrowdAgent ag in crowd.GetActiveAgents())
{
float height = ag.option.height;
float radius = ag.option.radius;
RcVec3f pos = ag.npos;
int col = DuRGBA(220, 220, 220, 128);
if (ag.targetState == DtMoveRequestState.DT_CROWDAGENT_TARGET_REQUESTING
|| ag.targetState == DtMoveRequestState.DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE)
col = DuLerpCol(col, DuRGBA(128, 0, 255, 128), 32);
else if (ag.targetState == DtMoveRequestState.DT_CROWDAGENT_TARGET_WAITING_FOR_PATH)
col = DuLerpCol(col, DuRGBA(128, 0, 255, 128), 128);
else if (ag.targetState == DtMoveRequestState.DT_CROWDAGENT_TARGET_FAILED)
col = DuRGBA(255, 32, 16, 128);
else if (ag.targetState == DtMoveRequestState.DT_CROWDAGENT_TARGET_VELOCITY)
col = DuLerpCol(col, DuRGBA(64, 255, 0, 128), 128);
dd.DebugDrawCylinder(pos.x - radius, pos.y + radius * 0.1f, pos.z - radius, pos.x + radius, pos.y + height,
pos.z + radius, col);
}
if (_showVO)
{
foreach (DtCrowdAgent ag in crowd.GetActiveAgents())
{
if (_showDetailAll == false && ag != agentDebug.agent)
continue;
// Draw detail about agent sela
DtObstacleAvoidanceDebugData vod = agentDebug.vod;
float dx = ag.npos.x;
float dy = ag.npos.y + ag.option.height;
float dz = ag.npos.z;
dd.DebugDrawCircle(dx, dy, dz, ag.option.maxSpeed, DuRGBA(255, 255, 255, 64), 2.0f);
dd.Begin(QUADS);
for (int j = 0; j < vod.GetSampleCount(); ++j)
{
RcVec3f p = vod.GetSampleVelocity(j);
float sr = vod.GetSampleSize(j);
float pen = vod.GetSamplePenalty(j);
float pen2 = vod.GetSamplePreferredSidePenalty(j);
int col = DuLerpCol(DuRGBA(255, 255, 255, 220), DuRGBA(128, 96, 0, 220), (int)(pen * 255));
col = DuLerpCol(col, DuRGBA(128, 0, 0, 220), (int)(pen2 * 128));
dd.Vertex(dx + p.x - sr, dy, dz + p.z - sr, col);
dd.Vertex(dx + p.x - sr, dy, dz + p.z + sr, col);
dd.Vertex(dx + p.x + sr, dy, dz + p.z + sr, col);
dd.Vertex(dx + p.x + sr, dy, dz + p.z - sr, col);
}
dd.End();
}
}
// Velocity stuff.
foreach (DtCrowdAgent ag in crowd.GetActiveAgents())
{
float radius = ag.option.radius;
float height = ag.option.height;
RcVec3f pos = ag.npos;
RcVec3f vel = ag.vel;
RcVec3f dvel = ag.dvel;
int col = DuRGBA(220, 220, 220, 192);
if (ag.targetState == DtMoveRequestState.DT_CROWDAGENT_TARGET_REQUESTING
|| ag.targetState == DtMoveRequestState.DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE)
col = DuLerpCol(col, DuRGBA(128, 0, 255, 192), 48);
else if (ag.targetState == DtMoveRequestState.DT_CROWDAGENT_TARGET_WAITING_FOR_PATH)
col = DuLerpCol(col, DuRGBA(128, 0, 255, 192), 128);
else if (ag.targetState == DtMoveRequestState.DT_CROWDAGENT_TARGET_FAILED)
col = DuRGBA(255, 32, 16, 192);
else if (ag.targetState == DtMoveRequestState.DT_CROWDAGENT_TARGET_VELOCITY)
col = DuLerpCol(col, DuRGBA(64, 255, 0, 192), 128);
dd.DebugDrawCircle(pos.x, pos.y + height, pos.z, radius, col, 2.0f);
dd.DebugDrawArrow(pos.x, pos.y + height, pos.z, pos.x + dvel.x, pos.y + height + dvel.y, pos.z + dvel.z,
0.0f, 0.4f, DuRGBA(0, 192, 255, 192), agentDebug.agent == ag ? 2.0f : 1.0f);
dd.DebugDrawArrow(pos.x, pos.y + height, pos.z, pos.x + vel.x, pos.y + height + vel.y, pos.z + vel.z, 0.0f,
0.4f, DuRGBA(0, 0, 0, 160), 2.0f);
}
dd.DepthMask(true);
@ -206,51 +468,13 @@ public class CrowdSampleTool : ISampleTool
if (navMesh != null && m_nav != navMesh)
{
m_nav = navMesh;
DtCrowdConfig config = new DtCrowdConfig(settings.agentRadius);
crowd = new DtCrowd(config, navMesh, __ => new DtQueryDefaultFilter(
SampleAreaModifications.SAMPLE_POLYFLAGS_ALL,
SampleAreaModifications.SAMPLE_POLYFLAGS_DISABLED,
new float[] { 1f, 10f, 1f, 1f, 2f, 1.5f })
);
// Setup local avoidance option to different qualities.
// Use mostly default settings, copy from dtCrowd.
DtObstacleAvoidanceParams option = new DtObstacleAvoidanceParams(crowd.GetObstacleAvoidanceParams(0));
// Low (11)
option.velBias = 0.5f;
option.adaptiveDivs = 5;
option.adaptiveRings = 2;
option.adaptiveDepth = 1;
crowd.SetObstacleAvoidanceParams(0, option);
// Medium (22)
option.velBias = 0.5f;
option.adaptiveDivs = 5;
option.adaptiveRings = 2;
option.adaptiveDepth = 2;
crowd.SetObstacleAvoidanceParams(1, option);
// Good (45)
option.velBias = 0.5f;
option.adaptiveDivs = 7;
option.adaptiveRings = 2;
option.adaptiveDepth = 3;
crowd.SetObstacleAvoidanceParams(2, option);
// High (66)
option.velBias = 0.5f;
option.adaptiveDivs = 7;
option.adaptiveRings = 3;
option.adaptiveDepth = 3;
crowd.SetObstacleAvoidanceParams(3, option);
_tool.Setup(settings.agentRadius, navMesh);
}
}
public void HandleClick(RcVec3f s, RcVec3f p, bool shift)
{
var crowd = _tool.GetCrowd();
if (crowd == null)
{
return;
@ -261,27 +485,28 @@ public class CrowdSampleTool : ISampleTool
if (shift)
{
// Delete
DtCrowdAgent ahit = HitTestAgents(s, p);
DtCrowdAgent ahit = _tool.HitTestAgents(s, p);
if (ahit != null)
{
RemoveAgent(ahit);
_tool.RemoveAgent(ahit);
}
}
else
{
// Add
AddAgent(p);
var settings = _sample.GetSettings();
_tool.AddAgent(p, settings.agentRadius, settings.agentHeight, settings.agentMaxAcceleration, settings.agentMaxSpeed);
}
}
else if (m_mode == CrowdToolMode.MOVE_TARGET)
{
SetMoveTarget(p, shift);
_tool.SetMoveTarget(p, shift);
}
else if (m_mode == CrowdToolMode.SELECT)
{
// Highlight
DtCrowdAgent ahit = HitTestAgents(s, p);
HighlightAgent(ahit);
DtCrowdAgent ahit = _tool.HitTestAgents(s, p);
_tool.HighlightAgent(ahit);
}
else if (m_mode == CrowdToolMode.TOGGLE_POLYS)
{
@ -304,208 +529,10 @@ public class CrowdSampleTool : ISampleTool
}
}
private void RemoveAgent(DtCrowdAgent agent)
{
crowd.RemoveAgent(agent);
if (agent == _agentDebug.agent)
{
_agentDebug.agent = null;
}
}
private void AddAgent(RcVec3f p)
{
DtCrowdAgentParams ap = GetAgentParams();
DtCrowdAgent ag = crowd.AddAgent(p, ap);
if (ag != null)
{
if (m_targetRef != 0)
crowd.RequestMoveTarget(ag, m_targetRef, m_targetPos);
// Init trail
if (!m_trails.TryGetValue(ag.idx, out var trail))
{
trail = new CrowdAgentTrail();
m_trails.Add(ag.idx, trail);
}
for (int i = 0; i < CrowdAgentTrail.AGENT_MAX_TRAIL; ++i)
{
trail.trail[i * 3] = p.x;
trail.trail[i * 3 + 1] = p.y;
trail.trail[i * 3 + 2] = p.z;
}
trail.htrail = 0;
}
}
private DtCrowdAgentParams GetAgentParams()
{
var settings = _sample.GetSettings();
DtCrowdAgentParams ap = new DtCrowdAgentParams();
ap.radius = settings.agentRadius;
ap.height = settings.agentHeight;
ap.maxAcceleration = settings.agentMaxAcceleration;
ap.maxSpeed = settings.agentMaxSpeed;
ap.collisionQueryRange = ap.radius * 12.0f;
ap.pathOptimizationRange = ap.radius * 30.0f;
ap.updateFlags = _option.GetUpdateFlags();
ap.obstacleAvoidanceType = _option.obstacleAvoidanceType;
ap.separationWeight = _option.separationWeight;
return ap;
}
private DtCrowdAgent HitTestAgents(RcVec3f s, RcVec3f p)
{
DtCrowdAgent isel = null;
float tsel = float.MaxValue;
foreach (DtCrowdAgent ag in crowd.GetActiveAgents())
{
RcVec3f bmin = new RcVec3f();
RcVec3f bmax = new RcVec3f();
GetAgentBounds(ag, ref bmin, ref bmax);
if (Intersections.IsectSegAABB(s, p, bmin, bmax, out var tmin, out var tmax))
{
if (tmin > 0 && tmin < tsel)
{
isel = ag;
tsel = tmin;
}
}
}
return isel;
}
private void GetAgentBounds(DtCrowdAgent ag, ref RcVec3f bmin, ref RcVec3f bmax)
{
RcVec3f p = ag.npos;
float r = ag.option.radius;
float h = ag.option.height;
bmin.x = p.x - r;
bmin.y = p.y;
bmin.z = p.z - r;
bmax.x = p.x + r;
bmax.y = p.y + h;
bmax.z = p.z + r;
}
private void SetMoveTarget(RcVec3f p, bool adjust)
{
if (crowd == null)
return;
// Find nearest point on navmesh and set move request to that location.
DtNavMeshQuery navquery = _sample.GetNavMeshQuery();
IDtQueryFilter filter = crowd.GetFilter(0);
RcVec3f halfExtents = crowd.GetQueryExtents();
if (adjust)
{
// Request velocity
if (_agentDebug.agent != null)
{
RcVec3f vel = CalcVel(_agentDebug.agent.npos, p, _agentDebug.agent.option.maxSpeed);
crowd.RequestMoveVelocity(_agentDebug.agent, vel);
}
else
{
foreach (DtCrowdAgent ag in crowd.GetActiveAgents())
{
RcVec3f vel = CalcVel(ag.npos, p, ag.option.maxSpeed);
crowd.RequestMoveVelocity(ag, vel);
}
}
}
else
{
navquery.FindNearestPoly(p, halfExtents, filter, out m_targetRef, out m_targetPos, out var _);
if (_agentDebug.agent != null)
{
crowd.RequestMoveTarget(_agentDebug.agent, m_targetRef, m_targetPos);
}
else
{
foreach (DtCrowdAgent ag in crowd.GetActiveAgents())
{
crowd.RequestMoveTarget(ag, m_targetRef, m_targetPos);
}
}
}
}
private RcVec3f CalcVel(RcVec3f pos, RcVec3f tgt, float speed)
{
RcVec3f vel = tgt.Subtract(pos);
vel.y = 0.0f;
vel.Normalize();
return vel.Scale(speed);
}
public void HandleUpdate(float dt)
{
if (crowd == null)
return;
DtNavMesh nav = _sample.GetNavMesh();
if (nav == null)
return;
long startTime = RcFrequency.Ticks;
crowd.Update(dt, _agentDebug);
long endTime = RcFrequency.Ticks;
// Update agent trails
foreach (DtCrowdAgent ag in crowd.GetActiveAgents())
{
CrowdAgentTrail trail = m_trails[ag.idx];
// Update agent movement trail.
trail.htrail = (trail.htrail + 1) % CrowdAgentTrail.AGENT_MAX_TRAIL;
trail.trail[trail.htrail * 3] = ag.npos.x;
trail.trail[trail.htrail * 3 + 1] = ag.npos.y;
trail.trail[trail.htrail * 3 + 2] = ag.npos.z;
}
_agentDebug.vod.NormalizeSamples();
// m_crowdSampleCount.addSample((float) crowd.GetVelocitySampleCount());
crowdUpdateTime = (endTime - startTime) / TimeSpan.TicksPerMillisecond;
}
private void HighlightAgent(DtCrowdAgent agent)
{
_agentDebug.agent = agent;
}
private void UpdateAgentParams()
{
if (crowd == null)
{
return;
}
foreach (DtCrowdAgent ag in crowd.GetActiveAgents())
{
DtCrowdAgentParams option = new DtCrowdAgentParams();
option.radius = ag.option.radius;
option.height = ag.option.height;
option.maxAcceleration = ag.option.maxAcceleration;
option.maxSpeed = ag.option.maxSpeed;
option.collisionQueryRange = ag.option.collisionQueryRange;
option.pathOptimizationRange = ag.option.pathOptimizationRange;
option.obstacleAvoidanceType = ag.option.obstacleAvoidanceType;
option.queryFilterType = ag.option.queryFilterType;
option.userData = ag.option.userData;
option.updateFlags = _option.GetUpdateFlags();
option.obstacleAvoidanceType = _option.obstacleAvoidanceType;
option.separationWeight = _option.separationWeight;
crowd.UpdateAgentParameters(ag, option);
}
_tool.Update(dt);
}

2
src/DotRecast.Recast.Toolset/Tools/CrowdOption.cs → src/DotRecast.Recast.Toolset/Tools/CrowdConfig.cs
View File

@ -2,7 +2,7 @@ using DotRecast.Detour.Crowd;
namespace DotRecast.Recast.Toolset.Tools
{
public class CrowdOption
public class CrowdConfig
{
public int expandOptions = 1;
public bool anticipateTurns = true;

305
src/DotRecast.Recast.Toolset/Tools/RcCrowdTool.cs
View File

@ -1,10 +1,313 @@
namespace DotRecast.Recast.Toolset.Tools
using System;
using System.Collections.Generic;
using DotRecast.Core;
using DotRecast.Detour;
using DotRecast.Detour.Crowd;
using DotRecast.Detour.Crowd.Tracking;
using DotRecast.Recast.Toolset.Builder;
namespace DotRecast.Recast.Toolset.Tools
{
public class RcCrowdTool : IRcToolable
{
private readonly CrowdConfig _cfg;
private DtCrowd crowd;
private readonly DtCrowdAgentDebugInfo _agentDebug;
private long crowdUpdateTime;
private readonly Dictionary<long, CrowdAgentTrail> _trails;
private long _moveTargetRef;
private RcVec3f _moveTargetPos;
public RcCrowdTool()
{
_cfg = new CrowdConfig();
_agentDebug = new DtCrowdAgentDebugInfo();
_agentDebug.vod = new DtObstacleAvoidanceDebugData(2048);
_trails = new Dictionary<long, CrowdAgentTrail>();
}
public string GetName()
{
return "Crowd Control";
}
public CrowdConfig GetCrowdConfig()
{
return _cfg;
}
public DtCrowdAgentDebugInfo GetCrowdAgentDebugInfo()
{
return _agentDebug;
}
public Dictionary<long, CrowdAgentTrail> GetCrowdAgentTrails()
{
return _trails;
}
public long GetMoveTargetRef()
{
return _moveTargetRef;
}
public RcVec3f GetMoveTargetPos()
{
return _moveTargetPos;
}
public void Setup(float agentRadius, DtNavMesh navMesh)
{
DtCrowdConfig config = new DtCrowdConfig(agentRadius);
crowd = new DtCrowd(config, navMesh, __ => new DtQueryDefaultFilter(
SampleAreaModifications.SAMPLE_POLYFLAGS_ALL,
SampleAreaModifications.SAMPLE_POLYFLAGS_DISABLED,
new float[] { 1f, 10f, 1f, 1f, 2f, 1.5f })
);
// Setup local avoidance option to different qualities.
// Use mostly default settings, copy from dtCrowd.
DtObstacleAvoidanceParams option = new DtObstacleAvoidanceParams(crowd.GetObstacleAvoidanceParams(0));
// Low (11)
option.velBias = 0.5f;
option.adaptiveDivs = 5;
option.adaptiveRings = 2;
option.adaptiveDepth = 1;
crowd.SetObstacleAvoidanceParams(0, option);
// Medium (22)
option.velBias = 0.5f;
option.adaptiveDivs = 5;
option.adaptiveRings = 2;
option.adaptiveDepth = 2;
crowd.SetObstacleAvoidanceParams(1, option);
// Good (45)
option.velBias = 0.5f;
option.adaptiveDivs = 7;
option.adaptiveRings = 2;
option.adaptiveDepth = 3;
crowd.SetObstacleAvoidanceParams(2, option);
// High (66)
option.velBias = 0.5f;
option.adaptiveDivs = 7;
option.adaptiveRings = 3;
option.adaptiveDepth = 3;
crowd.SetObstacleAvoidanceParams(3, option);
}
public void UpdateAgentParams()
{
if (crowd == null)
{
return;
}
foreach (DtCrowdAgent ag in crowd.GetActiveAgents())
{
DtCrowdAgentParams agOption = new DtCrowdAgentParams();
agOption.radius = ag.option.radius;
agOption.height = ag.option.height;
agOption.maxAcceleration = ag.option.maxAcceleration;
agOption.maxSpeed = ag.option.maxSpeed;
agOption.collisionQueryRange = ag.option.collisionQueryRange;
agOption.pathOptimizationRange = ag.option.pathOptimizationRange;
agOption.obstacleAvoidanceType = ag.option.obstacleAvoidanceType;
agOption.queryFilterType = ag.option.queryFilterType;
agOption.userData = ag.option.userData;
agOption.updateFlags = _cfg.GetUpdateFlags();
agOption.obstacleAvoidanceType = _cfg.obstacleAvoidanceType;
agOption.separationWeight = _cfg.separationWeight;
crowd.UpdateAgentParameters(ag, agOption);
}
}
public DtCrowd GetCrowd()
{
return crowd;
}
public void Update(float dt)
{
if (crowd == null)
return;
DtNavMesh nav = crowd.GetNavMesh();
if (nav == null)
return;
long startTime = RcFrequency.Ticks;
crowd.Update(dt, _agentDebug);
long endTime = RcFrequency.Ticks;
// Update agent trails
foreach (DtCrowdAgent ag in crowd.GetActiveAgents())
{
CrowdAgentTrail trail = _trails[ag.idx];
// Update agent movement trail.
trail.htrail = (trail.htrail + 1) % CrowdAgentTrail.AGENT_MAX_TRAIL;
trail.trail[trail.htrail * 3] = ag.npos.x;
trail.trail[trail.htrail * 3 + 1] = ag.npos.y;
trail.trail[trail.htrail * 3 + 2] = ag.npos.z;
}
_agentDebug.vod.NormalizeSamples();
// m_crowdSampleCount.addSample((float) crowd.GetVelocitySampleCount());
crowdUpdateTime = (endTime - startTime) / TimeSpan.TicksPerMillisecond;
}
public void RemoveAgent(DtCrowdAgent agent)
{
crowd.RemoveAgent(agent);
if (agent == _agentDebug.agent)
{
_agentDebug.agent = null;
}
}
public void AddAgent(RcVec3f p, float agentRadius, float agentHeight, float agentMaxAcceleration, float agentMaxSpeed)
{
DtCrowdAgentParams ap = CreateAgentParams(agentRadius, agentHeight, agentMaxAcceleration, agentMaxSpeed);
DtCrowdAgent ag = crowd.AddAgent(p, ap);
if (ag != null)
{
if (_moveTargetRef != 0)
crowd.RequestMoveTarget(ag, _moveTargetRef, _moveTargetPos);
// Init trail
if (!_trails.TryGetValue(ag.idx, out var trail))
{
trail = new CrowdAgentTrail();
_trails.Add(ag.idx, trail);
}
for (int i = 0; i < CrowdAgentTrail.AGENT_MAX_TRAIL; ++i)
{
trail.trail[i * 3] = p.x;
trail.trail[i * 3 + 1] = p.y;
trail.trail[i * 3 + 2] = p.z;
}
trail.htrail = 0;
}
}
private DtCrowdAgentParams CreateAgentParams(float agentRadius, float agentHeight, float agentMaxAcceleration, float agentMaxSpeed)
{
DtCrowdAgentParams ap = new DtCrowdAgentParams();
ap.radius = agentRadius;
ap.height = agentHeight;
ap.maxAcceleration = agentMaxAcceleration;
ap.maxSpeed = agentMaxSpeed;
ap.collisionQueryRange = ap.radius * 12.0f;
ap.pathOptimizationRange = ap.radius * 30.0f;
ap.updateFlags = _cfg.GetUpdateFlags();
ap.obstacleAvoidanceType = _cfg.obstacleAvoidanceType;
ap.separationWeight = _cfg.separationWeight;
return ap;
}
public DtCrowdAgent HitTestAgents(RcVec3f s, RcVec3f p)
{
DtCrowdAgent isel = null;
float tsel = float.MaxValue;
foreach (DtCrowdAgent ag in crowd.GetActiveAgents())
{
RcVec3f bmin = new RcVec3f();
RcVec3f bmax = new RcVec3f();
GetAgentBounds(ag, ref bmin, ref bmax);
if (Intersections.IsectSegAABB(s, p, bmin, bmax, out var tmin, out var tmax))
{
if (tmin > 0 && tmin < tsel)
{
isel = ag;
tsel = tmin;
}
}
}
return isel;
}
private void GetAgentBounds(DtCrowdAgent ag, ref RcVec3f bmin, ref RcVec3f bmax)
{
RcVec3f p = ag.npos;
float r = ag.option.radius;
float h = ag.option.height;
bmin.x = p.x - r;
bmin.y = p.y;
bmin.z = p.z - r;
bmax.x = p.x + r;
bmax.y = p.y + h;
bmax.z = p.z + r;
}
public void SetMoveTarget(RcVec3f p, bool adjust)
{
if (crowd == null)
return;
// Find nearest point on navmesh and set move request to that location.
DtNavMeshQuery navquery = crowd.GetNavMeshQuery();
IDtQueryFilter filter = crowd.GetFilter(0);
RcVec3f halfExtents = crowd.GetQueryExtents();
if (adjust)
{
// Request velocity
if (_agentDebug.agent != null)
{
RcVec3f vel = CalcVel(_agentDebug.agent.npos, p, _agentDebug.agent.option.maxSpeed);
crowd.RequestMoveVelocity(_agentDebug.agent, vel);
}
else
{
foreach (DtCrowdAgent ag in crowd.GetActiveAgents())
{
RcVec3f vel = CalcVel(ag.npos, p, ag.option.maxSpeed);
crowd.RequestMoveVelocity(ag, vel);
}
}
}
else
{
navquery.FindNearestPoly(p, halfExtents, filter, out _moveTargetRef, out _moveTargetPos, out var _);
if (_agentDebug.agent != null)
{
crowd.RequestMoveTarget(_agentDebug.agent, _moveTargetRef, _moveTargetPos);
}
else
{
foreach (DtCrowdAgent ag in crowd.GetActiveAgents())
{
crowd.RequestMoveTarget(ag, _moveTargetRef, _moveTargetPos);
}
}
}
}
private RcVec3f CalcVel(RcVec3f pos, RcVec3f tgt, float speed)
{
RcVec3f vel = tgt.Subtract(pos);
vel.y = 0.0f;
vel.Normalize();
return vel.Scale(speed);
}
public long GetCrowdUpdateTime()
{
return crowdUpdateTime;
}
public void HighlightAgent(DtCrowdAgent agent)
{
_agentDebug.agent = agent;
}
}
}