protected WayPointPriority _wayRouteIntern(WayPoint start, WayPoint end)
{
PriorityQueue <WayPointPriority> openList = new PriorityQueue <WayPointPriority>();
Dictionary <String, WayPoint> closedList = new Dictionary <string, WayPoint>();
openList.add(new WayPointPriority(start, 0.0f));
do
{
WayPointPriority wpp = openList.minRemove();
if (wpp.wp == end)
{
return(wpp);//Path found!
}
closedList.Add(wpp.wp.Name, wpp.wp);
_expandNode(wpp, ref openList, ref closedList, end);
} while (!openList.isEmpty());
return(null);
}
public WayRoute getWayRoute(WayPoint start, WayPoint end)
{
WayPointPriority wpp = _wayRouteIntern(start, end);
if (wpp == null)
{
return(null);
}
LinkedList <WayPoint> wpList = new LinkedList <WayPoint>();
while (wpp != null)
{
wpList.AddFirst(wpp.wp);
wpp = wpp.previous;
}
return(new WayRoute(wpList.ToArray()));
}
protected void _expandNode(WayPointPriority wpp, ref PriorityQueue <WayPointPriority> openList, ref Dictionary <String, WayPoint> closedList, WayPoint endWP)
{
foreach (WayPoint wp in wpp.wp.ConnectedWP)
{
if (closedList.ContainsKey(wp.Name))
{
continue;
}
float newValue = wpp.Distance + wpp.wp.Position.getDistance(wp.Position);
int index = openList.getIndexOf(new WayPointPriority(wp, 0));
WayPointPriority realConnected = null;
if (index != -1)
{
realConnected = openList.getAt(index);
}
if (realConnected != null && newValue >= realConnected.Distance)
{
continue;
}
float priority = newValue + endWP.Position.getDistance(wp.Position);
if (realConnected == null)
{
realConnected = new WayPointPriority(wp, newValue);
realConnected.Priority = priority;
openList.add(realConnected);
}
else
{
openList.remove(realConnected);
realConnected.Distance = newValue;
realConnected.Priority = priority;
openList.add(realConnected);
}
realConnected.previous = wpp;
}
}
