public static void StopUpdates(NavigatorInfo pathingInfo)
{
if (Instance && Instance.pathUpdateLookup != null)
{
Instance.pathUpdateLookup.Remove(pathingInfo);
}
}
public static void StartUpdates(NavigatorInfo pathingInfo)
{
if (Instance.pathUpdateLookup.Contains(pathingInfo))
{
return;
}
Instance.pathUpdateLookup.Add(pathingInfo);
Instance.pathingPendingUpdate.Enqueue(pathingInfo);
}
public void GetPath(NavigatorInfo info) // CustomIgnoreWaypoint IgnoreFunction, CustomGoal GoalFunction, CustomPenalty PenaltyFunction)
{
Waypoint startNode = null;
var path = info.Path;
var start = info.currentPosition;
var goal = info.goalPosition;
var IgnoreFunction = info.IgnoreWaypointFunction;
var GoalFunction = info.GoalWaypointFunction;
var PenaltyFunction = info.WaypointPenaltyFunction;
info.NodeTraversalCount = 0;
path.Clear();
startNode = GetClosestWaypoint(start); //, goal); // if no starting waypoint, just head toward gaol
//Debug.Log(startNode);
if (startNode == null || (start - goal).magnitude < info.minDistanceToWaypoint)
{
path.Add(start);
path.Add(goal);
return;
}
Waypoint endNode = GetClosestWaypoint(goal); //, start);
if (endNode != null)
{
if (startNode == endNode || (start - goal).sqrMagnitude < (endNode.position - goal).sqrMagnitude)
{
path.Add(start);
path.Add(goal);
return;
}
}
//
Waypoint currentNode = null;
vistedNodes.Clear(); // clear lists
ignoreNodes.Clear();
searchNodes.Clear();
if (IgnoreFunction == null) // set functions to default if not included
{
IgnoreFunction = (Waypoint waypoint) => { return(false); }
}
;
if (GoalFunction == null)
{
GoalFunction = (Waypoint waypoint) => { return(false); }
}
;
if (PenaltyFunction == null)
{
PenaltyFunction = (Waypoint waypoint) => { return(waypoint.penalty); }
}
;
searchNodes.Add(startNode);
startNode.distTravelled = 0f;
startNode.previous = null;
Waypoint closestNode = startNode;
int loopcount = 0;
while (searchNodes.Count > 0 && loopcount < maxNodeTraversal)
{
loopcount++;
currentNode = searchNodes[searchNodes.Count - 1];
searchNodes.RemoveAt(searchNodes.Count - 1);
if (closestNode.distToTarget > currentNode.distToTarget)
{
closestNode = currentNode;
}
vistedNodes.Add(currentNode);
var foundEndWaypoint = false;
foreach (var neighbourInfo in currentNode.neighbours)
{
var neighbour = neighbourInfo.neighbourWaypoint;
if (ignoreNodes.Contains(neighbour))
{
continue;
}
float distTravelled;
distTravelled = currentNode.distTravelled + neighbourInfo.distanceToNeighbour + Mathf.Max(0, PenaltyFunction(neighbour));
if (vistedNodes.Contains(neighbour))
{
continue;
// if (neighbour.distTravelled > distTravelled)
// {
// neighbour.distTravelled = distTravelled;
// neighbour.previous = currentNode;
// }
}
else
{
if (IgnoreFunction(neighbour))
{
ignoreNodes.Add(neighbour);
continue;
}
neighbour.distTravelled = distTravelled;
neighbour.distToTarget = (goal - neighbourInfo.neighbourWaypoint.position).magnitude + Mathf.Max(0, PenaltyFunction(neighbour));
neighbour.previous = currentNode;
if (neighbour == endNode || GoalFunction(neighbour))
{
closestNode = neighbour;
foundEndWaypoint = true;
break;
}
vistedNodes.Add(neighbour);
searchNodes.Add(neighbour);
}
}
if (foundEndWaypoint)
{
break;
}
searchNodes.Sort((x, y) => // sorting nodes in reverse order since it's faster to remove items from the end of a list
{
return(y.hueristic.CompareTo(x.hueristic));
});
}
currentNode = closestNode;
if ((goal - closestNode.position).magnitude > info.minDistanceToWaypoint)
{
ReverseList.Push(goal);
//path.Add(goal);
}
while (currentNode.previous != null)
{
ReverseList.Push(currentNode.position);
//path.Add(currentNode.position);
currentNode = currentNode.previous;
}
if ((currentNode.position - start).magnitude > info.minDistanceToWaypoint)
{
ReverseList.Push(currentNode.position);
//path.Add(currentNode.position);
}
while (ReverseList.Count > 0)
{
path.Add(ReverseList.Pop());
}
//path.Reverse();
info.NodeTraversalCount = loopcount;
return;
}
