public List<Hop> FindPath(Vector3 startVec, Vector3 endVec)
{
var extents = new Vector3(2.5f, 2.5f, 2.5f).ToFloatArray();
var start = startVec.ToRecast().ToFloatArray();
var end = endVec.ToRecast().ToFloatArray();
if (!IsDungeon)
{
LoadAround(startVec, 1);
LoadAround(endVec, 1);
}
var startRef = _query.FindNearestPolygon(start, extents, Filter);
if (startRef == 0)
throw new NavMeshException(DetourStatus.Failure, "No polyref found for start");
var endRef = _query.FindNearestPolygon(end, extents, Filter);
if (endRef == 0)
throw new NavMeshException(DetourStatus.Failure, "No polyref found for end");
uint[] pathCorridor;
var status = _query.FindPath(startRef, endRef, start, end, Filter, out pathCorridor);
if (status.HasFailed() || pathCorridor == null)
throw new NavMeshException(status, "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.HasFailed() || (finalPath == null || pathFlags == null || pathRefs == null))
throw new NavMeshException(status, "FindStraightPath failed, refs in corridor: " + pathCorridor.Length);
var ret = new List<Hop>(finalPath.Length/3);
for (int i = 0; i < (finalPath.Length / 3); i++)
{
if (pathFlags[i].HasFlag(StraightPathFlag.OffMeshConnection))
{
var polyRef = pathRefs[i];
MeshTile tile;
Poly poly;
if (_mesh.GetTileAndPolyByRef(polyRef, out tile, out poly).HasFailed() || (poly == null || tile == null))
throw new NavMeshException(DetourStatus.Failure, "FindStraightPath returned a hop with an unresolvable off-mesh connection");
int polyIndex = _mesh.DecodePolyIndex(polyRef);
int pathId = -1;
for (int j = 0; j < tile.Header.OffMeshConCount; j++)
{
var con = tile.GetOffMeshConnection(j);
if (con == null)
continue;
if (con.PolyId == polyIndex)
{
pathId = (int)con.UserID;
break;
}
}
if (pathId == -1)
throw new NavMeshException(DetourStatus.Failure, "FindStraightPath returned a hop with an poly that lacks a matching off-mesh connection");
ret.Add(BuildFlightmasterHop(pathId));
}
else
{
var hop = new Hop
{
Location =
new Vector3(finalPath[(i*3) + 0], finalPath[(i*3) + 1], finalPath[(i*3) + 2]).
ToWoW(),
Type = HopType.Waypoint
};
ret.Add(hop);
}
}
return ret;
}
public void GetTileByLocation(Vector3 loc, out int x, out int y)
{
CheckDungeon();
var input = loc.ToRecast().ToFloatArray();
float fx, fy;
GetTileByLocation(input, out fx, out fy);
x = (int)Math.Floor(fx);
y = (int)Math.Floor(fy);
}
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);
}
