public bool PathIsFound(Node currentGridNode, Node targetGridNode)
{
if (currentGridNode == targetGridNode)
return true;
if (GridMath.GetDistance(currentGridNode, targetGridNode) <= DistanceTolerance)
return true;
return false;
}
/// <summary>
/// Distance Tolerance is in 10x range, so if you want 1 it should be 10.
/// </summary>
/// <param name="zoneMap"></param>
/// <param name="startPos"></param>
/// <param name="targetPos"></param>
/// <param name="pathfindingStyle"></param>
/// <returns></returns>
public static Vector3[] FindWaypoints(ZoneMap zoneMap, Vector3 startPos, Vector3 targetPos,
IPathfindingStyle pathfindingStyle = null)
{
if (pathfindingStyle == null)
pathfindingStyle = new ExactMatchStyle();
var sw = new Stopwatch();
sw.Start();
var pathSuccess = false;
var startGridNode = zoneMap.GetNodeFromWorldPoint(startPos);
var targetGridNode = zoneMap.GetNodeFromWorldPoint(targetPos);
// // startGridNode.Walkable &&
// if (targetGridNode.Walkable)
// {
var openSet = new Heap<Node>(zoneMap.MaxSize);
var closedSet = new HashSet<Node>();
openSet.Add(startGridNode);
while (openSet.Count > 0)
{
var currentGridNode = openSet.RemoveFirst();
closedSet.Add(currentGridNode);
// var distance = GridMath.GetDistance(currentGridNode, targetGridNode);
// if (distance <= distanceTolerance)
if (pathfindingStyle.PathIsFound(currentGridNode, targetGridNode))
{
sw.Stop();
Console.WriteLine("Path found: " + sw.ElapsedMilliseconds + "ms");
pathSuccess = true;
var waypoints = RetracePath(startGridNode, currentGridNode);
return waypoints;
}
foreach (var neighbour in zoneMap.GetNeighbours(currentGridNode))
{
if ((!neighbour.Walkable && neighbour != targetGridNode) || closedSet.Contains(neighbour)) continue;
int newMovementCostToNeighbour =
currentGridNode.GCost + GridMath.GetDistance(currentGridNode, neighbour) +
neighbour.MovementPenalty;
if (newMovementCostToNeighbour < neighbour.GCost || !openSet.Contains(neighbour))
{
neighbour.GCost = newMovementCostToNeighbour;
neighbour.HCost = GridMath.GetDistance(neighbour, targetGridNode);
neighbour.Parent = currentGridNode;
if (!openSet.Contains(neighbour))
openSet.Add(neighbour);
else
openSet.UpdateItem(neighbour);
}
}
}
// }
if (!pathSuccess) return null;
// var waypoints = RetracePath(startGridNode, targetGridNode);
return null;
}
