protected override void OnCreate()
{
var settings = pathSystem.Settings;
var pointerArray = new PathMeshQueryPointer[JobsUtility.MaxJobThreadCount];
for (var i = 0; i < JobsUtility.MaxJobThreadCount; ++i)
{
pointerArray[i] = new PathMeshQueryPointer
{
Value = UnsafeUtility.Malloc(
UnsafeUtility.SizeOf <NavMeshQuery>(),
UnsafeUtility.AlignOf <NavMeshQuery>(),
Allocator.Persistent
)
};
var query = new NavMeshQuery(
NavMeshWorld.GetDefaultWorld(),
Allocator.Persistent,
settings.PathMeshQueryNodeMax
);
queryList.Add(query);
UnsafeUtility.CopyStructureToPtr(ref query, pointerArray[i].Value);
}
PointerArray = new NativeArray <PathMeshQueryPointer>(pointerArray, Allocator.Persistent);
}
/// <summary>
/// Move along the NavMeshQuery and update the position
/// </summary>
/// <param name="npos">Current position</param>
/// <param name="navquery">The NavMeshQuery</param>
/// <returns>True if position changed, false if not</returns>
public bool MovePosition(Vector3 npos, NavMeshQuery navquery)
{
const int MaxVisited = 16;
Vector3 result = new Vector3();
List <NavPolyId> visited = new List <NavPolyId>(MaxVisited);
NavPoint startPoint = new NavPoint(path[0], pos);
//move along navmesh and update new position
bool status = navquery.MoveAlongSurface(ref startPoint, ref npos, out result, visited);
if (status == true)
{
MergeCorridorStartMoved(path, visited);
//adjust the position to stay on top of the navmesh
float h = pos.Y;
navquery.GetPolyHeight(path[0], result, ref h);
result.Y = h;
pos = result;
return(true);
}
return(false);
}
public void FindCorners(StraightPath corners, NavMeshQuery navquery)
{
const float MinTargetDist = 0.01f;
navquery.FindStraightPath(pos, target, path, corners, 0);
//prune points in the beginning of the path which are too close
while (corners.Count > 0)
{
if (((corners[0].Flags & StraightPathFlags.OffMeshConnection) != 0) ||
Vector3Extensions.Distance2D(corners[0].Point.Position, pos) > MinTargetDist)
break;
corners.RemoveAt(0);
}
//prune points after an off-mesh connection
for (int i = 0; i < corners.Count; i++)
{
if ((corners[i].Flags & StraightPathFlags.OffMeshConnection) != 0)
{
corners.RemoveRange(i + 1, corners.Count - i);
break;
}
}
}
/// <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);
}
public void FindCorners(StraightPath corners, NavMeshQuery navquery)
{
const float MinTargetDist = 0.01f;
navquery.FindStraightPath(pos, target, path, corners, 0);
//prune points in the beginning of the path which are too close
while (corners.Count > 0)
{
if (((corners[0].Flags & StraightPathFlags.OffMeshConnection) != 0) ||
Vector3Extensions.Distance2D(corners[0].Point.Position, pos) > MinTargetDist)
{
break;
}
corners.RemoveAt(0);
}
//prune points after an off-mesh connection
for (int i = 0; i < corners.Count; i++)
{
if ((corners[i].Flags & StraightPathFlags.OffMeshConnection) != 0)
{
corners.RemoveRange(i + 1, corners.Count - i);
break;
}
}
}
public static NavMeshQuery CreateMeshQuery(NavMeshCreateParams meshCreateParams)
{
Detour.Detour.dtRawTileData navData;
if (!Detour.Detour.dtCreateNavMeshData(meshCreateParams, out navData))
{
return(null);
}
var m_navMesh = new NavMesh();
dtStatus status;
status = m_navMesh.init(navData, (int)Detour.Detour.dtTileFlags.DT_TILE_FREE_DATA);
if (Detour.Detour.dtStatusFailed(status))
{
return(null);
}
var m_navQuery = new NavMeshQuery();
status = m_navQuery.init(m_navMesh, 2048);
if (Detour.Detour.dtStatusFailed(status))
{
return(null);
}
return(m_navQuery);
}
// Retrace portals between corners and register if type of polygon changes:
public static int RetracePortals(NavMeshQuery query, int startIndex, int endIndex, NativeSlice <PolygonId> path, int n, float3 termPos, ref NativeArray <NavMeshLocation> straightPath, ref NativeArray <StraightPathFlags> straightPathFlags, int maxStraightPath)
{
for (var i = startIndex; i < endIndex - 1; ++i)
{
var type1 = query.GetPolygonType(path[i]);
var type2 = query.GetPolygonType(path[i + 1]);
if (type1 == type2)
{
continue;
}
var status = query.GetPortalPoints(path[i], path[i + 1], out var l, out var r);
GeometryUtils.SegmentSegmentCPA(out var cpa1, out var cpa2, l, r, straightPath[n - 1].position, termPos);
straightPath[n] = query.CreateLocation(cpa1, path[i + 1]);
straightPathFlags[n] = (type2 == NavMeshPolyTypes.OffMeshConnection) ? StraightPathFlags.OffMeshConnection : 0;
if (++n == maxStraightPath)
{
return(maxStraightPath);
}
}
straightPath[n] = query.CreateLocation(termPos, path[endIndex]);
straightPathFlags[n] = query.GetPolygonType(path[endIndex]) == NavMeshPolyTypes.OffMeshConnection ? StraightPathFlags.OffMeshConnection : 0;
return(++n);
}
public PathQueryQueueEcs(int nodePoolSize, int maxRequestCount)
{
var world = NavMeshWorld.GetDefaultWorld();
m_Query = new NavMeshQuery(world, Allocator.Persistent, nodePoolSize);
m_Requests = new NativeArray <RequestEcs>(maxRequestCount, Allocator.Persistent);
m_ResultNodes = new NativeArray <PolygonId>(2 * nodePoolSize, Allocator.Persistent);
m_ResultRanges = new NativeArray <PathInfo>(maxRequestCount + 1, Allocator.Persistent);
m_AgentIndices = new NativeArray <int>(maxRequestCount + 1, Allocator.Persistent);
m_Costs = new NativeArray <float>(32, Allocator.Persistent);
for (var i = 0; i < m_Costs.Length; ++i)
{
m_Costs[i] = 1.0f;
}
m_State = new NativeArray <QueryQueueState>(1, Allocator.Persistent);
m_State[0] = new QueryQueueState()
{
requestCount = 0,
requestIndex = 0,
resultNodesCount = 0,
resultPathsCount = 0,
currentAgentIndex = -1,
currentPathRequest = new PathInfo()
};
}
static bool GetSteerTarget( NavMeshQuery navMeshQuery, SharpNav.Geometry.Vector3 startPos, SharpNav.Geometry.Vector3 endPos, float minTargetDist, SharpNav.Pathfinding.Path path, ref SharpNav.Geometry.Vector3 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;
}
protected override void OnCreateManager(int capacity)
{
var world = NavMeshWorld.GetDefaultWorld();
query = new NavMeshQuery(world, Allocator.Persistent);
agentType = NavMesh.GetSettingsByIndex(1).agentTypeID;
}
private void LoadNavMeshFromFile(string path)
{
tiledNavMesh = new NavMeshJsonSerializer().Deserialize(path);
navMeshQuery = new NavMeshQuery(tiledNavMesh, 2048);
hasGenerated = true;
displayMode = DisplayMode.NavMesh;
}
public bool TrimInvalidPath(long safeRef, float[] safePos, NavMeshQuery navQuery, QueryFilter filter)
{
int n = 0;
while (n < _npath && navQuery.IsValidPolyRef(_path[n], filter))
{
n++;
}
if (n == _npath)
{
return(true);
}
else if (n == 0)
{
Helper.VCopy(ref _pos, safePos);
_path[0] = safeRef;
_npath = 1;
}
else
{
_npath = n;
}
float[] tgt = new float[3];
Helper.VCopy(ref tgt, _target);
navQuery.ClosestPointOnPolyBoundary(_path[_npath - 1], tgt, ref _target);
return(true);
}
/// <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)
{
if (pathCount < 3)
{
return(false);
}
const int MaxIter = 32;
const int MaxRes = 32;
PolyId[] res = new PolyId[MaxRes];
int numRes = 0;
int tempInt = 0;
navquery.InitSlicedFindPath(new NavPoint(path[0], pos), new NavPoint(path[pathCount - 1], target));
navquery.UpdateSlicedFindPath(MaxIter, ref tempInt);
bool status = navquery.FinalizedSlicedPathPartial(path, pathCount, res, ref numRes, MaxRes);
if (status == true && numRes > 0)
{
pathCount = MergeCorridorStartShortcut(path, pathCount, maxPath, res, numRes);
return(true);
}
return(false);
}
protected override void OnCreate()
{
var pointerArray = new NavMeshQueryPointer[JobsUtility.MaxJobThreadCount];
for (int i = 0; i < JobsUtility.MaxJobThreadCount; ++i)
{
pointerArray[i] = new NavMeshQueryPointer
{
Value = UnsafeUtility.Malloc(
UnsafeUtility.SizeOf <NavMeshQuery>(),
UnsafeUtility.AlignOf <NavMeshQuery>(),
Allocator.Persistent
)
};
var query = new NavMeshQuery(
NavMeshWorld.GetDefaultWorld(),
Allocator.Persistent,
NavConstants.PATH_NODE_MAX
);
queryList.Add(query);
UnsafeUtility.CopyStructureToPtr(ref query, pointerArray[i].Value);
}
PointerArray = new NativeArray <NavMeshQueryPointer>(pointerArray, Allocator.Persistent);
}
public bool LoadNavMesh(string mapName)
{
bool retCode = false;
string path = ConfigFileDir + mapName + ".bytes";
NavMeshInfo navInfo = MapMgr.Inst().GetNavMeshInfo(mapName);
if (navInfo == null)
{
retCode = LoadNavMeshByJsonFile(path, ref m_MapNavMesh);
if (MapNavMesh == null || retCode == false)
{
Console.WriteLine("LoadNavMeshByJsonFile {0} failed", path);
return(false);
}
m_MapNavMeshQuery = new NavMeshQuery();
m_MapNavMeshQuery.Init(MapNavMesh, (int)MapDefine.MAX_NAV_POLYS);
NavMeshInfo newNavInfo = new NavMeshInfo();
newNavInfo.pdtNavMesh = MapNavMesh;
newNavInfo.pdtNavMeshQuery = MapNavMeshQuery;
MapMgr.Inst().AddNavMeshInfo(mapName, newNavInfo);
}
else
{
m_MapNavMesh = navInfo.pdtNavMesh;
m_MapNavMeshQuery = navInfo.pdtNavMeshQuery;
}
return(true);
}
public void TryFindPath(Vector3 start, Vector3 end)
{
using (var polygonIds = new NativeArray <PolygonId>(100, Allocator.Persistent))
using (var query = new NavMeshQuery(NavMeshWorld.GetDefaultWorld(), Allocator.Persistent, 100))
{
int maxIterations = 1024;
var from = query.MapLocation(start, Vector3.one * 10, 0);
var to = query.MapLocation(end, Vector3.one * 10, 0);
var status = query.BeginFindPath(from, to);
status = query.UpdateFindPath(maxIterations, out int currentIterations);
var finalStatus = query.EndFindPath(out int pathLength);
var pathResult = query.GetPathResult(polygonIds);
var straightPath = new NativeArray <NavMeshLocation>(pathLength, Allocator.Temp);
paths = new Vector3[pathResult];
for (int i = 0; i < pathResult; i++)
{
var polyId = polygonIds[i];
var polyWorldToLocal = query.PolygonWorldToLocalMatrix(polyId);
var b = query.CreateLocation(paths[i], polyId);
paths[i] = b.position;
//Debug.Log(b.position);
}
//Debug.Log(pathResult);
Debug.DrawLine(from.position, to.position);
}
}
public virtual void trimInvalidPath(long safeRef, float[] safePos, NavMeshQuery navquery, QueryFilter filter)
{
// Keep valid path as far as possible.
int n = 0;
while (n < m_path.Count && navquery.isValidPolyRef(m_path[n], filter))
{
n++;
}
if (n == 0)
{
// The first polyref is bad, use current safe values.
DetourCommon.vCopy(m_pos, safePos);
m_path.Clear();
m_path.Add(safeRef);
}
else if (n < m_path.Count)
{
m_path = m_path.GetRange(0, n);
// The path is partially usable.
}
navquery.closestPointOnPolyBoundary(m_path[m_path.Count - 1], m_target, temp);
// Clamp target pos to last poly
DetourCommon.vCopy(m_target, temp);
}
IEnumerator StartQuery()
{
NavMeshWorld world = NavMeshWorld.GetDefaultWorld();
NavMeshQuery query = new NavMeshQuery(world, Allocator.Persistent, maxPath);
NavMeshLocation startLocation = query.MapLocation(start.position, Vector3.up * extents, 0);
NavMeshLocation endLocation = query.MapLocation(end.position, Vector3.up * extents, 0);
PathQueryStatus status = query.BeginFindPath(startLocation, endLocation);
yield return(new WaitWhile(() => {
status = query.UpdateFindPath(8, out int iterationsPerformed);
return status == PathQueryStatus.InProgress;
}));
status = query.EndFindPath(out int pathsize);
NativeArray <PolygonId> path = new NativeArray <PolygonId>(pathsize, Allocator.Temp);
int pathResult = query.GetPathResult(path);
//NativeArray<NavMeshLocation> pathStraight = new NativeArray<NavMeshLocation>(maxPath, Allocator.Temp);
////NativeArray<StraightPathFlag> pathStreaigthFlag = new NativeArray<StraightPathFlags>(maxPath, Allocator.Temp);
//NativeArray<float> vertexSize = new NativeArray<float>(maxPath, Allocator.Temp);
//
//int straghtPathCount = 0;
for (var i = 0; i < pathResult; i++)
{
var p = path[i];
var loc = query.CreateLocation(start.position, p);
var target = loc.position;
}
query.Dispose();
}
// This function checks if the path has a small U-turn, that is,
// a polygon further in the path is adjacent to the first polygon
// in the path. If that happens, a shortcut is taken.
// This can happen if the target (T) location is at tile boundary,
// and we're (S) approaching it parallel to the tile edge.
// The choice at the vertex can be arbitrary,
// +---+---+
// |:::|:::|
// +-S-+-T-+
// |:::| | <-- the step can end up in here, resulting U-turn path.
// +---+---+
static int fixupShortcuts(uint[] path, int npath, NavMeshQuery navQuery)
{
if (npath < 3)
{
return(npath);
}
// Get connected polygons
const int maxNeis = 16;
uint[] neis = new uint[maxNeis];
int nneis = 0;
Detour.dtMeshTile tile = null;
Detour.dtPoly poly = null;
if (Detour.dtStatusFailed(navQuery.getAttachedNavMesh().getTileAndPolyByRef(path[0], ref tile, ref poly)))
{
return(npath);
}
for (uint k = poly.firstLink; k != Detour.DT_NULL_LINK; k = tile.links[k].next)
{
Detour.dtLink link = tile.links[k];
if (link.polyRef != 0)
{
if (nneis < maxNeis)
{
neis[nneis++] = link.polyRef;
}
}
}
// If any of the neighbour polygons is within the next few polygons
// in the path, short cut to that polygon directly.
const int maxLookAhead = 6;
int cut = 0;
for (int i = Math.Min(maxLookAhead, npath) - 1; i > 1 && cut == 0; i--)
{
for (int j = 0; j < nneis; j++)
{
if (path[i] == neis[j])
{
cut = i;
break;
}
}
}
if (cut > 1)
{
int offset = cut - 1;
npath -= offset;
for (int i = 1; i < npath; i++)
{
path[i] = path[i + offset];
}
}
return(npath);
}
// Use this for initialization
void Start()
{
var navQuery = FindObjectOfType(typeof(RecastNavMeshQuery)) as RecastNavMeshQuery;
if (navQuery != null)
{
_navMeshQuery = navQuery._navMeshQuery;
filter = navQuery.filter;
crowd = navQuery.Crowd;
param = new CrowdAgentParams
{
Radius = Radius,
Height = Height,
MaxAcceleration = MaxAcceleration,
MaxSpeed = MaxSpeed,
CollisionQueryRange = CollisionQueryRange,
PathOptimizationRange = PathOptimizationRange,
UpdateFlags = UpdateFlags,
ObstacleAvoidanceType = ObstacleAvoidanceType,
SeparationWeight = SeparationWeight
};
AgentId = navQuery.Crowd.AddAgent(new[] { transform.position.x, transform.position.y, transform.position.z }, param);
ResetTarget();
}
else
{
Debug.LogError("Scene does not have a Nav Mesh Query, one must be added.");
}
}
public bool OptimizePathTopology(NavMeshQuery navQuery, QueryFilter filter)
{
if (_npath < 3)
{
return(false);
}
int MaxIters = 32;
int MaxRes = 32;
long[] res = new long[MaxRes];
int nres = 0;
navQuery.InitSlicedFindPath(_path[0], _path[_npath - 1], _pos, _target, filter);
int doneIters = 0;
navQuery.UpdateSlicedFindPath(MaxIters, ref doneIters);
Status status = navQuery.FinalizeSlicedFindPathPartial(_path, _npath, ref res, ref nres, MaxRes);
if ((status & Status.Success) != 0 && nres > 0)
{
_npath = MergeCorridorStartShortcut(ref _path, _npath, _maxPath, res, nres);
return(true);
}
return(false);
}
/// <summary>
/// Move along the NavMeshQuery and update the position
/// </summary>
/// <param name="npos">Current position</param>
/// <param name="navquery">The NavMeshQuery</param>
/// <returns>True if position changed, false if not</returns>
public bool MovePosition(Vector3 npos, NavMeshQuery navquery)
{
//move along navmesh and update new position
Vector3 result = new Vector3();
const int MaxVisited = 16;
PolyId[] visited = new PolyId[MaxVisited];
List <PolyId> listVisited = new List <PolyId>(MaxVisited);
bool status = navquery.MoveAlongSurface(new NavPoint(path[0], pos), npos, ref result, listVisited);
visited = listVisited.ToArray(); //HACK why?
if (status == true)
{
pathCount = MergeCorridorStartMoved(path, pathCount, maxPath, visited, listVisited.Count);
//adjust the position to stay on top of the navmesh
float h = pos.Y;
navquery.GetPolyHeight(path[0], result, ref h);
result.Y = h;
pos = result;
return(true);
}
return(false);
}
public bool Init(int maxAgents, float maxAgentRadius, NavMesh nav)
{
Purge();
_maxAgents = maxAgents;
_maxAgentRadius = maxAgentRadius;
Helper.VSet(ref _ext, _maxAgentRadius * 2.0f, _maxAgentRadius * 1.5f, _maxAgentRadius * 2.0f);
_grid = new ProximityGrid();
_grid.Init(_maxAgents * 4, maxAgentRadius * 3);
_obstacleQuery = new ObstacleAvoidanceQuery();
_obstacleQuery.Init(6, 8);
for (int i = 0; i < _obstacleQueryParams.Length; i++)
{
_obstacleQueryParams[i] = new ObstacleAvoidanceParams
{
velBias = 0.4f,
weightDesVel = 2.0f,
weightCurVel = 0.75f,
weightSide = 0.75f,
weightToi = 2.5f,
horizTime = 2.5f,
gridSize = 33,
adaptiveDivs = 7,
adaptiveRings = 2,
adaptiveDepth = 5
};
}
_maxPathResult = 256;
_pathResult = new long[_maxPathResult];
_pathq = new PathQueue();
_pathq.Init(_maxPathResult, MaxPathQueueNodes, nav);
_agents = new CrowdAgent[_maxAgents];
_activeAgents = new CrowdAgent[_maxAgents];
_agentAnims = new CrowdAgentAnimation[_maxAgents];
for (int i = 0; i < _maxAgents; i++)
{
_agents[i] = new CrowdAgent();
_agents[i].Active = false;
_agents[i].Corridor.Init(_maxPathResult);
}
for (int i = 0; i < _maxAgents; i++)
{
_agentAnims[i] = new CrowdAgentAnimation();
_agentAnims[i].Active = false;
}
_navQuery = new NavMeshQuery();
_navQuery.Init(nav, MaxCommonNodes);
return(true);
}
private void Purge()
{
_navQuery = null;
for (int i = 0; i < MaxQueue; i++)
{
_queue[i].path = null;
}
}
protected override void OnCreate()
{
AgentsGroup = GetEntityQuery(typeof(NavAgent), ComponentType.ReadWrite <Translation>(),
ComponentType.ReadWrite <Rotation>(), ComponentType.ReadWrite <LocalToWorld>());
var navMeshWorld = NavMeshWorld.GetDefaultWorld();
NavMeshQuery = new NavMeshQuery(navMeshWorld, Allocator.Persistent, 100);
}
public void Awake()
{
Instance = this; // worst singleton ever but it works
entityManager = World.Active.GetExistingManager <EntityManager>();
var navMeshWorld = NavMeshWorld.GetDefaultWorld();
mapLocationQuery = new NavMeshQuery(navMeshWorld, Allocator.Persistent);
}
public FindNearestPolyQuery(NavMeshQuery query, Vector3 center)
{
m_query = query;
m_center = center;
m_nearestDistanceSqr = float.MaxValue;
m_nearestRef = 0;
m_nearestPoint = Vector3.Zero;
}
private bool GetSteerTarget(NavMeshQuery navMeshQuery, float[] startPos, float[] endPos, float minTargetDistance,
long[] path, int pathSize, ref float[] steerPos, ref short steerPosFlag, ref long steerPosRef)
{
float[] outPoints = null;
int outPointsCount = 0;
return(GetSteerTarget(navMeshQuery, startPos, endPos, minTargetDistance, path, pathSize, ref steerPos, ref steerPosFlag, ref steerPosRef, ref outPoints, ref outPointsCount));
}
public Pather(string continent, ConnectionHandlerDelegate connectionHandler)
{
ConnectionHandler = connectionHandler;
Continent = continent.Substring(continent.LastIndexOf('\\') + 1);
if (Directory.Exists(continent))
{
_meshPath = continent;
}
else
{
var assembly = Assembly.GetCallingAssembly().Location;
var dir = Path.GetDirectoryName(assembly);
if (Directory.Exists(dir + "\\Meshes"))
{
_meshPath = dir + "\\Meshes\\" + continent;
}
else
{
_meshPath = dir + "\\" + continent;
}
}
if (!Directory.Exists(_meshPath))
{
throw new NavMeshException(DetourStatus.Failure, "No mesh for " + continent + " (Path: " + _meshPath + ")");
}
_mesh = new NavMesh();
DetourStatus status;
// check if this is a dungeon and initialize our mesh accordingly
string dungeonPath = GetDungeonPath();
if (File.Exists(dungeonPath))
{
var data = File.ReadAllBytes(dungeonPath);
status = _mesh.Initialize(data);
AddMemoryPressure(data.Length);
IsDungeon = true;
}
else
{
status = _mesh.Initialize(32768, 4096, Utility.Origin, Utility.TileSize, Utility.TileSize);
}
if (status.HasFailed())
{
throw new NavMeshException(status, "Failed to initialize the mesh");
}
_query = new NavMeshQuery(new PatherCallback(this));
_query.Initialize(_mesh, 65536);
Filter = new QueryFilter {
IncludeFlags = 0xFFFF, ExcludeFlags = 0x0
};
}
static bool getSteerTarget(NavMeshQuery navQuery, float[] startPos, float[] endPos,
float minTargetDist,
uint[] path, int pathSize,
float[] steerPos, ref byte steerPosFlag, ref uint steerPosRef,
ref float[] outPoints, ref int outPointCount)
{
// Find steer target.
const int MAX_STEER_POINTS = 3;
float[] steerPath = new float[MAX_STEER_POINTS * 3];
byte[] steerPathFlags = new byte[MAX_STEER_POINTS];
uint[] steerPathPolys = new uint[MAX_STEER_POINTS];
int nsteerPath = 0;
navQuery.findStraightPath(startPos, endPos, path, pathSize,
steerPath, steerPathFlags, steerPathPolys, ref nsteerPath, MAX_STEER_POINTS, 0);
if (nsteerPath == 0)
{
return(false);
}
//if (outPoints && outPointCount)
//{
outPointCount = nsteerPath;
for (int i = 0; i < nsteerPath; ++i)
{
Detour.dtVcopy(outPoints, i * 3, steerPath, i * 3);
}
//}
// Find vertex far enough to steer to.
int ns = 0;
while (ns < nsteerPath)
{
// Stop at Off-Mesh link or when point is further than slop away.
if ((steerPathFlags[ns] & (byte)Detour.dtStraightPathFlags.DT_STRAIGHTPATH_OFFMESH_CONNECTION) != 0 ||
!inRange(steerPath, ns * 3, startPos, 0, minTargetDist, 1000.0f))
{
break;
}
ns++;
}
// Failed to find good point to steer to.
if (ns >= nsteerPath)
{
return(false);
}
Detour.dtVcopy(steerPos, 0, steerPath, ns * 3);
steerPos[1] = startPos[1];
steerPosFlag = steerPathFlags[ns];
steerPosRef = steerPathPolys[ns];
return(true);
}
static void TestNavmesh(NavMeshBuilder build)
{
// Azeroth 28 28 / Deathknell (wow-style coordinates)
// Outside church: 1843.734 1604.214 94.55994
// Inside church: 1844.074 1642.581 97.62832
// Outside spawn: 1672.226 1662.989 139.2343
// Inside spawn: 1665.264 1678.277 120.5302
// Outside cave: 2051.3 1807.121 102.5225
// Inside cave: 2082.813 1950.718 98.04765
// Outside house: 1861.465 1582.03 92.79533
// Upstairs house: 1859.929 1560.804 99.07755
var tmesh = new TiledNavMesh(build);
var query = new NavMeshQuery(tmesh, 65536);
var extents = new Vector3(5f, 5f, 5f);
var posStart = WoWToSharpNav(new Vector3(1665.2f, 1678.2f, 120.5f)); // Inside spawn
var posEnd = WoWToSharpNav(new Vector3(1672.2f, 1662.9f, 139.2f)); // Outside spawn
NavPoint endPt;
query.FindNearestPoly(ref posEnd, ref extents, out endPt);
NavPoint startPt;
query.FindNearestPoly(ref posStart, ref extents, out startPt);
var path = new List <int>();
if (!query.FindPath(ref startPt, ref endPt, path))
{
Console.WriteLine("No path!");
}
Vector3 actualStart = new Vector3();
query.ClosestPointOnPoly(startPt.Polygon, startPt.Position, ref actualStart);
Vector3 actualEnd = new Vector3();
query.ClosestPointOnPoly(endPt.Polygon, endPt.Position, ref actualEnd);
var smoothPath = new List <Vector3>();
Vector3[] straightPath = new Vector3[2048];
int[] pathFlags = new int[2048];
int[] pathRefs = new int[2048];
int pathCount = -1;
query.FindStraightPath(actualStart, actualEnd, path.ToArray(), path.Count, straightPath, pathFlags, pathRefs, ref pathCount, 2048, PathBuildFlags.AllCrossingVertices);
foreach (var v in straightPath)
{
Console.WriteLine(v);
}
}
public PathQueue(int maxSearchNodeCount, ref TiledNavMesh nav)
{
this.navquery = new NavMeshQuery(nav, maxSearchNodeCount);
this.navqueryfilter = new NavQueryFilter();
this.queue = new PathQuery[MaxQueue];
for (int i = 0; i < MaxQueue; i++)
{
queue[i].Index = 0;
queue[i].Path = new Path();
}
this.queueHead = 0;
}
public PathQueue(int maxPathSize, int maxSearchNodeCount, ref TiledNavMesh nav)
{
this.navquery = new NavMeshQuery(nav, maxSearchNodeCount);
this.maxPathSize = maxPathSize;
this.queue = new PathQuery[MaxQueue];
for (int i = 0; i < MaxQueue; i++)
{
queue[i].Reference = Invalid;
queue[i].Path = new int[maxPathSize];
}
this.queueHead = 0;
}
public WowMapPather(string continent)
{
fullFileName = continent;
Continent = continent.Substring(continent.LastIndexOf('\\') + 1);
if (Directory.Exists(continent))
_meshPath = continent;
else
{
var assembly = Assembly.GetCallingAssembly().Location;
var dir = Path.GetDirectoryName(assembly);
if (Directory.Exists(dir + "\\Meshes"))
_meshPath = dir + "\\Meshes\\" + continent;
else
_meshPath = dir + "\\" + continent;
}
if (!Directory.Exists(_meshPath))
throw new Exception("No mesh for " + continent + " (Path: " + _meshPath + ")");
_mesh = new NavMesh();
DetourStatus status;
// check if this is a dungeon and initialize our mesh accordingly
string dungeonPath = GetDungeonPath();
if (File.Exists(dungeonPath))
{
var data = File.ReadAllBytes(dungeonPath);
status = _mesh.Initialize(data);
IsDungeon = true;
}
else
status = _mesh.Initialize(32768, 4096, Utility.Origin, Utility.TileSize, Utility.TileSize);
if (status.HasFailed())
throw new Exception("Failed to initialize the mesh");
// _query = new NavMeshQuery(new PatherCallback(this));
//TODO: Add Callback for Dynamic Tile Loading
_query = new NavMeshQuery();
_query.Initialize(_mesh, 65536);
Filter = new QueryFilter { IncludeFlags = 0xFFFF, ExcludeFlags = 0x0 };
}
public FloodFill(NavMesh mesh)
{
Mesh = mesh;
_query = new NavMeshQuery();
if (!_query.Initialize(Mesh, 1024).HasSucceeded())
throw new Exception("Failed to initialize navigation mesh query");
_filter = new QueryFilter();
var param = Mesh.Parameter;
// detour guarantees these are 2^x
var tileBits = (int) Math.Log(param.MaxTiles, 2);
var polyBits = (int) Math.Log(param.MaxPolygons, 2);
// we also don't care about salt, so create a mask to cut these off just in case
Visited = new bool[1 << (tileBits + polyBits)];
VisitedMask = (1 << (tileBits + polyBits)) - 1;
_open = new Queue<uint>(100);
}
public void TestNavMesh(byte[] data)
{
var extents = new Vector3(2.5f, 2.5f, 2.5f).ToFloatArray();
// var startVec = new Vector3(-9467.8f, 64.2f, 55.9f);
//var endVec = new Vector3(-9248.9f, -93.35f, 70.3f);
//Vector3 startVec = new Vector3(1672.2f, 1662.9f, 139.2f);
//Vector3 startVec = new Vector3(1665.2f, 1678.2f, 120.5f);
Vector3 startVec = new Vector3 ( -8949.95f, -132.493f, 83.5312f );
Vector3 endVec = new Vector3 ( -9046.507f, -45.71962f, 88.33186f );
var start = startVec.ToRecast().ToFloatArray();
var end = endVec.ToRecast().ToFloatArray();
NavMesh _mesh = new NavMesh();
_mesh.Initialize(32768, 4096, Helpers.Origin, Helpers.TileSize, Helpers.TileSize);
var meshData = data;
MeshTile tile;
_mesh.AddTile(data, out tile);
NavMeshQuery _query = new NavMeshQuery();
_query.Initialize(_mesh, 65536);
QueryFilter Filter = new QueryFilter { IncludeFlags = 0xFFFF, ExcludeFlags = 0x0 };
var startRef = _query.FindNearestPolygon(start, extents, Filter);
var endRef = _query.FindNearestPolygon(end, extents, Filter);
uint[] pathCorridor;
var status = _query.FindPath(startRef, endRef, start, end, Filter, out pathCorridor);
if (status.Equals(DetourStatus.Failure) || pathCorridor == null)
throw new Exception("FindPath failed, start: " + startRef + " end: " + endRef);
if (status.HasFlag(DetourStatus.PartialResult))
Console.WriteLine("Warning, partial result: " + status);
float[] finalPath;
StraightPathFlag[] pathFlags;
uint[] pathRefs;
status = _query.FindStraightPath(start, end, pathCorridor, out finalPath, out pathFlags, out pathRefs);
if (status.Equals(DetourStatus.Failure) || (finalPath == null || pathFlags == null || pathRefs == null))
throw new Exception("FindStraightPath failed, refs in corridor: " + pathCorridor.Length);
}
/// <summary>
/// Move along the NavMeshQuery and update the position
/// </summary>
/// <param name="npos">Current position</param>
/// <param name="navquery">The NavMeshQuery</param>
/// <returns>True if position changed, false if not</returns>
public bool MovePosition(Vector3 npos, NavMeshQuery navquery)
{
const int MaxVisited = 16;
Vector3 result = new Vector3();
List<NavPolyId> visited = new List<NavPolyId>(MaxVisited);
NavPoint startPoint = new NavPoint(path[0], pos);
//move along navmesh and update new position
bool status = navquery.MoveAlongSurface(ref startPoint, ref npos, out result, visited);
if (status == true)
{
MergeCorridorStartMoved(path, visited);
//adjust the position to stay on top of the navmesh
float h = pos.Y;
navquery.GetPolyHeight(path[0], result, ref h);
result.Y = h;
pos = result;
return true;
}
return false;
}
/// <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>
/// 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 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);
const int MaxRes = 32;
PolyId[] res = new PolyId[MaxRes];
float t = 0;
Vector3 norm = new Vector3();
int nres = 0;
navquery.Raycast(new NavPoint(path[0], pos), goal, ref t, ref norm, res, ref nres, MaxRes);
if (nres > 1 && t > 0.99f)
{
pathCount = MergeCorridorStartShortcut(path, pathCount, maxPath, res, nres);
}
}
// Use this for initialization
void Start()
{
var navQuery = FindObjectOfType(typeof(RecastNavMeshQuery)) as RecastNavMeshQuery;
if (navQuery != null)
{
_navMeshQuery = navQuery._navMeshQuery;
filter = navQuery.filter;
crowd = navQuery.Crowd;
param = new CrowdAgentParams
{
Radius = Radius,
Height = Height,
MaxAcceleration = MaxAcceleration,
MaxSpeed = MaxSpeed,
CollisionQueryRange = CollisionQueryRange,
PathOptimizationRange = PathOptimizationRange,
UpdateFlags = UpdateFlags,
ObstacleAvoidanceType = ObstacleAvoidanceType,
SeparationWeight = SeparationWeight
};
AgentId = navQuery.Crowd.AddAgent(new[] { transform.position.x, transform.position.y, transform.position.z }, param);
ResetTarget();
}
else
{
Debug.LogError("Scene does not have a Nav Mesh Query, one must be added.");
}
}
public bool MoveOverOffmeshConnection(PolyId offMeshConRef, PolyId[] refs, ref Vector3 startPos, ref Vector3 endPos, NavMeshQuery navquery)
{
//advance the path up to and over the off-mesh connection
PolyId prevRef = PolyId.Null, polyRef = path[0];
int npos = 0;
while (npos < pathCount && polyRef != offMeshConRef)
{
prevRef = polyRef;
polyRef = path[npos];
npos++;
}
if (npos == pathCount)
{
//could not find offMeshConRef
return false;
}
//prune path
for (int i = npos; i < pathCount; i++)
path[i - npos] = path[i];
pathCount -= npos;
refs[0] = prevRef;
refs[1] = polyRef;
TiledNavMesh nav = navquery.NavMesh;
if (nav.GetOffMeshConnectionPolyEndPoints(refs[0], refs[1], ref startPos, ref endPos) == true)
{
pos = endPos;
return true;
}
return false;
}
/// <summary>
/// Tries to find the straightest path between 2 polygons
/// </summary>
/// <param name="navMeshQuery"></param>
/// <param name="startPos"></param>
/// <param name="endPos"></param>
/// <param name="minTargetDistance"></param>
/// <param name="path"></param>
/// <param name="pathSize"></param>
/// <param name="steerPos"></param>
/// <param name="steerPosFlag"></param>
/// <param name="steerPosRef"></param>
/// <param name="outPoints"></param>
/// <param name="outPointCount"></param>
/// <returns></returns>
private bool GetSteerTarget(NavMeshQuery navMeshQuery, float[] startPos, float[] endPos, float minTargetDistance,
long[] path, int pathSize, ref float[] steerPos, ref short steerPosFlag,
ref long steerPosRef)
{
float[] outPoints = null;
int outPointsCount = 0;
return GetSteerTarget(navMeshQuery, startPos, endPos, minTargetDistance,
path, pathSize, ref steerPos, ref steerPosFlag,
ref steerPosRef, ref outPoints, ref outPointsCount);
}
/// <summary>
/// Initializes a new instance of the <see cref="Crowd" /> class.
/// </summary>
/// <param name="maxAgents">The maximum agents allowed</param>
/// <param name="maxAgentRadius">The maximum radius for an agent</param>
/// <param name="navMesh">The navigation mesh</param>
public Crowd(int maxAgents, float maxAgentRadius, ref TiledNavMesh navMesh)
{
this.maxAgents = maxAgents;
this.maxAgentRadius = maxAgentRadius;
this.ext = new Vector3(maxAgentRadius * 2.0f, maxAgentRadius * 1.5f, maxAgentRadius * 2.0f);
//initialize proximity grid
this.grid = new ProximityGrid<Agent>(maxAgents * 4, maxAgentRadius * 3);
//allocate obstacle avoidance query
this.obstacleQuery = new ObstacleAvoidanceQuery(6, 8);
//initialize obstancle query params
this.obstacleQueryParams = new ObstacleAvoidanceQuery.ObstacleAvoidanceParams[AgentMaxObstacleAvoidanceParams];
for (int i = 0; i < this.obstacleQueryParams.Length; i++)
{
this.obstacleQueryParams[i].VelBias = 0.4f;
this.obstacleQueryParams[i].WeightDesVel = 2.0f;
this.obstacleQueryParams[i].WeightCurVel = 0.75f;
this.obstacleQueryParams[i].WeightSide = 0.75f;
this.obstacleQueryParams[i].WeightToi = 2.5f;
this.obstacleQueryParams[i].HorizTime = 2.5f;
this.obstacleQueryParams[i].GridSize = 33;
this.obstacleQueryParams[i].AdaptiveDivs = 7;
this.obstacleQueryParams[i].AdaptiveRings = 2;
this.obstacleQueryParams[i].AdaptiveDepth = 5;
}
this.pathq = new PathQueue(4096, ref navMesh);
this.agents = new Agent[maxAgents];
this.activeAgents = new Agent[maxAgents];
this.agentAnims = new AgentAnimation[maxAgents];
for (int i = 0; i < maxAgents; i++)
{
this.agents[i] = new Agent();
}
for (int i = 0; i < maxAgents; i++)
{
this.agentAnims[i].Active = false;
}
//allocate nav mesh query
this.navQuery = new NavMeshQuery(navMesh, 512);
}
/// <summary>
/// Builds the initial query, normally called by awake
/// Can be callen manually with a different nav mesh.
/// </summary>
/// <param name="navMesh"></param>
public void InitializeQuery(Detour.NavMesh navMesh)
{
_navMeshQuery = new NavMeshQuery();
_navMeshQuery.Init(navMesh, 2048);
filter = new QueryFilter();
// These values need to me modifiable in the editor later using RecastArea
filter.IncludeFlags = 15;
filter.ExcludeFlags = 0;
filter.SetAreaCost(1, 1.0f);
filter.SetAreaCost(2, 10.0f);
filter.SetAreaCost(3, 1.0f);
filter.SetAreaCost(4, 1.0f);
filter.SetAreaCost(5, 2);
filter.SetAreaCost(6, 1.5f);
}
/// <summary>
/// Move along the NavMeshQuery and update the position
/// </summary>
/// <param name="npos">Current position</param>
/// <param name="navquery">The NavMeshQuery</param>
/// <returns>True if position changed, false if not</returns>
public bool MovePosition(Vector3 npos, NavMeshQuery navquery)
{
//move along navmesh and update new position
Vector3 result = new Vector3();
const int MaxVisited = 16;
PolyId[] visited = new PolyId[MaxVisited];
List<PolyId> listVisited = new List<PolyId>(MaxVisited);
bool status = navquery.MoveAlongSurface(new NavPoint(path[0], pos), npos, ref result, listVisited);
visited = listVisited.ToArray(); //HACK why?
if (status == true)
{
pathCount = MergeCorridorStartMoved(path, pathCount, maxPath, visited, listVisited.Count);
//adjust the position to stay on top of the navmesh
float h = pos.Y;
navquery.GetPolyHeight(path[0], result, ref h);
result.Y = h;
pos = result;
return true;
}
return false;
}
public int FindCorners(Vector3[] cornerVerts, int[] cornerFlags, PolyId[] cornerPolys, int maxCorners, NavMeshQuery navquery)
{
const float MinTargetDist = 0.01f;
int numCorners = 0;
navquery.FindStraightPath(pos, target, path, pathCount, cornerVerts, cornerFlags, cornerPolys, ref numCorners, maxCorners, 0);
//prune points in the beginning of the path which are too close
while (numCorners > 0)
{
if (((cornerFlags[0] & PathfindingCommon.STRAIGHTPATH_OFFMESH_CONNECTION) != 0) ||
Vector3Extensions.Distance2D(cornerVerts[0], pos) > MinTargetDist)
break;
numCorners--;
if (numCorners > 0)
{
for (int i = 0; i < numCorners; i++)
{
cornerFlags[i] = cornerFlags[i + 1];
cornerPolys[i] = cornerPolys[i + 1];
cornerVerts[i] = cornerVerts[i + 1];
}
}
}
//prune points after an off-mesh connection
for (int i = 0; i < numCorners; i++)
{
if ((cornerFlags[i] & PathfindingCommon.STRAIGHTPATH_OFFMESH_CONNECTION) != 0)
{
numCorners = i + 1;
break;
}
}
return numCorners;
}
/// <summary>
/// Determines whether all the polygons in the path are valid
/// </summary>
/// <param name="maxLookAhead">The amount of polygons to examine</param>
/// <param name="navquery">The NavMeshQuery</param>
/// <returns>True if all valid, false if otherwise</returns>
public bool IsValid(int maxLookAhead, NavMeshQuery navquery)
{
int n = Math.Min(pathCount, maxLookAhead);
for (int i = 0; i < n; i++)
{
if (!navquery.IsValidPolyRef(path[i]))
return false;
}
return true;
}
private bool GetSteerTarget(NavMeshQuery navMeshQuery, float[] startPos, float[] endPos, float minTargetDistance, long[] path, int pathSize, ref float[] steerPos, ref short steerPosFlag, ref long steerPosRef, ref float[] outPoints, ref int outPointCount)
{
int MaxSteerPoints = 3;
float[] steerPath = new float[MaxSteerPoints * 3];
short[] steerPathFlags = new short[MaxSteerPoints];
long[] steerPathPolys = new long[MaxSteerPoints];
int nSteerPath = 0;
navMeshQuery.FindStraightPath(startPos, endPos, path, pathSize, ref steerPath, ref steerPathFlags,
ref steerPathPolys, ref nSteerPath, MaxSteerPoints);
if (nSteerPath == 0)
return false;
if (outPoints != null && outPointCount > 0)
{
outPointCount = nSteerPath;
for (int i = 0; i < nSteerPath; i++)
{
Array.Copy(steerPath, i * 3, outPoints, i * 3, 3);
}
}
int ns = 0;
while (ns < nSteerPath)
{
if ((steerPathFlags[ns] & StraightPathOffMeshConnection) != 0 ||
!InRange(steerPath[ns * 3 + 0], steerPath[ns * 3 + 1], steerPath[ns * 3 + 2], startPos, minTargetDistance, 1000.0f))
break;
ns++;
}
if (ns >= nSteerPath)
return false;
Array.Copy(steerPath, ns * 3, steerPos, 0, 3);
steerPos[1] = startPos[1];
steerPosFlag = steerPathFlags[ns];
steerPosRef = steerPathPolys[ns];
return true;
}
/// <summary>
/// Determines whether the polygon reference is a part of the NavMeshQuery.
/// </summary>
/// <param name="navquery">The NavMeshQuery</param>
/// <returns>True if valid, false if not</returns>
public bool IsValid(NavMeshQuery navquery)
{
if (numPolys == 0)
return false;
for (int i = 0; i < numPolys; i++)
{
if (!navquery.IsValidPolyRef(polys[i]))
return false;
}
return true;
}
/// <summary>
/// Examine polygons in the NavMeshQuery and add polygon edges
/// </summary>
/// <param name="reference">The starting polygon reference</param>
/// <param name="pos">Current position</param>
/// <param name="collisionQueryRange">Range to query</param>
/// <param name="navquery">The NavMeshQuery</param>
public void Update(PolyId reference, Vector3 pos, float collisionQueryRange, NavMeshQuery navquery)
{
const int MAX_SEGS_PER_POLY = PathfindingCommon.VERTS_PER_POLYGON;
if (reference == PolyId.Null)
{
this.center = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue);
this.segCount = 0;
this.numPolys = 0;
return;
}
this.center = pos;
//first query non-overlapping polygons
PolyId[] tempArray = new PolyId[polys.Length];
navquery.FindLocalNeighbourhood(new NavPoint(reference, pos), collisionQueryRange, polys, tempArray, ref numPolys, MaxLocalPolys);
//secondly, store all polygon edges
this.segCount = 0;
Segment[] segs = new Segment[MAX_SEGS_PER_POLY];
int numSegs = 0;
for (int j = 0; j < numPolys; j++)
{
tempArray = new PolyId[segs.Length];
navquery.GetPolyWallSegments(polys[j], segs, tempArray, ref numSegs, MAX_SEGS_PER_POLY);
for (int k = 0; k < numSegs; k++)
{
//skip too distant segments
float tseg;
float distSqr = Distance.PointToSegment2DSquared(ref pos, ref segs[k].Start, ref segs[k].End, out tseg);
if (distSqr > collisionQueryRange * collisionQueryRange)
continue;
AddSegment(distSqr, segs[k]);
}
}
}
/// <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)
{
if (pathCount < 3)
return false;
const int MaxIter = 32;
const int MaxRes = 32;
PolyId[] res = new PolyId[MaxRes];
int numRes = 0;
int tempInt = 0;
navquery.InitSlicedFindPath(new NavPoint(path[0], pos), new NavPoint(path[pathCount - 1], target));
navquery.UpdateSlicedFindPath(MaxIter, ref tempInt);
bool status = navquery.FinalizedSlicedPathPartial(path, pathCount, res, ref numRes, MaxRes);
if (status == true && numRes > 0)
{
pathCount = MergeCorridorStartShortcut(path, pathCount, maxPath, res, numRes);
return true;
}
return false;
}
