public IEnumerable<Vector3> FindPath(Vector3 start, Vector3 end, QueryFilter filter, bool hardFail)
{
if (!LoadAppropriateTiles(start, end))
throw new Exception("Correct tiles were not loaded!");
//LoadAllTiles();
float[] extents = new[] { 0.5f, 0.5f, 0.5f };
PolygonReference startRef, endRef;
float[] transformedStart = start.ToFloatArray();
float[] transformedEnd = end.ToFloatArray();
RecastManaged.Helper.Transform(ref transformedStart);
RecastManaged.Helper.Transform(ref transformedEnd);
while ((startRef = DetourMesh.FindNearestPolygon(transformedStart, extents, filter)).PolyIndex == 0)
{
if (extents[0] > 100.0f)
throw new Exception("Extents got too huge");
extents[0] += 0.5f;
extents[1] += 0.5f;
extents[2] += 0.5f;
}
extents = new[] { 0.5f, 0.5f, 0.5f };
while ((endRef = DetourMesh.FindNearestPolygon(transformedEnd, extents, filter)).PolyIndex == 0)
{
if (extents[0] > 100.0f)
throw new Exception("Extents got too huge");
extents[0] += 0.5f;
extents[1] += 0.5f;
extents[2] += 0.5f;
}
var path = DetourMesh.FindPath(startRef, endRef, transformedStart, transformedEnd, filter);
if (path.Length <= 0)
return null;
// if the last poly in the path is not the end poly, a path was not found
if (hardFail && path[path.Length - 1].PolyIndex != endRef.PolyIndex)
return null;
StraightPathFlags[] flags;
PolygonReference[] straightPathRefs;
var straightPath = DetourMesh.FindStraightPath(transformedStart, transformedEnd, path, out flags, out straightPathRefs);
RecastManaged.Helper.InverseTransform(ref straightPath);
List<Vector3> ret = new List<Vector3>(straightPath.Length / 3);
for (int i = 0; i < straightPath.Length / 3; i++)
ret.Add(new Vector3(straightPath[i * 3 + 0], straightPath[i * 3 + 1], straightPath[i * 3 + 2]));
//for (int i = 0; i < straightPath.Length / 3; i++)
// yield return new Vector3(straightPath[i * 3 + 0], straightPath[i * 3 + 1], straightPath[i * 3 + 2]);
return ret;
}
public void GetTileByLocation(Vector3 loc, out int x, out int y)
{
var input = loc.ToFloatArray();
float fx, fy;
GetTileByLocation(input, out fx, out fy);
x = (int)Math.Floor(fx);
y = (int)Math.Floor(fy);
}
private uint FindPoly(Vector3 loc)
{
return _query.FindNearestPolygon(loc.ToFloatArray(), new[] {2.5f, 2.5f, 2.5f}, _filter);
}