// navmesh
public static bool CanSee(actors.area.Zone zone, float x1, float y1, float z1, float x2, float y2, float z2)
{
// todo: prolly shouldnt raycast
var navMesh = zone.tiledNavMesh;
if (navMesh != null)
{
var navMeshQuery = zone.navMeshQuery;
NavPoint startPt, endPt;
SharpNav.Pathfinding.Path path = new SharpNav.Pathfinding.Path();
RaycastHit hit = new RaycastHit();
SharpNav.Geometry.Vector3 c = new SharpNav.Geometry.Vector3(x1, y1, z1);
SharpNav.Geometry.Vector3 ep = new SharpNav.Geometry.Vector3(x2, y2, z2);
SharpNav.Geometry.Vector3 e = new SharpNav.Geometry.Vector3(5, 5, 5);
navMeshQuery.FindNearestPoly(ref c, ref e, out startPt);
navMeshQuery.FindNearestPoly(ref ep, ref e, out endPt);
if (navMeshQuery.Raycast(ref startPt, ref ep, RaycastOptions.None, out hit, path))
{
return(true);
}
return(false);
}
return(true);
}
public bool GetPathResult(int index, out Path path)
{
path = null;
for (int i = 0; i < MaxQueue; i++)
{
if (queue[i].Index == index)
{
PathQuery q = queue[i];
//free request for reuse
q.Index = 0;
q.Status = 0;
path = new Path(q.Path);
queue[i] = q;
return true;
}
}
return false;
}
/// <summary>
/// Update the vertices on the straight path
/// </summary>
/// <param name="startIdx">Original path's starting index</param>
/// <param name="endIdx">Original path's end index</param>
/// <param name="endPos">The end position</param>
/// <param name="path">The original path of polygon references</param>
/// <param name="straightPath">An array of points on the straight path</param>
/// <param name="straightPathFlags">An array of flags</param>
/// <param name="straightPathRefs">An array of polygon references</param>
/// <param name="straightPathCount">The number of points on the path</param>
/// <param name="maxStraightPath">The maximum length allowed for the straight path</param>
/// <param name="options">Options flag</param>
/// <returns></returns>
public bool AppendPortals(int startIdx, int endIdx, Vector3 endPos, Path path, StraightPath straightPath, PathBuildFlags options)
{
Vector3 startPos = straightPath[straightPath.Count - 1].Point.Position;
//append or update last vertex
bool stat = false;
for (int i = startIdx; i < endIdx; i++)
{
//calculate portal
NavPolyId from = path[i];
NavTile fromTile;
NavPoly fromPoly;
if (nav.TryGetTileAndPolyByRef(from, out fromTile, out fromPoly) == false)
return false;
NavPolyId to = path[i + 1];
NavTile toTile;
NavPoly toPoly;
if (nav.TryGetTileAndPolyByRef(to, out toTile, out toPoly) == false)
return false;
Vector3 left = new Vector3();
Vector3 right = new Vector3();
if (GetPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, ref left, ref right) == false)
break;
if ((options & PathBuildFlags.AreaCrossingVertices) != 0)
{
//skip intersection if only area crossings are requested
if (fromPoly.Area == toPoly.Area)
continue;
}
//append intersection
float s, t;
if (Intersection.SegmentSegment2D(ref startPos, ref endPos, ref left, ref right, out s, out t))
{
Vector3 pt = Vector3.Lerp(left, right, t);
stat = straightPath.AppendVertex(new StraightPathVertex(new NavPoint(path[i + 1], pt), StraightPathFlags.None));
if (stat != true)
return true;
}
}
return true;
}
/// <summary>
/// Merge two paths when a shorter path is found
/// </summary>
/// <param name="path">The current path</param>
/// <param name="npath">Current path length</param>
/// <param name="maxPath">Maximum path length allowed</param>
/// <param name="visited">The visited polygons</param>
/// <param name="nvisited">Visited path length</param>
/// <returns>New path length</returns>
public int MergeCorridorStartShortcut(Path corridorPath, Path visitedPath)
{
int furthestPath = -1;
int furthestVisited = -1;
//find furthest common polygon
for (int i = corridorPath.Count - 1; i >= 0; i--)
{
bool found = false;
for (int j = visitedPath.Count - 1; j >= 0; j--)
{
if (path[i] == visitedPath[j])
{
furthestPath = i;
furthestVisited = j;
found = true;
}
}
if (found)
break;
}
//if no intersection found, return current path
if (furthestPath == -1 || furthestVisited == -1)
return corridorPath.Count;
//concatenate paths
//adjust beginning of the buffer to include the visited
int req = furthestVisited;
if (req <= 0)
return corridorPath.Count;
int orig = furthestPath;
int size = Math.Max(0, corridorPath.Count - orig);
if (req + size > path.Count)
size = path.Count - req;
for (int i = 0; i < size; i++)
path[req + i] = path[orig + i];
//store visited
for (int i = 0; i < req; i++)
path[i] = visitedPath[i];
return req + size;
}
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);
}
public Path(Path otherPath)
: this()
{
polys.AddRange(otherPath.polys);
cost = otherPath.Cost;
}
/// <summary>
/// Save the sliced path
/// </summary>
/// <param name="path">The path in terms of polygon references</param>
/// <param name="pathCount">The path length</param>
/// <param name="maxPath">The maximum path length allowed</param>
/// <returns>True if the path is saved, false if not</returns>
public bool FinalizeSlicedFindPath(Path path)
{
path.Clear();
if (query.Status == false)
{
query = new QueryData();
return false;
}
int n = 0;
if (query.Start.Polygon == query.End.Polygon)
{
//special case: the search starts and ends at the same poly
path.Add(query.Start.Polygon);
}
else
{
//reverse the path
NavNode prev = null;
NavNode node = query.LastBestNode;
NodeFlags prevRay = 0;
do
{
NavNode next = nodePool.GetNodeAtIdx(node.ParentIndex);
node.ParentIndex = nodePool.GetNodeIdx(prev);
prev = node;
NodeFlags nextRay = node.Flags & NodeFlags.ParentDetached;
node.Flags = (node.Flags & ~NodeFlags.ParentDetached) | prevRay;
prevRay = nextRay;
node = next;
}
while (node != null);
//store path
node = prev;
do
{
NavNode next = nodePool.GetNodeAtIdx(node.ParentIndex);
if ((node.Flags & NodeFlags.ParentDetached) != 0)
{
RaycastHit hit;
Path m = new Path();
NavPoint startPoint = new NavPoint(node.Id, node.Position);
bool result = Raycast(ref startPoint, ref next.Position, RaycastOptions.None, out hit, m);
path.AppendPath(m);
if (path[path.Count - 1] == next.Id)
path.RemoveAt(path.Count - 1);
}
else
{
path.Add(node.Id);
}
node = next;
}
while (node != null);
}
//reset query
query = new QueryData();
return true;
}
public bool Raycast(ref NavPoint startPoint, ref Vector3 endPos, NavPolyId prevRef, RaycastOptions options, out RaycastHit hit, Path hitPath)
{
hit = new RaycastHit();
if (hitPath != null)
hitPath.Clear();
//validate input
if (startPoint.Polygon == NavPolyId.Null || !nav.IsValidPolyRef(startPoint.Polygon))
return false;
if (prevRef != NavPolyId.Null && !nav.IsValidPolyRef(prevRef))
return false;
Vector3[] verts = new Vector3[PathfindingCommon.VERTS_PER_POLYGON];
NavTile prevTile, curTile, nextTile;
NavPoly prevPoly, curPoly, nextPoly;
NavPolyId curRef = startPoint.Polygon;
nav.TryGetTileAndPolyByRefUnsafe(curRef, out curTile, out curPoly);
nextTile = prevTile = curTile;
nextPoly = prevPoly = curPoly;
if (prevRef != NavPolyId.Null)
nav.TryGetTileAndPolyByRefUnsafe(prevRef, out prevTile, out prevPoly);
while (curRef != NavPolyId.Null)
{
//collect vertices
int nv = 0;
for (int i = 0; i < curPoly.VertCount; i++)
{
verts[nv] = curTile.Verts[curPoly.Verts[i]];
nv++;
}
float tmin, tmax;
int segMin, segMax;
if (!Intersection.SegmentPoly2D(startPoint.Position, endPos, verts, nv, out tmin, out tmax, out segMin, out segMax))
{
//could not hit the polygon, keep the old t and report hit
return true;
}
hit.EdgeIndex = segMax;
//keep track of furthest t so far
if (tmax > hit.T)
hit.T = tmax;
//store visited polygons
if (hitPath != null)
hitPath.Add(curRef);
//ray end is completely inside the polygon
if (segMax == -1)
{
hit.T = float.MaxValue;
return true;
}
//follow neighbors
NavPolyId nextRef = NavPolyId.Null;
foreach (Link link in curPoly.Links)
{
//find link which contains the edge
if (link.Edge != segMax)
continue;
//get pointer to the next polygon
nav.TryGetTileAndPolyByRefUnsafe(link.Reference, out nextTile, out nextPoly);
//skip off-mesh connection
if (nextPoly.PolyType == NavPolyType.OffMeshConnection)
continue;
//TODO QueryFilter
//if the link is internal, just return the ref
if (link.Side == BoundarySide.Internal)
{
nextRef = link.Reference;
break;
}
//if the link is at the tile boundary
//check if the link spans the whole edge and accept
if (link.BMin == 0 && link.BMax == 255)
{
nextRef = link.Reference;
break;
}
//check for partial edge links
int v0 = curPoly.Verts[link.Edge];
int v1 = curPoly.Verts[(link.Edge + 1) % curPoly.VertCount];
Vector3 left = curTile.Verts[v0];
Vector3 right = curTile.Verts[v1];
//check that the intersection lies inside the link portal
if (link.Side == BoundarySide.PlusX || link.Side == BoundarySide.MinusX)
{
//calculate link size
float s = 1.0f / 255.0f;
float lmin = left.Z + (right.Z - left.Z) * (link.BMin * s);
float lmax = left.Z + (right.Z - left.Z) * (link.BMax * s);
if (lmin > lmax)
{
//swap
float temp = lmin;
lmin = lmax;
lmax = temp;
}
//find z intersection
float z = startPoint.Position.Z + (endPos.Z - startPoint.Position.Z) * tmax;
if (z >= lmin && z <= lmax)
{
nextRef = link.Reference;
break;
}
}
else if (link.Side == BoundarySide.PlusZ || link.Side == BoundarySide.MinusZ)
{
//calculate link size
float s = 1.0f / 255.0f;
float lmin = left.X + (right.X - left.X) * (link.BMin * s);
float lmax = left.X + (right.X - left.X) * (link.BMax * s);
if (lmin > lmax)
{
//swap
float temp = lmin;
lmin = lmax;
lmax = temp;
}
//find x intersection
float x = startPoint.Position.X + (endPos.X - startPoint.Position.X) * tmax;
if (x >= lmin && x <= lmax)
{
nextRef = link.Reference;
break;
}
}
}
if ((options & RaycastOptions.UseCosts) != 0)
{
//TODO add cost
}
if (nextRef == NavPolyId.Null)
{
//no neighbor, we hit a wall
//calculate hit normal
int a = segMax;
int b = (segMax + 1) < nv ? segMax + 1 : 0;
Vector3 va = verts[a];
Vector3 vb = verts[b];
float dx = vb.X - va.X;
float dz = vb.Z - va.Z;
hit.Normal = new Vector3(dz, 0, dx);
hit.Normal.Normalize();
return true;
}
//no hit, advance to neighbor polygon
prevRef = curRef;
curRef = nextRef;
prevTile = curTile;
curTile = nextTile;
prevPoly = curPoly;
curPoly = nextPoly;
}
return true;
}
public void GeneratePathfinding()
{
/* 生成路径寻找
* 1. 根据polymesh
*
*/
Random rand = new Random();
NavQueryFilter filter = new NavQueryFilter();
//1.
buildData = new NavMeshBuilder(polyMesh, polyMeshDetail, new SharpNav.Pathfinding.OffMeshConnection[0], settings);
tiledNavMesh = new TiledNavMesh(buildData);
OutMesh();
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(21.4f, 0, -122f);
SVector3 e = new SVector3(5, 5, 5);
navMeshQuery.FindNearestPoly(ref c, ref e, out startPt);
//navMeshQuery.FindRandomPointAroundCircle(ref startPt, 1000, out endPt);
//endPt = new NavPoint(new NavPolyId(20), new SVector3(-10, -2, 10));
c = new SVector3(150.7f, 0, -59f);
e = new SVector3(5, 5, 5);
navMeshQuery.FindNearestPoly(ref c, ref e, out endPt);
//calculate the overall path, which contains an array of polygon references
int MAX_POLYS = 256;
path = new SharpNav.Pathfinding.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 = 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);
// }
//}
StraightPath corners = new StraightPath();
FindCorners(startPt.Position, endPt.Position, corners, navMeshQuery);
}
public void FindPath( FindPathContext context )
{
if( tiledNavMesh != null )
{
if( navMeshQuery == null )
navMeshQuery = new NavMeshQuery( tiledNavMesh, PathfindingMaxNodes );
try
{
var extents = ToSharpNav( context.PolygonPickExtents, true );
var startPt = navMeshQuery.FindNearestPoly( ToSharpNav( context.Start ), extents );
var endPt = navMeshQuery.FindNearestPoly( ToSharpNav( context.End ), extents );
var filter = new NavQueryFilter();
var path = new SharpNav.Pathfinding.Path();
var found = navMeshQuery.FindPath( ref startPt, ref endPt, filter, path );
Vector3[] pathResult = null;
if( found )
{
//find a smooth path over the mesh surface
int npolys = path.Count;
SharpNav.Geometry.Vector3 iterPos = new SharpNav.Geometry.Vector3();
SharpNav.Geometry.Vector3 targetPos = new SharpNav.Geometry.Vector3();
navMeshQuery.ClosestPointOnPoly( startPt.Polygon, startPt.Position, ref iterPos );
navMeshQuery.ClosestPointOnPoly( path[ npolys - 1 ], endPt.Position, ref targetPos );
var smoothPath = new List<SharpNav.Geometry.Vector3>( context.MaxSmoothPath );
smoothPath.Add( iterPos );
//for( int n = 0; n < path.Count; n++ )
//{
// Vector3 closest = Vector3.Zero;
// if( navMeshQuery.ClosestPointOnPoly( path[ n ], startPt.Position, ref closest ) )
// smoothPath.Add( closest );
//}
float stepSize = (float)context.StepSize;
float slop = (float)context.Slop;
while( npolys > 0 && smoothPath.Count < smoothPath.Capacity )
{
//find location to steer towards
SharpNav.Geometry.Vector3 steerPos = new SharpNav.Geometry.Vector3();
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
SharpNav.Geometry.Vector3 delta = steerPos - iterPos;
float len = (float)Math.Sqrt( SharpNav.Geometry.Vector3.Dot( delta, delta ) );
//if steer target is at end of path or off-mesh link
//don't move past location
if( ( endOfPath || offMeshConnection ) && len < stepSize )
len = 1;
else
len = stepSize / len;
SharpNav.Geometry.Vector3 moveTgt = new SharpNav.Geometry.Vector3();
VMad( ref moveTgt, iterPos, delta, len );
//move
SharpNav.Geometry.Vector3 result = new SharpNav.Geometry.Vector3();
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 );
}
pathResult = new Vector3[ smoothPath.Count ];
for( int n = 0; n < pathResult.Length; n++ )
pathResult[ n ] = ToEngine( smoothPath[ n ] );
//if( pathCount > 0 )
//{
// path = new Vec3[ pathCount ];
// //!!!!double support
// for( int n = 0; n < pathCount; n++ )
// path[ n ] = ToEngineVec3( pathPointer[ n ] );
//}
//else
// path = new Vec3[] { context.Start };
}
context.Path = pathResult;
}
catch( Exception e )
{
context.Error = e.Message;
}
}
else
context.Error = "No NavMesh";
context.Finished = true;
}
private bool GetSteerTarget(
NavMeshQuery navMeshQuery,
SVector3 startPos,
SVector3 endPos,
float minTargetDist,
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;
}
public PathCorridor()
{
this.path = new Path();
}
/// <summary>
/// The current corridor position is expected to be within the first polygon in the path. The target
/// is expected to be in the last polygon.
/// </summary>
/// <param name="target">The target</param>
/// <param name="path">The polygon path</param>
public void SetCorridor(Vector3 target, Path path)
{
this.target = target;
this.path = path;
}
/// <summary>
/// Use an efficient local visibility search to try to optimize the corridor between the current position and the next.
/// </summary>
/// <param name="next">The next postion</param>
/// <param name="pathOptimizationRange">The range</param>
/// <param name="navquery">The NavMeshQuery</param>
public void OptimizePathVisibility(Vector3 next, float pathOptimizationRange, NavMeshQuery navquery)
{
//clamp the ray to max distance
Vector3 goal = next;
float dist = Vector3Extensions.Distance2D(pos, goal);
//if too close to the goal, do not try to optimize
if (dist < 0.01f)
return;
dist = Math.Min(dist + 0.01f, pathOptimizationRange);
//adjust ray length
Vector3 delta = goal - pos;
goal = pos + delta * (pathOptimizationRange / dist);
NavPoint startPoint = new NavPoint(path[0], pos);
Path raycastPath = new Path();
RaycastHit hit;
navquery.Raycast(ref startPoint, ref goal, RaycastOptions.None, out hit, raycastPath);
if (raycastPath.Count > 1 && hit.T > 0.99f)
{
MergeCorridorStartShortcut(raycastPath, raycastPath);
}
}
/// <summary>
/// Use a local area path search to try to reoptimize this corridor
/// </summary>
/// <param name="navquery">The NavMeshQuery</param>
/// <returns>True if optimized, false if not</returns>
public bool OptimizePathTopology(NavMeshQuery navquery, NavQueryFilter filter)
{
if (path.Count < 3)
return false;
const int MaxIter = 32;
const int MaxRes = 32;
Path res = new Path();
int numRes = 0;
int tempInt = 0;
NavPoint startPoint = new NavPoint(path[0], pos);
NavPoint endPoint = new NavPoint(path[path.Count - 1], target);
navquery.InitSlicedFindPath(ref startPoint, ref endPoint, filter, FindPathOptions.None);
navquery.UpdateSlicedFindPath(MaxIter, ref tempInt);
bool status = navquery.FinalizedSlicedPathPartial(path, res);
if (status == true && numRes > 0)
{
MergeCorridorStartShortcut(path, res);
return true;
}
return false;
}
/// <summary>
/// Add vertices and portals to a regular path computed from the method FindPath().
/// </summary>
/// <param name="startPos">Starting position</param>
/// <param name="endPos">Ending position</param>
/// <param name="path">Path of polygon references</param>
/// <param name="pathSize">Length of path</param>
/// <param name="straightPath">An array of points on the straight path</param>
/// <param name="straightPathFlags">An array of flags</param>
/// <param name="straightPathRefs">An array of polygon references</param>
/// <param name="straightPathCount">The number of points on the path</param>
/// <param name="maxStraightPath">The maximum length allowed for the straight path</param>
/// <param name="options">Options flag</param>
/// <returns>True, if path found. False, if otherwise.</returns>
public bool FindStraightPath(Vector3 startPos, Vector3 endPos, Path path, StraightPath straightPath, PathBuildFlags options)
{
straightPath.Clear();
if (path.Count == 0)
return false;
bool stat = false;
Vector3 closestStartPos = new Vector3();
ClosestPointOnPolyBoundary(path[0], startPos, ref closestStartPos);
Vector3 closestEndPos = new Vector3();
ClosestPointOnPolyBoundary(path[path.Count - 1], endPos, ref closestEndPos);
stat = straightPath.AppendVertex(new StraightPathVertex(new NavPoint(path[0], closestStartPos), StraightPathFlags.Start));
if (!stat)
return true;
if (path.Count > 1)
{
Vector3 portalApex = closestStartPos;
Vector3 portalLeft = portalApex;
Vector3 portalRight = portalApex;
int apexIndex = 0;
int leftIndex = 0;
int rightIndex = 0;
NavPolyType leftPolyType = 0;
NavPolyType rightPolyType = 0;
NavPolyId leftPolyRef = path[0];
NavPolyId rightPolyRef = path[0];
for (int i = 0; i < path.Count; i++)
{
Vector3 left = new Vector3();
Vector3 right = new Vector3();
NavPolyType fromType = 0, toType = 0;
if (i + 1 < path.Count)
{
//next portal
if (GetPortalPoints(path[i], path[i + 1], ref left, ref right, ref fromType, ref toType) == false)
{
//failed to get portal points means path[i + 1] is an invalid polygon
//clamp end point to path[i] and return path so far
if (ClosestPointOnPolyBoundary(path[i], endPos, ref closestEndPos) == false)
{
//first polygon is invalid
return false;
}
if ((options & (PathBuildFlags.AreaCrossingVertices | PathBuildFlags.AllCrossingVertices)) != 0)
{
//append portals
stat = AppendPortals(apexIndex, i, closestEndPos, path, straightPath, options);
}
stat = straightPath.AppendVertex(new StraightPathVertex(new NavPoint(path[i], closestEndPos), StraightPathFlags.None));
return true;
}
//if starting really close to the portal, advance
if (i == 0)
{
float t;
if (Distance.PointToSegment2DSquared(ref portalApex, ref left, ref right, out t) < 0.001 * 0.001)
continue;
}
}
else
{
//end of the path
left = closestEndPos;
right = closestEndPos;
fromType = toType = NavPolyType.Ground;
}
//right vertex
float triArea2D;
Triangle3.Area2D(ref portalApex, ref portalRight, ref right, out triArea2D);
if (triArea2D <= 0.0)
{
Triangle3.Area2D(ref portalApex, ref portalLeft, ref right, out triArea2D);
if (portalApex == portalRight || triArea2D > 0.0)
{
portalRight = right;
rightPolyRef = (i + 1 < path.Count) ? path[i + 1] : NavPolyId.Null;
rightPolyType = toType;
rightIndex = i;
}
else
{
//append portals along current straight path segment
if ((options & (PathBuildFlags.AreaCrossingVertices | PathBuildFlags.AllCrossingVertices)) != 0)
{
stat = AppendPortals(apexIndex, leftIndex, portalLeft, path, straightPath, options);
if (stat != true)
return true;
}
portalApex = portalLeft;
apexIndex = leftIndex;
StraightPathFlags flags = 0;
if (leftPolyRef == NavPolyId.Null)
flags = StraightPathFlags.End;
else if (leftPolyType == NavPolyType.OffMeshConnection)
flags = StraightPathFlags.OffMeshConnection;
NavPolyId reference = leftPolyRef;
//append or update vertex
stat = straightPath.AppendVertex(new StraightPathVertex(new NavPoint(reference, portalApex), flags));
if (stat != true)
return true;
portalLeft = portalApex;
portalRight = portalApex;
leftIndex = apexIndex;
rightIndex = apexIndex;
//restart
i = apexIndex;
continue;
}
}
//left vertex
Triangle3.Area2D(ref portalApex, ref portalLeft, ref left, out triArea2D);
if (triArea2D >= 0.0)
{
Triangle3.Area2D(ref portalApex, ref portalRight, ref left, out triArea2D);
if (portalApex == portalLeft || triArea2D < 0.0f)
{
portalLeft = left;
leftPolyRef = (i + 1 < path.Count) ? path[i + 1] : NavPolyId.Null;
leftPolyType = toType;
leftIndex = i;
}
else
{
if ((options & (PathBuildFlags.AreaCrossingVertices | PathBuildFlags.AllCrossingVertices)) != 0)
{
stat = AppendPortals(apexIndex, rightIndex, portalRight, path, straightPath, options);
if (stat != true)
return true;
}
portalApex = portalRight;
apexIndex = rightIndex;
StraightPathFlags flags = 0;
if (rightPolyRef == NavPolyId.Null)
flags = StraightPathFlags.End;
else if (rightPolyType == NavPolyType.OffMeshConnection)
flags = StraightPathFlags.OffMeshConnection;
NavPolyId reference = rightPolyRef;
//append or update vertex
stat = straightPath.AppendVertex(new StraightPathVertex(new NavPoint(reference, portalApex), flags));
if (stat != true)
return true;
portalLeft = portalApex;
portalRight = portalApex;
leftIndex = apexIndex;
rightIndex = apexIndex;
//restart
i = apexIndex;
continue;
}
}
}
//append portals along the current straight line segment
if ((options & (PathBuildFlags.AreaCrossingVertices | PathBuildFlags.AllCrossingVertices)) != 0)
{
stat = AppendPortals(apexIndex, path.Count - 1, closestEndPos, path, straightPath, options);
if (stat != true)
return true;
}
}
stat = straightPath.AppendVertex(new StraightPathVertex(new NavPoint(NavPolyId.Null, closestEndPos), StraightPathFlags.End));
return true;
}
public bool Raycast(ref NavPoint startPoint, ref Vector3 endPos, RaycastOptions options, out RaycastHit hit, Path hitPath)
{
return Raycast(ref startPoint, ref endPos, NavPolyId.Null, options, out hit, hitPath);
}
//为了使接口适应而附加的
public List <Vector2> SmothPathfinding(Vector2 start, Vector2 target)
{
Random rand = new Random();
SVector3 startVector = new SVector3(start.x_, 0, start.y_);
SVector3 endVector = new SVector3(target.x_, 0, target.y_);
NavQueryFilter filter = new NavQueryFilter();
//buildData = new NavMeshBuilder(polyMesh, polyMeshDetail, new SharpNav.Pathfinding.OffMeshConnection[0], settings);
//tiledNavMesh = new TiledNavMesh(buildData);
//navMeshQuery = new NavMeshQuery(tiledNavMesh, 2048);
SVector3 extents = new SVector3(5, 5, 5);
navMeshQuery.FindNearestPoly(ref startVector, ref extents, out startPt);
navMeshQuery.FindNearestPoly(ref endVector, ref extents, out endPt);
path = new SharpNav.Pathfinding.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 = 2f;
float SLOP = 0.1f;
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);
}
}
List <Vector2> _path = new List <Vector2>();
for (int i = 0; i < smoothPath.Count; i++)
{
_path.Add(new Vector2(smoothPath[i].X, smoothPath[i].Z));
}
return(_path);
}
/// <summary>
/// Update the sliced path as agents move across the path.
/// </summary>
/// <param name="maxIter">Maximum iterations</param>
/// <param name="doneIters">Number of times iterated through</param>
/// <returns>True if updated, false if not</returns>
public bool UpdateSlicedFindPath(int maxIter, ref int doneIters)
{
if (query.Status != true)
return query.Status;
//make sure the request is still valid
if (!nav.IsValidPolyRef(query.Start.Polygon) || !nav.IsValidPolyRef(query.End.Polygon))
{
query.Status = false;
return false;
}
int iter = 0;
while (iter < maxIter && !openList.Empty())
{
iter++;
//remove node from open list and put it in closed list
NavNode bestNode = openList.Pop();
SetNodeFlagClosed(ref bestNode);
//reached the goal, stop searching
if (bestNode.Id == query.End.Polygon)
{
query.LastBestNode = bestNode;
query.Status = true;
doneIters = iter;
return query.Status;
}
//get current poly and tile
NavPolyId bestRef = bestNode.Id;
NavTile bestTile;
NavPoly bestPoly;
if (!nav.TryGetTileAndPolyByRef(bestRef, out bestTile, out bestPoly))
{
//the polygon has disappeared during the sliced query, fail
query.Status = false;
doneIters = iter;
return query.Status;
}
//get parent poly and tile
NavPolyId parentRef = NavPolyId.Null;
NavPolyId grandpaRef = NavPolyId.Null;
NavTile parentTile = null;
NavPoly parentPoly = null;
NavNode parentNode = null;
if (bestNode.ParentIndex != 0)
{
parentNode = nodePool.GetNodeAtIdx(bestNode.ParentIndex);
parentRef = parentNode.Id;
if (parentNode.ParentIndex != 0)
grandpaRef = nodePool.GetNodeAtIdx(parentNode.ParentIndex).Id;
}
if (parentRef != NavPolyId.Null)
{
bool invalidParent = !nav.TryGetTileAndPolyByRef(parentRef, out parentTile, out parentPoly);
if (invalidParent || (grandpaRef != NavPolyId.Null && !nav.IsValidPolyRef(grandpaRef)))
{
//the polygon has disappeared during the sliced query, fail
query.Status = false;
doneIters = iter;
return query.Status;
}
}
//decide whether to test raycast to previous nodes
bool tryLOS = false;
if ((query.Options & FindPathOptions.AnyAngle) != 0)
{
if ((parentRef != NavPolyId.Null) && (parentNode.Position - bestNode.Position).LengthSquared() < query.RaycastLimitSquared)
tryLOS = true;
}
foreach (Link link in bestPoly.Links)
{
NavPolyId neighborRef = link.Reference;
//skip invalid ids and do not expand back to where we came from
if (neighborRef == NavPolyId.Null || neighborRef == parentRef)
continue;
//get neighbor poly and tile
NavTile neighborTile;
NavPoly neighborPoly;
nav.TryGetTileAndPolyByRefUnsafe(neighborRef, out neighborTile, out neighborPoly);
if (!query.Filter.PassFilter(neighborRef, neighborTile, neighborPoly))
continue;
NavNode neighborNode = nodePool.GetNode(neighborRef);
if (neighborNode == null)
continue;
if (neighborNode.ParentIndex != 0 && neighborNode.ParentIndex == bestNode.ParentIndex)
continue;
if (neighborNode.Flags == 0)
{
GetEdgeMidPoint(bestRef, bestPoly, bestTile, neighborRef, neighborPoly, neighborTile, ref neighborNode.Position);
}
//calculate cost and heuristic
float cost = 0;
float heuristic = 0;
bool foundShortCut = false;
RaycastHit hit;
Path hitPath = new Path();
if (tryLOS)
{
NavPoint startPoint = new NavPoint(parentRef, parentNode.Position);
Raycast(ref startPoint, ref neighborNode.Position, grandpaRef, RaycastOptions.UseCosts, out hit, hitPath);
foundShortCut = hit.T >= 1.0f;
}
if (foundShortCut)
{
cost = parentNode.PolyCost + hitPath.Cost;
}
else
{
float curCost = query.Filter.GetCost(bestNode.Position, neighborNode.Position,
parentRef, parentTile, parentPoly,
bestRef, bestTile, bestPoly,
neighborRef, neighborTile, neighborPoly);
cost = bestNode.PolyCost + curCost;
}
//special case for last node
if (neighborRef == query.End.Polygon)
{
//cost
float endCost = query.Filter.GetCost(bestNode.Position, neighborNode.Position,
bestRef, bestTile, bestPoly,
neighborRef, neighborTile, neighborPoly,
NavPolyId.Null, null, null);
cost = cost + endCost;
heuristic = 0;
}
else
{
heuristic = (neighborNode.Position - query.End.Position).Length() * HeuristicScale;
}
float total = cost + heuristic;
//the node is already in open list and new result is worse, skip
if (IsInOpenList(neighborNode) && total >= neighborNode.TotalCost)
continue;
//the node is already visited and processesd, and the new result is worse, skip
if (IsInClosedList(neighborNode) && total >= neighborNode.TotalCost)
continue;
//add or update the node
neighborNode.ParentIndex = nodePool.GetNodeIdx(bestNode);
neighborNode.Id = neighborRef;
neighborNode.Flags = RemoveNodeFlagClosed(neighborNode);
neighborNode.PolyCost = cost;
neighborNode.TotalCost = total;
if (foundShortCut)
neighborNode.Flags |= NodeFlags.ParentDetached;
if (IsInOpenList(neighborNode))
{
//already in open, update node location
openList.Modify(neighborNode);
}
else
{
//put the node in the open list
SetNodeFlagOpen(ref neighborNode);
openList.Push(neighborNode);
}
//update nearest node to target so far
if (heuristic < query.LastBestNodeCost)
{
query.LastBestNodeCost = heuristic;
query.LastBestNode = neighborNode;
}
}
}
//exhausted all nodes, but could not find path
if (openList.Empty())
{
query.Status = true;
}
doneIters = iter;
return query.Status;
}
//只找Corner
public StraightPath Pathfinding(SVector3 startVector, SVector3 endVector)
{
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);
SVector3 extents = new SVector3(5, 5, 5);
navMeshQuery.FindNearestPoly(ref startVector, ref extents, out startPt);
navMeshQuery.FindNearestPoly(ref endVector, ref extents, out endPt);
path = new SharpNav.Pathfinding.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 = 1f;
//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);
// }
//}
StraightPath corners = new StraightPath();
FindCorners(startVector, endVector, corners, navMeshQuery);
return(corners);
//return smoothPath;
}
/// <summary>
/// Find a path from the start polygon to the end polygon.
/// -If the end polygon can't be reached, the last polygon will be nearest the end polygon
/// -If the path array is too small, it will be filled as far as possible
/// -start and end positions are used to calculate traversal costs
/// </summary>
/// <param name="startPt">The start point.</param>
/// <param name="endPt">The end point.</param>
/// <param name="filter">A filter for the navmesh data.</param>
/// <param name="path">The path of polygon references</param>
/// <returns>True, if path found. False, if otherwise.</returns>
public bool FindPath(ref NavPoint startPt, ref NavPoint endPt, NavQueryFilter filter, Path path)
{
//reset path of polygons
path.Clear();
NavPolyId startRef = startPt.Polygon;
Vector3 startPos = startPt.Position;
NavPolyId endRef = endPt.Polygon;
Vector3 endPos = endPt.Position;
if (startRef == NavPolyId.Null || endRef == NavPolyId.Null)
return false;
//validate input
if (!nav.IsValidPolyRef(startRef) || !nav.IsValidPolyRef(endRef))
return false;
//special case: both start and end are in the same polygon
if (startRef == endRef)
{
path.Add(startRef);
return true;
}
nodePool.Clear();
openList.Clear();
//initial node is located at the starting position
NavNode startNode = nodePool.GetNode(startRef);
startNode.Position = startPos;
startNode.ParentIndex = 0;
startNode.PolyCost = 0;
startNode.TotalCost = (startPos - endPos).Length() * HeuristicScale;
startNode.Id = startRef;
startNode.Flags = NodeFlags.Open;
openList.Push(startNode);
NavNode lastBestNode = startNode;
float lastBestTotalCost = startNode.TotalCost;
while (openList.Count > 0)
{
//remove node from open list and put it in closed list
NavNode bestNode = openList.Pop();
SetNodeFlagClosed(ref bestNode);
//reached the goal. stop searching
if (bestNode.Id == endRef)
{
lastBestNode = bestNode;
break;
}
//get current poly and tile
NavPolyId bestRef = bestNode.Id;
NavTile bestTile;
NavPoly bestPoly;
nav.TryGetTileAndPolyByRefUnsafe(bestRef, out bestTile, out bestPoly);
//get parent poly and tile
NavPolyId parentRef = NavPolyId.Null;
NavTile parentTile = null;
NavPoly parentPoly = null;
if (bestNode.ParentIndex != 0)
parentRef = nodePool.GetNodeAtIdx(bestNode.ParentIndex).Id;
if (parentRef != NavPolyId.Null)
nav.TryGetTileAndPolyByRefUnsafe(parentRef, out parentTile, out parentPoly);
//examine neighbors
foreach (Link link in bestPoly.Links)
{
NavPolyId neighborRef = link.Reference;
//skip invalid ids and do not expand back to where we came from
if (neighborRef == NavPolyId.Null || neighborRef == parentRef)
continue;
//get neighbor poly and tile
NavTile neighborTile;
NavPoly neighborPoly;
nav.TryGetTileAndPolyByRefUnsafe(neighborRef, out neighborTile, out neighborPoly);
NavNode neighborNode = nodePool.GetNode(neighborRef);
if (neighborNode == null)
continue;
//if node is visited the first time, calculate node position
if (neighborNode.Flags == 0)
{
GetEdgeMidPoint(bestRef, bestPoly, bestTile, neighborRef, neighborPoly, neighborTile, ref neighborNode.Position);
}
//calculate cost and heuristic
float cost = 0;
float heuristic = 0;
//special case for last node
if (neighborRef == endRef)
{
//cost
float curCost = filter.GetCost(bestNode.Position, neighborNode.Position,
parentRef, parentTile, parentPoly,
bestRef, bestTile, bestPoly,
neighborRef, neighborTile, neighborPoly);
float endCost = filter.GetCost(neighborNode.Position, endPos,
bestRef, bestTile, bestPoly,
neighborRef, neighborTile, neighborPoly,
NavPolyId.Null, null, null);
cost = bestNode.PolyCost + curCost + endCost;
heuristic = 0;
}
else
{
//cost
float curCost = filter.GetCost(bestNode.Position, neighborNode.Position,
parentRef, parentTile, parentPoly,
bestRef, bestTile, bestPoly,
neighborRef, neighborTile, neighborPoly);
cost = bestNode.PolyCost + curCost;
heuristic = (neighborNode.Position - endPos).Length() * HeuristicScale;
}
float total = cost + heuristic;
//the node is already in open list and new result is worse, skip
if (IsInOpenList(neighborNode) && total >= neighborNode.TotalCost)
continue;
//the node is already visited and processesd, and the new result is worse, skip
if (IsInClosedList(neighborNode) && total >= neighborNode.TotalCost)
continue;
//add or update the node
neighborNode.ParentIndex = nodePool.GetNodeIdx(bestNode);
neighborNode.Id = neighborRef;
neighborNode.Flags = RemoveNodeFlagClosed(neighborNode);
neighborNode.PolyCost = cost;
neighborNode.TotalCost = total;
if (IsInOpenList(neighborNode))
{
//already in open, update node location
openList.Modify(neighborNode);
}
else
{
//put the node in the open list
SetNodeFlagOpen(ref neighborNode);
openList.Push(neighborNode);
}
//update nearest node to target so far
if (heuristic < lastBestTotalCost)
{
lastBestTotalCost = heuristic;
lastBestNode = neighborNode;
}
}
}
//save path
NavNode node = lastBestNode;
do
{
path.Add(node.Id);
node = nodePool.GetNodeAtIdx(node.ParentIndex);
}
while (node != null);
//reverse the path since it's backwards
path.Reverse();
return true;
}
/// <summary>
/// Change the move requests for all the agents
/// </summary>
public void UpdateMoveRequest()
{
const int PATH_MAX_AGENTS = 8;
Agent[] queue = new Agent[PATH_MAX_AGENTS];
int numQueue = 0;
Status status;
//fire off new requests
for (int i = 0; i < maxAgents; i++)
{
if (!agents[i].IsActive)
continue;
if (agents[i].State == AgentState.Invalid)
continue;
if (agents[i].TargetState == TargetState.None || agents[i].TargetState == TargetState.Velocity)
continue;
if (agents[i].TargetState == TargetState.Requesting)
{
Path path = agents[i].Corridor.NavPath;
Vector3 reqPos = new Vector3();
Path reqPath = new Path();
//quick search towards the goal
const int MAX_ITER = 20;
NavPoint startPoint = new NavPoint(path[0], agents[i].Position);
NavPoint endPoint = new NavPoint(agents[i].TargetRef, agents[i].TargetPosition);
navQuery.InitSlicedFindPath(ref startPoint, ref endPoint, navQueryFilter, FindPathOptions.None);
int tempInt = 0;
navQuery.UpdateSlicedFindPath(MAX_ITER, ref tempInt);
status = Status.Failure;
if (agents[i].TargetReplan)
{
//try to use an existing steady path during replan if possible
status = navQuery.FinalizedSlicedPathPartial(path, reqPath).ToStatus();
}
else
{
//try to move towards the target when the goal changes
status = navQuery.FinalizeSlicedFindPath(reqPath).ToStatus();
}
if (status != Status.Failure && reqPath.Count > 0)
{
//in progress or succeed
if (reqPath[reqPath.Count - 1] != agents[i].TargetRef)
{
//partial path, constrain target position in last polygon
bool tempBool;
status = navQuery.ClosestPointOnPoly(reqPath[reqPath.Count - 1], agents[i].TargetPosition, out reqPos, out tempBool).ToStatus();
if (status == Status.Failure)
reqPath.Clear();
}
else
{
reqPos = agents[i].TargetPosition;
}
}
else
{
reqPath.Clear();
}
if (reqPath.Count == 0)
{
//could not find path, start the request from the current location
reqPos = agents[i].Position;
reqPath.Add(path[0]);
}
agents[i].Corridor.SetCorridor(reqPos, reqPath);
agents[i].Boundary.Reset();
agents[i].IsPartial = false;
if (reqPath[reqPath.Count - 1] == agents[i].TargetRef)
{
agents[i].TargetState = TargetState.Valid;
agents[i].TargetReplanTime = 0.0f;
}
else
{
//the path is longer or potentially unreachable, full plan
agents[i].TargetState = TargetState.WaitingForQueue;
}
}
if (agents[i].TargetState == TargetState.WaitingForQueue)
{
numQueue = AddToPathQueue(agents[i], queue, numQueue, PATH_MAX_AGENTS);
}
}
for (int i = 0; i < numQueue; i++)
{
queue[i].TargetPathQueryIndex = pathq.Request(new NavPoint(queue[i].Corridor.GetLastPoly(), queue[i].Corridor.Target), new NavPoint(queue[i].TargetRef, queue[i].TargetPosition));
if (queue[i].TargetPathQueryIndex != PathQueue.Invalid)
queue[i].TargetState = TargetState.WaitingForPath;
}
//update requests
pathq.Update(MaxItersPerUpdate);
//process path results
for (int i = 0; i < maxAgents; i++)
{
if (!agents[i].IsActive)
continue;
if (agents[i].TargetState == TargetState.None || agents[i].TargetState == TargetState.Velocity)
continue;
if (agents[i].TargetState == TargetState.WaitingForPath)
{
//poll path queue
status = pathq.GetRequestStatus(agents[i].TargetPathQueryIndex);
if (status == Status.Failure)
{
//path find failed, retry if the target location is still valid
agents[i].TargetPathQueryIndex = PathQueue.Invalid;
if (agents[i].TargetRef != NavPolyId.Null)
agents[i].TargetState = TargetState.Requesting;
else
agents[i].TargetState = TargetState.Failed;
agents[i].TargetReplanTime = 0.0f;
}
else if (status == Status.Success)
{
Path path = agents[i].Corridor.NavPath;
//apply results
Vector3 targetPos = new Vector3();
targetPos = agents[i].TargetPosition;
Path res;
bool valid = true;
status = pathq.GetPathResult(agents[i].TargetPathQueryIndex, out res).ToStatus();
if (status == Status.Failure || res.Count == 0)
valid = false;
//Merge result and existing path
if (valid && path[path.Count - 1] != res[0])
valid = false;
if (valid)
{
//put the old path infront of the old path
if (path.Count > 1)
{
//make space for the old path
//if ((path.Count - 1) + nres > maxPathResult)
//nres = maxPathResult - (npath - 1);
for (int j = 0; j < res.Count; j++)
res[path.Count - 1 + j] = res[j];
//copy old path in the beginning
for (int j = 0; j < path.Count - 1; j++)
res.Add(path[j]);
//remove trackbacks
res.RemoveTrackbacks();
}
//check for partial path
if (res[res.Count - 1] != agents[i].TargetRef)
{
//partial path, constrain target position inside the last polygon
Vector3 nearest;
bool tempBool = false;
status = navQuery.ClosestPointOnPoly(res[res.Count - 1], targetPos, out nearest, out tempBool).ToStatus();
if (status == Status.Success)
targetPos = nearest;
else
valid = false;
}
}
if (valid)
{
//set current corridor
agents[i].Corridor.SetCorridor(targetPos, res);
//forced to update boundary
agents[i].Boundary.Reset();
agents[i].TargetState = TargetState.Valid;
}
else
{
//something went wrong
agents[i].TargetState = TargetState.Failed;
}
agents[i].TargetReplanTime = 0.0f;
}
}
}
}
public void AppendPath(Path other)
{
polys.AddRange(other.polys);
}
// Copyright (c) 2013-2016 Robert Rouhani <*****@*****.**> and other contributors (see CONTRIBUTORS file).
// Licensed under the MIT License - https://raw.github.com/Robmaister/SharpNav/master/LICENSE
public static List <Vector3> GetPath(actors.area.Zone zone, Vector3 startVec, Vector3 endVec, float stepSize = 0.70f, int pathSize = 45, float polyRadius = 0.0f, bool skipToTarget = false)
{
var navMesh = zone.tiledNavMesh;
var navMeshQuery = zone.navMeshQuery;
// no navmesh loaded, run straight to player
if (navMesh == null)
{
return(new List <Vector3>()
{
endVec
});
}
// no need to waste cycles finding path to same point
if (startVec.X == endVec.X && startVec.Y == endVec.Y && startVec.Z == endVec.Z && polyRadius == 0.0f)
{
return(null);
}
var smoothPath = new List <Vector3>(pathSize)
{
};
NavQueryFilter filter = new NavQueryFilter();
NavPoint startPt, endPt;
try
{
SharpNav.Geometry.Vector3 c = new SharpNav.Geometry.Vector3(startVec.X, startVec.Y, startVec.Z);
SharpNav.Geometry.Vector3 ep = new SharpNav.Geometry.Vector3(endVec.X, endVec.Y, endVec.Z);
SharpNav.Geometry.Vector3 e = new SharpNav.Geometry.Vector3(5, 5, 5);
navMeshQuery.FindNearestPoly(ref c, ref e, out startPt);
navMeshQuery.FindNearestPoly(ref ep, ref e, out endPt);
//calculate the overall path, which contains an array of polygon references
int MAX_POLYS = 256;
var path = new SharpNav.Pathfinding.Path();
navMeshQuery.FindPath(ref startPt, ref endPt, filter, path);
//find a smooth path over the mesh surface
int npolys = path.Count;
SharpNav.Geometry.Vector3 iterPos = new SharpNav.Geometry.Vector3();
SharpNav.Geometry.Vector3 targetPos = new SharpNav.Geometry.Vector3();
navMeshQuery.ClosestPointOnPoly(startPt.Polygon, startPt.Position, ref iterPos);
navMeshQuery.ClosestPointOnPoly(path[npolys - 1], endPt.Position, ref targetPos);
// set target to random point at end of path
if (polyRadius != 0.0f)
{
var randPoly = navMeshQuery.FindRandomPointAroundCircle(endPt, polyRadius);
targetPos = randPoly.Position;
}
if (skipToTarget)
{
return(new List <Vector3>()
{
new Vector3(targetPos.X, targetPos.Y, targetPos.Z)
});
}
smoothPath.Add(new Vector3(iterPos.X, iterPos.Y, iterPos.Z));
//float STEP_SIZE = 0.70f;
float SLOP = 0.15f;
while (npolys > 0 && smoothPath.Count < smoothPath.Capacity)
{
//find location to steer towards
SharpNav.Geometry.Vector3 steerPos = new SharpNav.Geometry.Vector3();
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
SharpNav.Geometry.Vector3 delta = steerPos - iterPos;
float len = (float)Math.Sqrt(SharpNav.Geometry.Vector3.Dot(delta, delta));
//if steer target is at end of path or off-mesh link
//don't move past location
if ((endOfPath || offMeshConnection) && len < stepSize)
{
len = 1;
}
else
{
len = stepSize / len;
}
SharpNav.Geometry.Vector3 moveTgt = new SharpNav.Geometry.Vector3();
VMad(ref moveTgt, iterPos, delta, len);
//move
SharpNav.Geometry.Vector3 result = new SharpNav.Geometry.Vector3();
List <NavPolyId> visited = new List <NavPolyId>(pathSize);
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, 1000.0f))
{
//reached end of path
iterPos = targetPos;
if (smoothPath.Count < smoothPath.Capacity)
{
smoothPath.Add(new Vector3(iterPos.X, iterPos.Y, iterPos.Z));
}
break;
}
//store results
if (smoothPath.Count < smoothPath.Capacity)
{
smoothPath.Add(new Vector3(iterPos.X, iterPos.Y, iterPos.Z));
}
}
}
catch (Exception e)
{
Program.Log.Error(e.Message);
Program.Log.Error("Start pos {0} {1} {2} end pos {3} {4} {5}", startVec.X, startVec.Y, startVec.Z, endVec.X, endVec.Y, endVec.Z);
// todo: probably log this
return(new List <Vector3>()
{
endVec
});
}
return(smoothPath);
}
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);
}
}
}
/// <summary>
/// Merge two paths after the start is changed
/// </summary>
/// <param name="path">The current path</param>
/// <param name="npath">Current path length</param>
/// <param name="maxPath">Maximum path length allowed</param>
/// <param name="visited">The visited polygons</param>
/// <param name="nvisited">Visited path length</param>
/// <returns>New path length</returns>
public int MergeCorridorStartMoved(Path path, List<NavPolyId> visited)
{
int furthestPath = -1;
int furthestVisited = -1;
//find furthest common polygon
for (int i = path.Count - 1; i >= 0; i--)
{
bool found = false;
for (int j = visited.Count - 1; j >= 0; j--)
{
if (path[i] == visited[j])
{
furthestPath = i;
furthestVisited = j;
found = true;
}
}
if (found)
break;
}
//if no intersection found just return current path
if (furthestPath == -1 || furthestVisited == -1)
return path.Count;
//concatenate paths
//adjust beginning of buffer to include the visited
int req = visited.Count - furthestVisited;
int orig = Math.Min(furthestPath + 1, path.Count);
int size = Math.Max(0, path.Count - orig);
if (req + size > path.Count)
size = path.Count - req;
if (size > 0)
{
for (int i = 0; i < size; i++)
path[req + i] = path[orig + i];
}
//store visited
for (int i = 0; i < req; i++)
path[i] = visited[(visited.Count - 1) - i];
return req + size;
}
