private void LoadNavMeshFromFile(string path)
{
try
{
tiledNavMesh = new NavMeshJsonSerializer().Deserialize(path);
navMeshQuery = new NavMeshQuery(tiledNavMesh, 2048);
hasGenerated = true;
displayMode = DisplayMode.NavMesh;
}
catch (Exception e)
{
if (!interceptExceptions)
throw;
else
{
hasGenerated = false;
tiledNavMesh = null;
navMeshQuery = null;
Console.WriteLine("Navmesh loading failed with exception:" + Environment.NewLine + e.ToString());
}
}
}
private bool GetSteerTarget(NavMeshQuery navMeshQuery, SVector3 startPos, SVector3 endPos, float minTargetDist, SharpNav.Pathfinding.Path path,
ref SVector3 steerPos, ref StraightPathFlags steerPosFlag, ref NavPolyId steerPosRef)
{
StraightPath steerPath = new StraightPath();
navMeshQuery.FindStraightPath(startPos, endPos, path, steerPath, 0);
int nsteerPath = steerPath.Count;
if (nsteerPath == 0)
return false;
//find vertex far enough to steer to
int ns = 0;
while (ns < nsteerPath)
{
if ((steerPath[ns].Flags & StraightPathFlags.OffMeshConnection) != 0 ||
!InRange(steerPath[ns].Point.Position, startPos, minTargetDist, 1000.0f))
break;
ns++;
}
//failed to find good point to steer to
if (ns >= nsteerPath)
return false;
steerPos = steerPath[ns].Point.Position;
steerPos.Y = startPos.Y;
steerPosFlag = steerPath[ns].Flags;
if (steerPosFlag == StraightPathFlags.None && ns == (nsteerPath - 1))
steerPosFlag = StraightPathFlags.End; // otherwise seeks path infinitely!!!
steerPosRef = steerPath[ns].Point.Polygon;
return true;
}
private bool GetSteerTarget(NavMeshQuery navMeshQuery, SVector3 startPos, SVector3 endPos, float minTargetDist, int[] path, int pathSize,
ref SVector3 steerPos, ref int steerPosFlag, ref int steerPosRef)
{
int MAX_STEER_POINTS = 3;
SVector3[] steerPath = new SVector3[MAX_STEER_POINTS];
int[] steerPathFlags = new int[MAX_STEER_POINTS];
int[] steerPathPolys = new int[MAX_STEER_POINTS];
int nsteerPath = 0;
navMeshQuery.FindStraightPath(startPos, endPos, path, pathSize,
steerPath, steerPathFlags, steerPathPolys, ref nsteerPath, MAX_STEER_POINTS, 0);
if (nsteerPath == 0)
return false;
//find vertex far enough to steer to
int ns = 0;
while (ns < nsteerPath)
{
if ((steerPathFlags[ns] & PathfindingCommon.STRAIGHTPATH_OFFMESH_CONNECTION) != 0 ||
!InRange(steerPath[ns], startPos, minTargetDist, 1000.0f))
break;
ns++;
}
//failed to find good point to steer to
if (ns >= nsteerPath)
return false;
steerPos = steerPath[ns];
steerPos.Y = startPos.Y;
steerPosFlag = steerPathFlags[ns];
steerPosRef = steerPathPolys[ns];
return true;
}
private void GeneratePathfinding()
{
if (!hasGenerated)
return;
Random rand = new Random();
NavQueryFilter filter = new NavQueryFilter();
buildData = new NavMeshBuilder(polyMesh, polyMeshDetail, new SharpNav.Pathfinding.OffMeshConnection[0], settings);
tiledNavMesh = new TiledNavMesh(buildData);
navMeshQuery = new NavMeshQuery(tiledNavMesh, 2048);
//Find random start and end points on the poly mesh
/*int startRef;
navMeshQuery.FindRandomPoint(out startRef, out startPos);*/
SVector3 c = new SVector3(10, 0, 0);
SVector3 e = new SVector3(5, 5, 5);
navMeshQuery.FindNearestPoly(ref c, ref e, out startPt);
navMeshQuery.FindRandomPointAroundCircle(ref startPt, 1000, out endPt);
//calculate the overall path, which contains an array of polygon references
int MAX_POLYS = 256;
path = new Path();
navMeshQuery.FindPath(ref startPt, ref endPt, filter, path);
//find a smooth path over the mesh surface
int npolys = path.Count;
SVector3 iterPos = new SVector3();
SVector3 targetPos = new SVector3();
navMeshQuery.ClosestPointOnPoly(startPt.Polygon, startPt.Position, ref iterPos);
navMeshQuery.ClosestPointOnPoly(path[npolys - 1], endPt.Position, ref targetPos);
smoothPath = new List<SVector3>(2048);
smoothPath.Add(iterPos);
float STEP_SIZE = 0.5f;
float SLOP = 0.01f;
while (npolys > 0 && smoothPath.Count < smoothPath.Capacity)
{
//find location to steer towards
SVector3 steerPos = new SVector3();
StraightPathFlags steerPosFlag = 0;
NavPolyId steerPosRef = NavPolyId.Null;
if (!GetSteerTarget(navMeshQuery, iterPos, targetPos, SLOP, path, ref steerPos, ref steerPosFlag, ref steerPosRef))
break;
bool endOfPath = (steerPosFlag & StraightPathFlags.End) != 0 ? true : false;
bool offMeshConnection = (steerPosFlag & StraightPathFlags.OffMeshConnection) != 0 ? true : false;
//find movement delta
SVector3 delta = steerPos - iterPos;
float len = (float)Math.Sqrt(SVector3.Dot(delta, delta));
//if steer target is at end of path or off-mesh link
//don't move past location
if ((endOfPath || offMeshConnection) && len < STEP_SIZE)
len = 1;
else
len = STEP_SIZE / len;
SVector3 moveTgt = new SVector3();
VMad(ref moveTgt, iterPos, delta, len);
//move
SVector3 result = new SVector3();
List<NavPolyId> visited = new List<NavPolyId>(16);
NavPoint startPoint = new NavPoint(path[0], iterPos);
navMeshQuery.MoveAlongSurface(ref startPoint, ref moveTgt, out result, visited);
path.FixupCorridor(visited);
npolys = path.Count;
float h = 0;
navMeshQuery.GetPolyHeight(path[0], result, ref h);
result.Y = h;
iterPos = result;
//handle end of path when close enough
if (endOfPath && InRange(iterPos, steerPos, SLOP, 1.0f))
{
//reached end of path
iterPos = targetPos;
if (smoothPath.Count < smoothPath.Capacity)
{
smoothPath.Add(iterPos);
}
break;
}
//store results
if (smoothPath.Count < smoothPath.Capacity)
{
smoothPath.Add(iterPos);
}
}
}
private void LoadNavMeshFromFile(string path)
{
tiledNavMesh = new NavMeshJsonSerializer().Deserialize(path);
navMeshQuery = new NavMeshQuery(tiledNavMesh, 2048);
hasGenerated = true;
displayMode = DisplayMode.NavMesh;
}