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);
navMeshQuery.FindCenterPath(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, 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 void GeneratePathfinding()
{
if (!hasGenerated)
{
return;
}
NavQueryFilter filter = new NavQueryFilter();
buildData = new NavMeshBuilder(polyMesh, polyMeshDetail, new SharpNav.Pathfinding.OffMeshConnection[0], settings);
tiledNavMesh = new TiledNavMesh(buildData);
for (int i = 0; i < tiledNavMesh.Tiles.Count; ++i)
{
for (int j = 0; j < tiledNavMesh.Tiles[i].Verts.Length; ++j)
{
//if (j < tiledNavMesh.Tiles[i].Verts.Length - 1)
// Debug.DrawLine(ExportNavMeshToObj.ToUnityVector(tiledNavMesh.Tiles[i].Verts[j]), ExportNavMeshToObj.ToUnityVector(tiledNavMesh.Tiles[i].Verts[j + 1]), Color.blue, 99);
}
}
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);
startPt = navMeshQuery.FindRandomPoint();
endPt = navMeshQuery.FindRandomPoint();
//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);
}
}
for (int i = 0; i < smoothPath.Count; i++)
{
//if (i < smoothPath.Count - 1)
// Debug.DrawLine(ExportNavMeshToObj.ToUnityVector(smoothPath[i]), ExportNavMeshToObj.ToUnityVector(smoothPath[i + 1]), Color.red, 99);
}
}
public Vector2[] GetPath(Vector2 start, Vector2 end)
{
NavPoint startPt = navMeshQuery.FindNearestPoly(new SVector3(-start.X, 0, start.Y), new SharpNav.Geometry.Vector3(2f, 2f, 2f));
NavPoint endPt = navMeshQuery.FindNearestPoly(new SVector3(-end.X, 0, end.Y), new SharpNav.Geometry.Vector3(2f, 2f, 2f));
Path path = new Path();
navMeshQuery.FindPath(ref startPt, ref endPt, new NavQueryFilter(), path);
List <SVector3> smoothPath;
//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);
}
}
return(smoothPath.Select(x => new Vector2(-x.X, x.Z)).ToArray());
}
// 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);
}
public StraightPathVertex(NavPoint point, StraightPathFlags flags)
{
Point = point;
Flags = flags;
}
//为了使接口适应而附加的
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);
}
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;
}
public StraightPathVertex(NavPoint point, StraightPathFlags flags)
{
Point = point;
Flags = flags;
}
