forked from mirror/DotRecast
730 lines
28 KiB
C#
730 lines
28 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using DotRecast.Core;
|
|
using DotRecast.Core.Numerics;
|
|
using DotRecast.Detour;
|
|
using DotRecast.Recast.Toolset.Builder;
|
|
using DotRecast.Recast.Demo.Draw;
|
|
using DotRecast.Recast.Toolset;
|
|
using DotRecast.Recast.Toolset.Tools;
|
|
using ImGuiNET;
|
|
using Serilog;
|
|
using static DotRecast.Recast.Demo.Draw.DebugDraw;
|
|
using static DotRecast.Recast.Demo.Draw.DebugDrawPrimitives;
|
|
|
|
namespace DotRecast.Recast.Demo.Tools;
|
|
|
|
public class TestNavmeshSampleTool : ISampleTool
|
|
{
|
|
private static readonly ILogger Logger = Log.ForContext<TestNavmeshSampleTool>();
|
|
|
|
private const int MAX_POLYS = 256;
|
|
|
|
private DemoSample _sample;
|
|
private readonly RcTestNavMeshTool _tool;
|
|
|
|
// mode select
|
|
private RcTestNavmeshToolMode _mode = RcTestNavmeshToolMode.Values[RcTestNavmeshToolMode.PATHFIND_FOLLOW.Idx];
|
|
|
|
// flags
|
|
private int _includeFlags = SampleAreaModifications.SAMPLE_POLYFLAGS_ALL;
|
|
private int _excludeFlags = 0;
|
|
|
|
private bool _enableRaycast = true;
|
|
|
|
// for pathfind straight mode
|
|
private int _straightPathOption;
|
|
|
|
// for random point in circle mode
|
|
private int _randomPointCount = 300;
|
|
private bool _constrainByCircle;
|
|
|
|
//
|
|
private bool m_sposSet;
|
|
private long m_startRef;
|
|
private RcVec3f m_spos;
|
|
|
|
private bool m_eposSet;
|
|
private long m_endRef;
|
|
private RcVec3f m_epos;
|
|
|
|
private readonly DtQueryDefaultFilter m_filter;
|
|
private readonly RcVec3f m_polyPickExt = new RcVec3f(2, 4, 2);
|
|
|
|
// for hit
|
|
private RcVec3f m_hitPos;
|
|
private RcVec3f m_hitNormal;
|
|
private bool m_hitResult;
|
|
|
|
private float m_distanceToWall;
|
|
private DtStraightPath[] m_straightPath;
|
|
private int m_straightPathCount;
|
|
private List<long> m_polys;
|
|
private List<long> m_parent;
|
|
private float m_neighbourhoodRadius;
|
|
private RcVec3f[] m_queryPoly = new RcVec3f[4];
|
|
private List<RcVec3f> m_smoothPath;
|
|
private DtStatus m_pathFindStatus = DtStatus.DT_FAILURE;
|
|
|
|
// for mode RANDOM_POINTS_IN_CIRCLE
|
|
private List<RcVec3f> _randomPoints = new();
|
|
|
|
public TestNavmeshSampleTool()
|
|
{
|
|
_tool = new();
|
|
|
|
m_filter = new DtQueryDefaultFilter(
|
|
SampleAreaModifications.SAMPLE_POLYFLAGS_ALL,
|
|
SampleAreaModifications.SAMPLE_POLYFLAGS_DISABLED,
|
|
new float[] { 1f, 1f, 1f, 1f, 2f, 1.5f }
|
|
);
|
|
m_straightPath = new DtStraightPath[MAX_POLYS];
|
|
m_straightPathCount = 0;
|
|
}
|
|
|
|
public void Layout()
|
|
{
|
|
var prevMode = _mode;
|
|
int prevModeIdx = _mode.Idx;
|
|
|
|
int prevIncludeFlags = m_filter.GetIncludeFlags();
|
|
int prevExcludeFlags = m_filter.GetExcludeFlags();
|
|
|
|
bool prevEnableRaycast = _enableRaycast;
|
|
|
|
int prevStraightPathOption = _straightPathOption;
|
|
bool prevConstrainByCircle = _constrainByCircle;
|
|
|
|
ImGui.Text("Mode");
|
|
ImGui.Separator();
|
|
ImGui.RadioButton(RcTestNavmeshToolMode.PATHFIND_FOLLOW.Label, ref prevModeIdx, RcTestNavmeshToolMode.PATHFIND_FOLLOW.Idx);
|
|
ImGui.RadioButton(RcTestNavmeshToolMode.PATHFIND_STRAIGHT.Label, ref prevModeIdx, RcTestNavmeshToolMode.PATHFIND_STRAIGHT.Idx);
|
|
ImGui.RadioButton(RcTestNavmeshToolMode.PATHFIND_SLICED.Label, ref prevModeIdx, RcTestNavmeshToolMode.PATHFIND_SLICED.Idx);
|
|
ImGui.RadioButton(RcTestNavmeshToolMode.DISTANCE_TO_WALL.Label, ref prevModeIdx, RcTestNavmeshToolMode.DISTANCE_TO_WALL.Idx);
|
|
ImGui.RadioButton(RcTestNavmeshToolMode.RAYCAST.Label, ref prevModeIdx, RcTestNavmeshToolMode.RAYCAST.Idx);
|
|
ImGui.RadioButton(RcTestNavmeshToolMode.FIND_POLYS_IN_CIRCLE.Label, ref prevModeIdx, RcTestNavmeshToolMode.FIND_POLYS_IN_CIRCLE.Idx);
|
|
ImGui.RadioButton(RcTestNavmeshToolMode.FIND_POLYS_IN_SHAPE.Label, ref prevModeIdx, RcTestNavmeshToolMode.FIND_POLYS_IN_SHAPE.Idx);
|
|
ImGui.RadioButton(RcTestNavmeshToolMode.FIND_LOCAL_NEIGHBOURHOOD.Label, ref prevModeIdx, RcTestNavmeshToolMode.FIND_LOCAL_NEIGHBOURHOOD.Idx);
|
|
ImGui.RadioButton(RcTestNavmeshToolMode.RANDOM_POINTS_IN_CIRCLE.Label, ref prevModeIdx, RcTestNavmeshToolMode.RANDOM_POINTS_IN_CIRCLE.Idx);
|
|
ImGui.NewLine();
|
|
|
|
if (prevModeIdx != _mode.Idx)
|
|
{
|
|
_mode = RcTestNavmeshToolMode.Values[prevModeIdx];
|
|
}
|
|
|
|
// selecting mode
|
|
ImGui.Text(_mode.Label);
|
|
ImGui.Separator();
|
|
ImGui.NewLine();
|
|
|
|
if (_mode == RcTestNavmeshToolMode.PATHFIND_FOLLOW)
|
|
{
|
|
}
|
|
|
|
if (_mode == RcTestNavmeshToolMode.PATHFIND_STRAIGHT)
|
|
{
|
|
ImGui.Text("Vertices at crossings");
|
|
ImGui.Separator();
|
|
ImGui.RadioButton("None", ref _straightPathOption, DtStraightPathOption.None.Value);
|
|
ImGui.RadioButton("Area", ref _straightPathOption, DtStraightPathOption.AreaCrossings.Value);
|
|
ImGui.RadioButton("All", ref _straightPathOption, DtStraightPathOption.AllCrossings.Value);
|
|
}
|
|
|
|
if (_mode == RcTestNavmeshToolMode.RANDOM_POINTS_IN_CIRCLE)
|
|
{
|
|
ImGui.SliderInt("Random point count", ref _randomPointCount, 0, 10000);
|
|
ImGui.Checkbox("Constrained", ref _constrainByCircle);
|
|
}
|
|
|
|
ImGui.Text("Common");
|
|
ImGui.Separator();
|
|
|
|
ImGui.Text("+ Include Flags");
|
|
ImGui.Separator();
|
|
ImGui.CheckboxFlags("+ Walk", ref _includeFlags, SampleAreaModifications.SAMPLE_POLYFLAGS_WALK);
|
|
ImGui.CheckboxFlags("+ Swim", ref _includeFlags, SampleAreaModifications.SAMPLE_POLYFLAGS_SWIM);
|
|
ImGui.CheckboxFlags("+ Door", ref _includeFlags, SampleAreaModifications.SAMPLE_POLYFLAGS_DOOR);
|
|
ImGui.CheckboxFlags("+ Jump", ref _includeFlags, SampleAreaModifications.SAMPLE_POLYFLAGS_JUMP);
|
|
ImGui.NewLine();
|
|
|
|
m_filter.SetIncludeFlags(_includeFlags);
|
|
|
|
ImGui.Text("- Exclude Flags");
|
|
ImGui.Separator();
|
|
ImGui.CheckboxFlags("- Walk", ref _excludeFlags, SampleAreaModifications.SAMPLE_POLYFLAGS_WALK);
|
|
ImGui.CheckboxFlags("- Swim", ref _excludeFlags, SampleAreaModifications.SAMPLE_POLYFLAGS_SWIM);
|
|
ImGui.CheckboxFlags("- Door", ref _excludeFlags, SampleAreaModifications.SAMPLE_POLYFLAGS_DOOR);
|
|
ImGui.CheckboxFlags("- Jump", ref _excludeFlags, SampleAreaModifications.SAMPLE_POLYFLAGS_JUMP);
|
|
ImGui.NewLine();
|
|
|
|
m_filter.SetExcludeFlags(_excludeFlags);
|
|
|
|
ImGui.Checkbox("Raycast shortcuts", ref _enableRaycast);
|
|
|
|
if (prevMode != _mode || prevIncludeFlags != _includeFlags
|
|
|| prevExcludeFlags != _excludeFlags
|
|
|| prevEnableRaycast != _enableRaycast
|
|
|| prevStraightPathOption != _straightPathOption
|
|
|| prevConstrainByCircle != _constrainByCircle)
|
|
{
|
|
Recalc();
|
|
}
|
|
}
|
|
|
|
public void HandleRender(NavMeshRenderer renderer)
|
|
{
|
|
RecastDebugDraw dd = renderer.GetDebugDraw();
|
|
int startCol = DuRGBA(128, 25, 0, 192);
|
|
int endCol = DuRGBA(51, 102, 0, 129);
|
|
int pathCol = DuRGBA(0, 0, 0, 64);
|
|
|
|
var settings = _sample.GetSettings();
|
|
float agentRadius = settings.agentRadius;
|
|
float agentHeight = settings.agentHeight;
|
|
float agentClimb = settings.agentMaxClimb;
|
|
|
|
if (m_sposSet)
|
|
{
|
|
DrawAgent(dd, m_spos, startCol);
|
|
}
|
|
|
|
if (m_eposSet)
|
|
{
|
|
DrawAgent(dd, m_epos, endCol);
|
|
}
|
|
|
|
dd.DepthMask(true);
|
|
|
|
DtNavMesh m_navMesh = _sample.GetNavMesh();
|
|
if (m_navMesh == null)
|
|
{
|
|
return;
|
|
}
|
|
|
|
if (_mode == RcTestNavmeshToolMode.PATHFIND_FOLLOW)
|
|
{
|
|
dd.DebugDrawNavMeshPoly(m_navMesh, m_startRef, startCol);
|
|
dd.DebugDrawNavMeshPoly(m_navMesh, m_endRef, endCol);
|
|
|
|
if (m_polys != null)
|
|
{
|
|
foreach (long poly in m_polys)
|
|
{
|
|
if (poly == m_startRef || poly == m_endRef)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
dd.DebugDrawNavMeshPoly(m_navMesh, poly, pathCol);
|
|
}
|
|
}
|
|
|
|
if (m_smoothPath != null)
|
|
{
|
|
dd.DepthMask(false);
|
|
int spathCol = DuRGBA(0, 0, 0, 220);
|
|
dd.Begin(LINES, 3.0f);
|
|
for (int i = 0; i < m_smoothPath.Count; ++i)
|
|
{
|
|
dd.Vertex(m_smoothPath[i].X, m_smoothPath[i].Y + 0.1f, m_smoothPath[i].Z, spathCol);
|
|
}
|
|
|
|
dd.End();
|
|
dd.DepthMask(true);
|
|
}
|
|
/*
|
|
if (m_pathIterNum)
|
|
{
|
|
DuDebugDrawNavMeshPoly(&dd, *m_navMesh, m_pathIterPolys.x, DebugDraw.DuRGBA(255,255,255,128));
|
|
|
|
dd.DepthMask(false);
|
|
dd.Begin(DebugDrawPrimitives.LINES, 1.0f);
|
|
|
|
int prevCol = DebugDraw.DuRGBA(255,192,0,220);
|
|
int curCol = DebugDraw.DuRGBA(255,255,255,220);
|
|
int steerCol = DebugDraw.DuRGBA(0,192,255,220);
|
|
|
|
dd.Vertex(m_prevIterPos.x,m_prevIterPos.y-0.3f,m_prevIterPos.z, prevCol);
|
|
dd.Vertex(m_prevIterPos.x,m_prevIterPos.y+0.3f,m_prevIterPos.z, prevCol);
|
|
|
|
dd.Vertex(m_iterPos.x,m_iterPos.y-0.3f,m_iterPos.z, curCol);
|
|
dd.Vertex(m_iterPos.x,m_iterPos.y+0.3f,m_iterPos.z, curCol);
|
|
|
|
dd.Vertex(m_prevIterPos.x,m_prevIterPos.y+0.3f,m_prevIterPos.z, prevCol);
|
|
dd.Vertex(m_iterPos.x,m_iterPos.y+0.3f,m_iterPos.z, prevCol);
|
|
|
|
dd.Vertex(m_prevIterPos.x,m_prevIterPos.y+0.3f,m_prevIterPos.z, steerCol);
|
|
dd.Vertex(m_steerPos.x,m_steerPos.y+0.3f,m_steerPos.z, steerCol);
|
|
|
|
for (int i = 0; i < m_steerPointCount-1; ++i)
|
|
{
|
|
dd.Vertex(m_steerPoints[i*3+0],m_steerPoints[i*3+1]+0.2f,m_steerPoints[i*3+2], DuDarkenCol(steerCol));
|
|
dd.Vertex(m_steerPoints[(i+1)*3+0],m_steerPoints[(i+1)*3+1]+0.2f,m_steerPoints[(i+1)*3+2], DuDarkenCol(steerCol));
|
|
}
|
|
|
|
dd.End();
|
|
dd.DepthMask(true);
|
|
}
|
|
*/
|
|
}
|
|
else if (_mode == RcTestNavmeshToolMode.PATHFIND_STRAIGHT || _mode == RcTestNavmeshToolMode.PATHFIND_SLICED)
|
|
{
|
|
dd.DebugDrawNavMeshPoly(m_navMesh, m_startRef, startCol);
|
|
dd.DebugDrawNavMeshPoly(m_navMesh, m_endRef, endCol);
|
|
|
|
if (m_polys != null)
|
|
{
|
|
foreach (long poly in m_polys)
|
|
{
|
|
dd.DebugDrawNavMeshPoly(m_navMesh, poly, pathCol);
|
|
}
|
|
}
|
|
|
|
if (m_straightPath != null)
|
|
{
|
|
dd.DepthMask(false);
|
|
int spathCol = DuRGBA(64, 16, 0, 220);
|
|
int offMeshCol = DuRGBA(128, 96, 0, 220);
|
|
dd.Begin(LINES, 2.0f);
|
|
for (int i = 0; i < m_straightPathCount - 1; ++i)
|
|
{
|
|
DtStraightPath straightPathItem = m_straightPath[i];
|
|
DtStraightPath straightPathItem2 = m_straightPath[i + 1];
|
|
int col;
|
|
if ((straightPathItem.flags & DtStraightPathFlags.DT_STRAIGHTPATH_OFFMESH_CONNECTION) != 0)
|
|
{
|
|
col = offMeshCol;
|
|
}
|
|
else
|
|
{
|
|
col = spathCol;
|
|
}
|
|
|
|
dd.Vertex(straightPathItem.pos.X, straightPathItem.pos.Y + 0.4f, straightPathItem.pos.Z, col);
|
|
dd.Vertex(straightPathItem2.pos.X, straightPathItem2.pos.Y + 0.4f, straightPathItem2.pos.Z, col);
|
|
}
|
|
|
|
dd.End();
|
|
dd.Begin(POINTS, 6.0f);
|
|
for (int i = 0; i < m_straightPathCount; ++i)
|
|
{
|
|
DtStraightPath straightPathItem = m_straightPath[i];
|
|
int col;
|
|
if ((straightPathItem.flags & DtStraightPathFlags.DT_STRAIGHTPATH_START) != 0)
|
|
{
|
|
col = startCol;
|
|
}
|
|
else if ((straightPathItem.flags & DtStraightPathFlags.DT_STRAIGHTPATH_END) != 0)
|
|
{
|
|
col = endCol;
|
|
}
|
|
else if ((straightPathItem.flags & DtStraightPathFlags.DT_STRAIGHTPATH_OFFMESH_CONNECTION) != 0)
|
|
{
|
|
col = offMeshCol;
|
|
}
|
|
else
|
|
{
|
|
col = spathCol;
|
|
}
|
|
|
|
dd.Vertex(straightPathItem.pos.X, straightPathItem.pos.Y + 0.4f, straightPathItem.pos.Z, col);
|
|
}
|
|
|
|
dd.End();
|
|
dd.DepthMask(true);
|
|
}
|
|
}
|
|
else if (_mode == RcTestNavmeshToolMode.RAYCAST)
|
|
{
|
|
dd.DebugDrawNavMeshPoly(m_navMesh, m_startRef, startCol);
|
|
|
|
if (m_straightPath != null)
|
|
{
|
|
if (m_polys != null)
|
|
{
|
|
foreach (long poly in m_polys)
|
|
{
|
|
dd.DebugDrawNavMeshPoly(m_navMesh, poly, pathCol);
|
|
}
|
|
}
|
|
|
|
dd.DepthMask(false);
|
|
int spathCol = m_hitResult ? DuRGBA(64, 16, 0, 220) : DuRGBA(240, 240, 240, 220);
|
|
dd.Begin(LINES, 2.0f);
|
|
for (int i = 0; i < m_straightPathCount - 1; ++i)
|
|
{
|
|
DtStraightPath straightPathItem = m_straightPath[i];
|
|
DtStraightPath straightPathItem2 = m_straightPath[i + 1];
|
|
dd.Vertex(straightPathItem.pos.X, straightPathItem.pos.Y + 0.4f, straightPathItem.pos.Z, spathCol);
|
|
dd.Vertex(straightPathItem2.pos.X, straightPathItem2.pos.Y + 0.4f, straightPathItem2.pos.Z, spathCol);
|
|
}
|
|
|
|
dd.End();
|
|
dd.Begin(POINTS, 4.0f);
|
|
for (int i = 0; i < m_straightPathCount; ++i)
|
|
{
|
|
DtStraightPath straightPathItem = m_straightPath[i];
|
|
dd.Vertex(straightPathItem.pos.X, straightPathItem.pos.Y + 0.4f, straightPathItem.pos.Z, spathCol);
|
|
}
|
|
|
|
dd.End();
|
|
|
|
if (m_hitResult)
|
|
{
|
|
int hitCol = DuRGBA(0, 0, 0, 128);
|
|
dd.Begin(LINES, 2.0f);
|
|
dd.Vertex(m_hitPos.X, m_hitPos.Y + 0.4f, m_hitPos.Z, hitCol);
|
|
dd.Vertex(m_hitPos.X + m_hitNormal.X * agentRadius, m_hitPos.Y + 0.4f + m_hitNormal.Y * agentRadius, m_hitPos.Z + m_hitNormal.Z * agentRadius, hitCol);
|
|
dd.End();
|
|
}
|
|
|
|
dd.DepthMask(true);
|
|
}
|
|
}
|
|
else if (_mode == RcTestNavmeshToolMode.DISTANCE_TO_WALL)
|
|
{
|
|
dd.DebugDrawNavMeshPoly(m_navMesh, m_startRef, startCol);
|
|
dd.DepthMask(false);
|
|
if (m_spos != RcVec3f.Zero)
|
|
{
|
|
dd.DebugDrawCircle(m_spos.X, m_spos.Y + agentHeight / 2, m_spos.Z, m_distanceToWall, DuRGBA(64, 16, 0, 220), 2.0f);
|
|
}
|
|
|
|
if (m_hitPos != RcVec3f.Zero)
|
|
{
|
|
dd.Begin(LINES, 3.0f);
|
|
dd.Vertex(m_hitPos.X, m_hitPos.Y + 0.02f, m_hitPos.Z, DuRGBA(0, 0, 0, 192));
|
|
dd.Vertex(m_hitPos.X, m_hitPos.Y + agentHeight, m_hitPos.Z, DuRGBA(0, 0, 0, 192));
|
|
dd.End();
|
|
}
|
|
|
|
dd.DepthMask(true);
|
|
}
|
|
else if (_mode == RcTestNavmeshToolMode.FIND_POLYS_IN_CIRCLE)
|
|
{
|
|
if (m_polys != null)
|
|
{
|
|
for (int i = 0; i < m_polys.Count; i++)
|
|
{
|
|
dd.DebugDrawNavMeshPoly(m_navMesh, m_polys[i], pathCol);
|
|
dd.DepthMask(false);
|
|
if (m_parent[i] != 0)
|
|
{
|
|
dd.DepthMask(false);
|
|
RcVec3f p0 = m_navMesh.GetPolyCenter(m_parent[i]);
|
|
RcVec3f p1 = m_navMesh.GetPolyCenter(m_polys[i]);
|
|
dd.DebugDrawArc(p0.X, p0.Y, p0.Z, p1.X, p1.Y, p1.Z, 0.25f, 0.0f, 0.4f, DuRGBA(0, 0, 0, 128), 2.0f);
|
|
dd.DepthMask(true);
|
|
}
|
|
|
|
dd.DepthMask(true);
|
|
}
|
|
}
|
|
|
|
if (m_sposSet && m_eposSet)
|
|
{
|
|
dd.DepthMask(false);
|
|
float dx = m_epos.X - m_spos.X;
|
|
float dz = m_epos.Z - m_spos.Z;
|
|
float dist = MathF.Sqrt(dx * dx + dz * dz);
|
|
dd.DebugDrawCircle(m_spos.X, m_spos.Y + agentHeight / 2, m_spos.Z, dist, DuRGBA(64, 16, 0, 220), 2.0f);
|
|
dd.DepthMask(true);
|
|
}
|
|
}
|
|
else if (_mode == RcTestNavmeshToolMode.FIND_POLYS_IN_SHAPE)
|
|
{
|
|
if (m_polys != null)
|
|
{
|
|
for (int i = 0; i < m_polys.Count; i++)
|
|
{
|
|
dd.DebugDrawNavMeshPoly(m_navMesh, m_polys[i], pathCol);
|
|
dd.DepthMask(false);
|
|
if (m_parent[i] != 0)
|
|
{
|
|
dd.DepthMask(false);
|
|
RcVec3f p0 = m_navMesh.GetPolyCenter(m_parent[i]);
|
|
RcVec3f p1 = m_navMesh.GetPolyCenter(m_polys[i]);
|
|
dd.DebugDrawArc(p0.X, p0.Y, p0.Z, p1.X, p1.Y, p1.Z, 0.25f, 0.0f, 0.4f, DuRGBA(0, 0, 0, 128), 2.0f);
|
|
dd.DepthMask(true);
|
|
}
|
|
|
|
dd.DepthMask(true);
|
|
}
|
|
}
|
|
|
|
if (m_sposSet && m_eposSet)
|
|
{
|
|
dd.DepthMask(false);
|
|
int col = DuRGBA(64, 16, 0, 220);
|
|
dd.Begin(LINES, 2.0f);
|
|
for (int i = 0, j = 3; i < 4; j = i++)
|
|
{
|
|
dd.Vertex(m_queryPoly[j].X, m_queryPoly[j].Y, m_queryPoly[j].Z, col);
|
|
dd.Vertex(m_queryPoly[i].X, m_queryPoly[i].Y, m_queryPoly[i].Z, col);
|
|
}
|
|
|
|
dd.End();
|
|
dd.DepthMask(true);
|
|
}
|
|
}
|
|
else if (_mode == RcTestNavmeshToolMode.FIND_LOCAL_NEIGHBOURHOOD)
|
|
{
|
|
if (m_polys != null)
|
|
{
|
|
const int MAX_SEGS = DtDetour.DT_VERTS_PER_POLYGON * 4;
|
|
Span<RcSegmentVert> segs = stackalloc RcSegmentVert[MAX_SEGS];
|
|
Span<long> refs = stackalloc long[MAX_SEGS];
|
|
|
|
for (int i = 0; i < m_polys.Count; i++)
|
|
{
|
|
dd.DebugDrawNavMeshPoly(m_navMesh, m_polys[i], pathCol);
|
|
dd.DepthMask(false);
|
|
if (m_parent[i] != 0)
|
|
{
|
|
dd.DepthMask(false);
|
|
RcVec3f p0 = m_navMesh.GetPolyCenter(m_parent[i]);
|
|
RcVec3f p1 = m_navMesh.GetPolyCenter(m_polys[i]);
|
|
dd.DebugDrawArc(p0.X, p0.Y, p0.Z, p1.X, p1.Y, p1.Z, 0.25f, 0.0f, 0.4f, DuRGBA(0, 0, 0, 128), 2.0f);
|
|
dd.DepthMask(true);
|
|
}
|
|
|
|
dd.DepthMask(true);
|
|
if (_sample.GetNavMeshQuery() != null)
|
|
{
|
|
int nsegs = 0;
|
|
var result = _sample
|
|
.GetNavMeshQuery()
|
|
.GetPolyWallSegments(m_polys[i], m_filter, segs, refs, ref nsegs, MAX_SEGS);
|
|
|
|
if (result.Succeeded())
|
|
{
|
|
dd.Begin(LINES, 2.0f);
|
|
for (int j = 0; j < nsegs; ++j)
|
|
{
|
|
ref RcSegmentVert s = ref segs[j];
|
|
var v0 = s.vmin;
|
|
var s3 = s.vmax;
|
|
|
|
// Skip too distant segments.
|
|
var distSqr = DtUtils.DistancePtSegSqr2D(m_spos, v0, s3, out var tseg);
|
|
if (distSqr > RcMath.Sqr(m_neighbourhoodRadius))
|
|
{
|
|
continue;
|
|
}
|
|
|
|
RcVec3f delta = RcVec3f.Subtract(s3, s.vmin);
|
|
RcVec3f p0 = RcVec.Mad(s.vmin, delta, 0.5f);
|
|
RcVec3f norm = new RcVec3f(delta.Z, 0, -delta.X);
|
|
norm = RcVec3f.Normalize(norm);
|
|
RcVec3f p1 = RcVec.Mad(p0, norm, agentRadius * 0.5f);
|
|
|
|
// Skip backfacing segments.
|
|
if (refs[j] != 0)
|
|
{
|
|
int col = DuRGBA(255, 255, 255, 32);
|
|
dd.Vertex(s.vmin.X, s.vmin.Y + agentClimb, s.vmin.Z, col);
|
|
dd.Vertex(s.vmax.X, s.vmax.Y + agentClimb, s.vmax.Z, col);
|
|
}
|
|
else
|
|
{
|
|
int col = DuRGBA(192, 32, 16, 192);
|
|
if (DtUtils.TriArea2D(m_spos, s.vmin, s3) < 0.0f)
|
|
{
|
|
col = DuRGBA(96, 32, 16, 192);
|
|
}
|
|
|
|
dd.Vertex(p0.X, p0.Y + agentClimb, p0.Z, col);
|
|
dd.Vertex(p1.X, p1.Y + agentClimb, p1.Z, col);
|
|
|
|
dd.Vertex(s.vmin.X, s.vmin.Y + agentClimb, s.vmin.Z, col);
|
|
dd.Vertex(s.vmax.X, s.vmax.Y + agentClimb, s.vmax.Z, col);
|
|
}
|
|
}
|
|
|
|
dd.End();
|
|
}
|
|
}
|
|
|
|
dd.DepthMask(true);
|
|
}
|
|
|
|
if (m_sposSet)
|
|
{
|
|
dd.DepthMask(false);
|
|
dd.DebugDrawCircle(m_spos.X, m_spos.Y + agentHeight / 2, m_spos.Z, m_neighbourhoodRadius, DuRGBA(64, 16, 0, 220), 2.0f);
|
|
dd.DepthMask(true);
|
|
}
|
|
}
|
|
}
|
|
else if (_mode == RcTestNavmeshToolMode.RANDOM_POINTS_IN_CIRCLE)
|
|
{
|
|
dd.DepthMask(false);
|
|
dd.Begin(POINTS, 4.0f);
|
|
int col = DuRGBA(64, 16, 0, 220);
|
|
foreach (RcVec3f point in _randomPoints)
|
|
{
|
|
dd.Vertex(point.X, point.Y + 0.1f, point.Z, col);
|
|
}
|
|
|
|
dd.End();
|
|
if (m_sposSet && m_eposSet)
|
|
{
|
|
dd.DepthMask(false);
|
|
float dx = m_epos.X - m_spos.X;
|
|
float dz = m_epos.Z - m_spos.Z;
|
|
float dist = MathF.Sqrt(dx * dx + dz * dz);
|
|
dd.DebugDrawCircle(m_spos.X, m_spos.Y + agentHeight / 2, m_spos.Z, dist, DuRGBA(64, 16, 0, 220), 2.0f);
|
|
dd.DepthMask(true);
|
|
}
|
|
|
|
dd.DepthMask(true);
|
|
}
|
|
}
|
|
|
|
private void DrawAgent(RecastDebugDraw dd, RcVec3f pos, int col)
|
|
{
|
|
var settings = _sample.GetSettings();
|
|
float r = settings.agentRadius;
|
|
float h = settings.agentHeight;
|
|
float c = settings.agentMaxClimb;
|
|
dd.DepthMask(false);
|
|
// Agent dimensions.
|
|
dd.DebugDrawCylinderWire(pos.X - r, pos.Y + 0.02f, pos.Z - r, pos.X + r, pos.Y + h, pos.Z + r, col, 2.0f);
|
|
dd.DebugDrawCircle(pos.X, pos.Y + c, pos.Z, r, DuRGBA(0, 0, 0, 64), 1.0f);
|
|
int colb = DuRGBA(0, 0, 0, 196);
|
|
dd.Begin(LINES);
|
|
dd.Vertex(pos.X, pos.Y - c, pos.Z, colb);
|
|
dd.Vertex(pos.X, pos.Y + c, pos.Z, colb);
|
|
dd.Vertex(pos.X - r / 2, pos.Y + 0.02f, pos.Z, colb);
|
|
dd.Vertex(pos.X + r / 2, pos.Y + 0.02f, pos.Z, colb);
|
|
dd.Vertex(pos.X, pos.Y + 0.02f, pos.Z - r / 2, colb);
|
|
dd.Vertex(pos.X, pos.Y + 0.02f, pos.Z + r / 2, colb);
|
|
dd.End();
|
|
dd.DepthMask(true);
|
|
}
|
|
|
|
public IRcToolable GetTool()
|
|
{
|
|
return _tool;
|
|
}
|
|
|
|
public void SetSample(DemoSample sample)
|
|
{
|
|
_sample = sample;
|
|
}
|
|
|
|
public void OnSampleChanged()
|
|
{
|
|
// ..
|
|
}
|
|
|
|
|
|
public void HandleClick(RcVec3f s, RcVec3f p, bool shift)
|
|
{
|
|
if (shift)
|
|
{
|
|
m_sposSet = true;
|
|
m_spos = p;
|
|
}
|
|
else
|
|
{
|
|
m_eposSet = true;
|
|
m_epos = p;
|
|
}
|
|
|
|
Recalc();
|
|
}
|
|
|
|
|
|
private void Recalc()
|
|
{
|
|
var geom = _sample.GetInputGeom();
|
|
var settings = _sample.GetSettings();
|
|
var navMesh = _sample.GetNavMesh();
|
|
var navQuery = _sample.GetNavMeshQuery();
|
|
|
|
if (null == geom || null == navQuery)
|
|
return;
|
|
|
|
if (m_sposSet)
|
|
{
|
|
navQuery.FindNearestPoly(m_spos, m_polyPickExt, m_filter, out m_startRef, out var _, out var _);
|
|
}
|
|
else
|
|
{
|
|
m_startRef = 0;
|
|
}
|
|
|
|
if (m_eposSet)
|
|
{
|
|
navQuery.FindNearestPoly(m_epos, m_polyPickExt, m_filter, out m_endRef, out var _, out var _);
|
|
}
|
|
else
|
|
{
|
|
m_endRef = 0;
|
|
}
|
|
|
|
if (_mode == RcTestNavmeshToolMode.PATHFIND_FOLLOW)
|
|
{
|
|
_tool.FindFollowPath(navMesh, navQuery, m_startRef, m_endRef, m_spos, m_epos, m_filter, _enableRaycast,
|
|
ref m_polys, m_polys?.Count ?? 0, ref m_smoothPath);
|
|
}
|
|
else if (_mode == RcTestNavmeshToolMode.PATHFIND_STRAIGHT)
|
|
{
|
|
_tool.FindStraightPath(navQuery, m_startRef, m_endRef, m_spos, m_epos, m_filter, _enableRaycast,
|
|
ref m_polys, m_straightPath, out m_straightPathCount, MAX_POLYS, _straightPathOption);
|
|
}
|
|
else if (_mode == RcTestNavmeshToolMode.PATHFIND_SLICED)
|
|
{
|
|
m_polys?.Clear();
|
|
m_straightPathCount = 0;
|
|
m_pathFindStatus = _tool.InitSlicedFindPath(navQuery, m_startRef, m_endRef, m_spos, m_epos, m_filter, _enableRaycast);
|
|
}
|
|
else if (_mode == RcTestNavmeshToolMode.RAYCAST)
|
|
{
|
|
_tool.Raycast(navQuery, m_startRef, m_endRef, m_spos, m_epos, m_filter,
|
|
ref m_polys, m_straightPath, out m_straightPathCount, MAX_POLYS, ref m_hitPos, ref m_hitNormal, ref m_hitResult);
|
|
}
|
|
else if (_mode == RcTestNavmeshToolMode.DISTANCE_TO_WALL)
|
|
{
|
|
_tool.FindDistanceToWall(navQuery, m_startRef, m_spos, 100.0f, m_filter, ref m_distanceToWall, ref m_hitPos, ref m_hitNormal);
|
|
}
|
|
else if (_mode == RcTestNavmeshToolMode.FIND_POLYS_IN_CIRCLE)
|
|
{
|
|
_tool.FindPolysAroundCircle(navQuery, m_startRef, m_endRef, m_spos, m_epos, m_filter, ref m_polys, ref m_parent);
|
|
}
|
|
else if (_mode == RcTestNavmeshToolMode.FIND_POLYS_IN_SHAPE)
|
|
{
|
|
_tool.FindPolysAroundShape(navQuery, settings.agentHeight, m_startRef, m_endRef, m_spos, m_epos, m_filter, ref m_polys, ref m_parent, ref m_queryPoly);
|
|
}
|
|
else if (_mode == RcTestNavmeshToolMode.FIND_LOCAL_NEIGHBOURHOOD)
|
|
{
|
|
m_neighbourhoodRadius = settings.agentRadius * 20.0f;
|
|
_tool.FindLocalNeighbourhood(navQuery, m_startRef, m_spos, m_neighbourhoodRadius, m_filter, ref m_polys, ref m_parent);
|
|
}
|
|
else if (_mode == RcTestNavmeshToolMode.RANDOM_POINTS_IN_CIRCLE)
|
|
{
|
|
_randomPoints.Clear();
|
|
_tool.FindRandomPointAroundCircle(navQuery, m_startRef, m_endRef, m_spos, m_epos, m_filter, _constrainByCircle, _randomPointCount, ref _randomPoints);
|
|
}
|
|
}
|
|
|
|
|
|
public void HandleUpdate(float dt)
|
|
{
|
|
if (_mode == RcTestNavmeshToolMode.PATHFIND_SLICED)
|
|
{
|
|
DtNavMeshQuery navQuery = _sample.GetNavMeshQuery();
|
|
|
|
if (m_pathFindStatus.InProgress())
|
|
{
|
|
m_pathFindStatus = _tool.UpdateSlicedFindPath(navQuery, 1, m_endRef, m_spos, m_epos, ref m_polys, m_straightPath, out m_straightPathCount, MAX_POLYS);
|
|
}
|
|
}
|
|
}
|
|
|
|
public void HandleClickRay(RcVec3f start, RcVec3f direction, bool shift)
|
|
{
|
|
}
|
|
} |