forked from bit/DotRecastNetSim
323 lines
12 KiB
C#
323 lines
12 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using DotRecast.Core;
|
|
using DotRecast.Detour;
|
|
|
|
namespace DotRecast.Recast.DemoTool.Tools
|
|
{
|
|
public class TestNavmeshToolImpl : ISampleTool
|
|
{
|
|
private const int MAX_POLYS = 256;
|
|
private const int MAX_SMOOTH = 2048;
|
|
|
|
private Sample _sample;
|
|
private readonly TestNavmeshToolOption _option;
|
|
|
|
public TestNavmeshToolImpl()
|
|
{
|
|
_option = new TestNavmeshToolOption();
|
|
}
|
|
|
|
public string GetName()
|
|
{
|
|
return "Test Navmesh";
|
|
}
|
|
|
|
|
|
public void SetSample(Sample sample)
|
|
{
|
|
_sample = sample;
|
|
}
|
|
|
|
public Sample GetSample()
|
|
{
|
|
return _sample;
|
|
}
|
|
|
|
public TestNavmeshToolOption GetOption()
|
|
{
|
|
return _option;
|
|
}
|
|
|
|
public DtStatus FindFollowPath(long startRef, long endRef, RcVec3f startPt, RcVec3f endPt, IDtQueryFilter filter, bool enableRaycast,
|
|
ref List<long> polys, ref List<RcVec3f> smoothPath)
|
|
{
|
|
var navMesh = _sample.GetNavMesh();
|
|
var navQuery = _sample.GetNavMeshQuery();
|
|
|
|
navQuery.FindPath(startRef, endRef, startPt, endPt, filter, ref polys,
|
|
new DtFindPathOption(enableRaycast ? DtNavMeshQuery.DT_FINDPATH_ANY_ANGLE : 0, float.MaxValue));
|
|
|
|
if (0 >= polys.Count)
|
|
return DtStatus.DT_FAILURE;
|
|
|
|
// Iterate over the path to find smooth path on the detail mesh surface.
|
|
navQuery.ClosestPointOnPoly(startRef, startPt, out var iterPos, out var _);
|
|
navQuery.ClosestPointOnPoly(polys[polys.Count - 1], endPt, out var targetPos, out var _);
|
|
|
|
float STEP_SIZE = 0.5f;
|
|
float SLOP = 0.01f;
|
|
|
|
smoothPath.Clear();
|
|
smoothPath.Add(iterPos);
|
|
var visited = new List<long>();
|
|
|
|
// Move towards target a small advancement at a time until target reached or
|
|
// when ran out of memory to store the path.
|
|
while (0 < polys.Count && smoothPath.Count < MAX_SMOOTH)
|
|
{
|
|
// Find location to steer towards.
|
|
if (!PathUtils.GetSteerTarget(navQuery, iterPos, targetPos, SLOP,
|
|
polys, out var steerPos, out var steerPosFlag, out var steerPosRef))
|
|
{
|
|
break;
|
|
}
|
|
|
|
bool endOfPath = (steerPosFlag & DtNavMeshQuery.DT_STRAIGHTPATH_END) != 0
|
|
? true
|
|
: false;
|
|
bool offMeshConnection = (steerPosFlag & DtNavMeshQuery.DT_STRAIGHTPATH_OFFMESH_CONNECTION) != 0
|
|
? true
|
|
: false;
|
|
|
|
// Find movement delta.
|
|
RcVec3f delta = steerPos.Subtract(iterPos);
|
|
float len = (float)Math.Sqrt(RcVec3f.Dot(delta, delta));
|
|
// If the steer target is end of path or off-mesh link, do not move past the location.
|
|
if ((endOfPath || offMeshConnection) && len < STEP_SIZE)
|
|
{
|
|
len = 1;
|
|
}
|
|
else
|
|
{
|
|
len = STEP_SIZE / len;
|
|
}
|
|
|
|
RcVec3f moveTgt = RcVec3f.Mad(iterPos, delta, len);
|
|
|
|
// Move
|
|
navQuery.MoveAlongSurface(polys[0], iterPos, moveTgt, filter, out var result, ref visited);
|
|
|
|
iterPos = result;
|
|
|
|
polys = PathUtils.MergeCorridorStartMoved(polys, visited);
|
|
polys = PathUtils.FixupShortcuts(polys, navQuery);
|
|
|
|
var status = navQuery.GetPolyHeight(polys[0], result, out var h);
|
|
if (status.Succeeded())
|
|
{
|
|
iterPos.y = h;
|
|
}
|
|
|
|
// Handle end of path and off-mesh links when close enough.
|
|
if (endOfPath && PathUtils.InRange(iterPos, steerPos, SLOP, 1.0f))
|
|
{
|
|
// Reached end of path.
|
|
iterPos = targetPos;
|
|
if (smoothPath.Count < MAX_SMOOTH)
|
|
{
|
|
smoothPath.Add(iterPos);
|
|
}
|
|
|
|
break;
|
|
}
|
|
else if (offMeshConnection && PathUtils.InRange(iterPos, steerPos, SLOP, 1.0f))
|
|
{
|
|
// Reached off-mesh connection.
|
|
RcVec3f startPos = RcVec3f.Zero;
|
|
RcVec3f endPos = RcVec3f.Zero;
|
|
|
|
// Advance the path up to and over the off-mesh connection.
|
|
long prevRef = 0;
|
|
long polyRef = polys[0];
|
|
int npos = 0;
|
|
while (npos < polys.Count && polyRef != steerPosRef)
|
|
{
|
|
prevRef = polyRef;
|
|
polyRef = polys[npos];
|
|
npos++;
|
|
}
|
|
|
|
polys = polys.GetRange(npos, polys.Count - npos);
|
|
|
|
// Handle the connection.
|
|
var status2 = navMesh.GetOffMeshConnectionPolyEndPoints(prevRef, polyRef, ref startPos, ref endPos);
|
|
if (status2.Succeeded())
|
|
{
|
|
if (smoothPath.Count < MAX_SMOOTH)
|
|
{
|
|
smoothPath.Add(startPos);
|
|
// Hack to make the dotted path not visible during off-mesh connection.
|
|
if ((smoothPath.Count & 1) != 0)
|
|
{
|
|
smoothPath.Add(startPos);
|
|
}
|
|
}
|
|
|
|
// Move position at the other side of the off-mesh link.
|
|
iterPos = endPos;
|
|
navQuery.GetPolyHeight(polys[0], iterPos, out var eh);
|
|
iterPos.y = eh;
|
|
}
|
|
}
|
|
|
|
// Store results.
|
|
if (smoothPath.Count < MAX_SMOOTH)
|
|
{
|
|
smoothPath.Add(iterPos);
|
|
}
|
|
}
|
|
|
|
return DtStatus.DT_SUCCSESS;
|
|
}
|
|
|
|
public DtStatus FindStraightPath(long startRef, long endRef, RcVec3f startPt, RcVec3f endPt, IDtQueryFilter filter, bool enableRaycast,
|
|
ref List<long> polys, ref List<StraightPathItem> straightPath, int straightPathOptions)
|
|
{
|
|
var navQuery = _sample.GetNavMeshQuery();
|
|
|
|
navQuery.FindPath(startRef, endRef, startPt, endPt, filter, ref polys,
|
|
new DtFindPathOption(enableRaycast ? DtNavMeshQuery.DT_FINDPATH_ANY_ANGLE : 0, float.MaxValue));
|
|
|
|
if (0 >= polys.Count)
|
|
return DtStatus.DT_FAILURE;
|
|
|
|
// In case of partial path, make sure the end point is clamped to the last polygon.
|
|
var epos = RcVec3f.Of(endPt.x, endPt.y, endPt.z);
|
|
if (polys[polys.Count - 1] != endRef)
|
|
{
|
|
var result = navQuery.ClosestPointOnPoly(polys[polys.Count - 1], endPt, out var closest, out var _);
|
|
if (result.Succeeded())
|
|
{
|
|
epos = closest;
|
|
}
|
|
}
|
|
|
|
navQuery.FindStraightPath(startPt, epos, polys, ref straightPath, MAX_POLYS, straightPathOptions);
|
|
|
|
return DtStatus.DT_SUCCSESS;
|
|
}
|
|
|
|
public DtStatus InitSlicedFindPath(long startRef, long endRef, RcVec3f startPos, RcVec3f m_epos, IDtQueryFilter filter, bool enableRaycast)
|
|
{
|
|
var navQuery = _sample.GetNavMeshQuery();
|
|
return navQuery.InitSlicedFindPath(startRef, endRef, startPos, m_epos, filter,
|
|
enableRaycast ? DtNavMeshQuery.DT_FINDPATH_ANY_ANGLE : 0,
|
|
float.MaxValue
|
|
);
|
|
}
|
|
|
|
public DtStatus Raycast(long startRef, RcVec3f startPos, RcVec3f endPos, IDtQueryFilter filter,
|
|
ref List<long> polys, ref List<StraightPathItem> straightPath, out RcVec3f hitPos, out RcVec3f hitNormal, out bool hitResult)
|
|
{
|
|
hitPos = RcVec3f.Zero;
|
|
hitNormal = RcVec3f.Zero;
|
|
hitResult = false;
|
|
|
|
var navQuery = _sample.GetNavMeshQuery();
|
|
var status = navQuery.Raycast(startRef, startPos, endPos, filter, 0, 0, out var rayHit);
|
|
if (!status.Succeeded())
|
|
return status;
|
|
|
|
polys = rayHit.path;
|
|
if (rayHit.t > 1)
|
|
{
|
|
// No hit
|
|
hitPos = endPos;
|
|
hitResult = false;
|
|
}
|
|
else
|
|
{
|
|
// Hit
|
|
hitPos = RcVec3f.Lerp(startPos, endPos, rayHit.t);
|
|
hitNormal = rayHit.hitNormal;
|
|
hitResult = true;
|
|
}
|
|
|
|
// Adjust height.
|
|
if (rayHit.path.Count > 0)
|
|
{
|
|
var result = navQuery.GetPolyHeight(rayHit.path[rayHit.path.Count - 1], hitPos, out var h);
|
|
if (result.Succeeded())
|
|
{
|
|
hitPos.y = h;
|
|
}
|
|
}
|
|
|
|
straightPath.Clear();
|
|
straightPath.Add(new StraightPathItem(startPos, 0, 0));
|
|
straightPath.Add(new StraightPathItem(hitPos, 0, 0));
|
|
|
|
return status;
|
|
}
|
|
|
|
public DtStatus FindPolysAroundCircle(long startRef, RcVec3f spos, RcVec3f epos, IDtQueryFilter filter, ref List<long> resultRef, ref List<long> resultParent)
|
|
{
|
|
float dx = epos.x - spos.x;
|
|
float dz = epos.z - spos.z;
|
|
float dist = (float)Math.Sqrt(dx * dx + dz * dz);
|
|
|
|
List<float> costs = new List<float>();
|
|
var navQuery = _sample.GetNavMeshQuery();
|
|
|
|
return navQuery.FindPolysAroundCircle(startRef, spos, dist, filter, ref resultRef, ref resultParent, ref costs);
|
|
}
|
|
|
|
public DtStatus FindPolysAroundShape(long startRef, RcVec3f spos, RcVec3f epos, IDtQueryFilter filter, ref List<long> resultRef, ref List<long> resultParent, out RcVec3f[] queryPoly)
|
|
{
|
|
float nx = (epos.z - spos.z) * 0.25f;
|
|
float nz = -(epos.x - spos.x) * 0.25f;
|
|
float agentHeight = GetSample() != null ? GetSample().GetSettings().agentHeight : 0;
|
|
|
|
queryPoly = new RcVec3f[4];
|
|
queryPoly[0].x = spos.x + nx * 1.2f;
|
|
queryPoly[0].y = spos.y + agentHeight / 2;
|
|
queryPoly[0].z = spos.z + nz * 1.2f;
|
|
|
|
queryPoly[1].x = spos.x - nx * 1.3f;
|
|
queryPoly[1].y = spos.y + agentHeight / 2;
|
|
queryPoly[1].z = spos.z - nz * 1.3f;
|
|
|
|
queryPoly[2].x = epos.x - nx * 0.8f;
|
|
queryPoly[2].y = epos.y + agentHeight / 2;
|
|
queryPoly[2].z = epos.z - nz * 0.8f;
|
|
|
|
queryPoly[3].x = epos.x + nx;
|
|
queryPoly[3].y = epos.y + agentHeight / 2;
|
|
queryPoly[3].z = epos.z + nz;
|
|
|
|
var costs = new List<float>();
|
|
var navQuery = _sample.GetNavMeshQuery();
|
|
return navQuery.FindPolysAroundShape(startRef, queryPoly, filter, ref resultRef, ref resultParent, ref costs);
|
|
}
|
|
|
|
public DtStatus FindRandomPointAroundCircle(long startRef, RcVec3f spos, RcVec3f epos, IDtQueryFilter filter, bool constrainByCircle, int count, ref List<RcVec3f> points)
|
|
{
|
|
float dx = epos.x - spos.x;
|
|
float dz = epos.z - spos.z;
|
|
float dist = (float)Math.Sqrt(dx * dx + dz * dz);
|
|
|
|
IPolygonByCircleConstraint constraint = constrainByCircle
|
|
? StrictPolygonByCircleConstraint.Strict
|
|
: NoOpPolygonByCircleConstraint.Noop;
|
|
|
|
var frand = new FRand();
|
|
var navQuery = _sample.GetNavMeshQuery();
|
|
|
|
int prevCnt = points.Count;
|
|
|
|
while (0 < count && points.Count < prevCnt + count)
|
|
{
|
|
var status = navQuery.FindRandomPointAroundCircle(startRef, spos, dist, filter, frand, constraint,
|
|
out var randomRef, out var randomPt);
|
|
|
|
if (status.Succeeded())
|
|
{
|
|
points.Add(randomPt);
|
|
}
|
|
}
|
|
|
|
return DtStatus.DT_SUCCSESS;
|
|
}
|
|
}
|
|
} |