IEnumerator Pathfind()
{
frontier.Enqueue(new InternalPathNode(startPos, ActionType.WALK), 0);
movementCosts.Set(startPos, 0);
parents.Set(startPos, new InternalPathNode(startPos, ActionType.WALK));
closestDistance = int.MaxValue;
InternalPathNode current;
var cycles = 0;
while (frontier.Count > 0)
{
++cycles;
current = frontier.Dequeue();
var adj = GetAdjacent(current.coord);
foreach (var pos in adj)
{
if (!IsClosed(pos.coord))
{
if (!frontier.Contains(pos))
{
movementCosts.Set(pos.coord, Mathf.Infinity); // so (new < orig) comparison works
}
UpdateNode(current, pos);
if (pos.coord == endPos)
{
ReconstructPath(pos);
yield break;
}
if (HexUtil.DistanceFlat(pos.coord, endPos) < closestDistance)
{
closestDistance = HexUtil.DistanceFlat(pos.coord, endPos);
closestPoint = pos;
}
}
}
if (cycles >= cyclesPerFrame)
{
cycles = 0;
yield return(null);
}
}
// endpos was not on connected graph; return closest node
ReconstructPath(closestPoint);
}
private void Update()
{
var adjacencyRule = (AdjacencyRule)_baseMap.DistanceMeasurement;
var openSet = new GenericPriorityQueue <PositionNode, double>(Width * Height);
foreach (var point in _baseMap.Walkable)
{
var newPoint = _baseMap[point] * -Magnitude;
_goalMap[point] = newPoint;
openSet.Enqueue(_nodes[point], newPoint.Value);
}
var edgeSet = new HashSet <Coord>();
var closedSet = new HashSet <Coord>();
while (openSet.Count > 0) //multiple runs are needed to deal with islands
{
var minNode = openSet.Dequeue();
closedSet.Add(minNode.Position);
foreach (var openPoint in adjacencyRule.Neighbors(minNode.Position))
{
if ((!closedSet.Contains(openPoint)) && _baseMap.BaseMap[openPoint] != GoalState.Obstacle)
{
edgeSet.Add(openPoint);
}
}
while (edgeSet.Count > 0)
{
foreach (var coord in edgeSet.ToArray())
{
var current = _goalMap[coord].Value;
foreach (var openPoint in adjacencyRule.Neighbors(coord))
{
if (closedSet.Contains(openPoint) || _baseMap.BaseMap[openPoint] == GoalState.Obstacle)
{
continue;
}
var neighborValue = _goalMap[openPoint].Value;
var newValue = current + _baseMap.DistanceMeasurement.Calculate(coord, openPoint);
if (newValue < neighborValue)
{
_goalMap[openPoint] = newValue;
openSet.UpdatePriority(_nodes[openPoint], newValue);
edgeSet.Add(openPoint);
}
}
edgeSet.Remove(coord);
closedSet.Add(coord);
openSet.Remove(_nodes[coord]);
}
}
}
}
/// <summary>
/// Enqueue the item with the given priority, without calling lock(_queue) or AddToNodeCache(node)
/// </summary>
/// <param name="item"></param>
/// <param name="priority"></param>
/// <returns></returns>
private SimpleNode EnqueueNoLockOrCache(TItem item, TPriority priority)
{
SimpleNode node = new SimpleNode(item);
if (_queue.Count == _queue.MaxSize)
{
_queue.Resize(_queue.MaxSize * 2 + 1);
}
_queue.Enqueue(node, priority);
return(node);
}
/// <summary>
/// Enqueue a node to the priority queue. Lower values are placed in front. Ties are broken by first-in-first-out.
/// This queue automatically resizes itself, so there's no concern of the queue becoming 'full'.
/// Duplicates are allowed.
/// O(log n)
/// </summary>
public void Enqueue(TItem item, TPriority priority)
{
lock (_queue)
{
SimpleNode node = new SimpleNode(item);
if (_queue.Count == _queue.MaxSize)
{
_queue.Resize(_queue.MaxSize * 2 + 1);
}
_queue.Enqueue(node, priority);
}
}
private static Path shortestPath(Tile a, Tile b, Map m)
{
//If runtime is too high, use a hash table to store indices
dijkstraNode[] nodes = new dijkstraNode[moveList.Count];
index[] indices = new index[moveList.Count];
for (int i = 0; i < moveList.Count; ++i)
{
nodes[i].found = false;
nodes[i].dist = 999;
nodes[i].previous = -1;
indices[i] = new index(i);
}
int start = moveList.IndexOf(a);
int end = moveList.IndexOf(b);
//<index,weight>
GenericPriorityQueue <index, int> queue = new GenericPriorityQueue <index, int>(moveList.Count);
nodes[start].dist = 0;
for (int i = 0; i < moveList.Count; i++)
{
if (i == start)
{
queue.Enqueue(indices[i], 0);
}
else
{
queue.Enqueue(indices[i], 999);
}
}
index tempi;
int x, y, r;
while (queue.Count != 0)
{
//for(int k = 0; k<queue.Count; k++) {
tempi = queue.Dequeue();
nodes[tempi.i].found = true;
if (tempi.i == end)
{
break;
}
x = moveList[tempi.i].getX();
y = moveList[tempi.i].getY();
//ideally we shorten this; maybe do some sin/cos stuff in UserMath
//east
r = moveList.IndexOf(m.findTile(x + 1, y));
if (x < m.getMapWidth() - 1 && r != -1 && !nodes[r].found)
{
if (!nodes[r].found && nodes[r].dist > movementCosts[moveList[r].getType()] + nodes[tempi.i].dist)
{
nodes[r].dist = movementCosts[moveList[r].getType()] + nodes[tempi.i].dist;
nodes[r].previous = tempi.i;
queue.UpdatePriority(indices[r], nodes[r].dist);
}
}
//north
r = moveList.IndexOf(m.findTile(x, y + 1));
if (r != -1 && !nodes[r].found && y < m.getMapHeight() - 1)
{
if (!nodes[r].found && nodes[r].dist > movementCosts[moveList[r].getType()] + nodes[tempi.i].dist)
{
nodes[r].dist = movementCosts[moveList[r].getType()] + nodes[tempi.i].dist;
nodes[r].previous = tempi.i;
queue.UpdatePriority(indices[r], nodes[r].dist);
}
}
//west
r = moveList.IndexOf(m.findTile(x - 1, y));
if (r != -1 && !nodes[r].found && x > 0)
{
if (!nodes[r].found && nodes[r].dist > movementCosts[moveList[r].getType()] + nodes[tempi.i].dist)
{
nodes[r].dist = movementCosts[moveList[r].getType()] + nodes[tempi.i].dist;
nodes[r].previous = tempi.i;
queue.UpdatePriority(indices[r], nodes[r].dist);
}
}
//south
r = moveList.IndexOf(m.findTile(x, y - 1));
if (r != -1 && !nodes[r].found && y > 0)
{
if (!nodes[r].found && nodes[r].dist > movementCosts[moveList[r].getType()] + nodes[tempi.i].dist)
{
nodes[r].dist = movementCosts[moveList[r].getType()] + nodes[tempi.i].dist;
nodes[r].previous = tempi.i;
queue.UpdatePriority(indices[r], nodes[r].dist);
}
}
} //end of while-loop
Path ret = new Path();
if (nodes[end].previous == -1)
{
return(null);
}
for (int i = end; i != start;)
{
ret.insertHead(moveList[i], movementCosts[moveList[i].getType()]);
i = nodes[i].previous;
}
//To impelement; high runtimes might result in reworking this algorithm
return(ret);
}
public static List <IPathNode> FindPath(IPathNode start, IPathNode finish)
{
if (start == finish)
{
return(new List <IPathNode>());
}
var frontier = new GenericPriorityQueue <IPathNode, int>(1000);
frontier.Enqueue(start, 0);
var cameFrom = new Dictionary <IPathNode, IPathNode>();
var costSoFar = new Dictionary <IPathNode, int>();
IPathNode current;
IPathNode next;
List <IPathNode> neighbours;
int newCost;
int priority;
cameFrom[start] = null;
costSoFar[start] = 0;
while (frontier.Count > 0)
{
current = frontier.Dequeue();
if (current == finish)
{
break;
}
neighbours = current.GetNeighbours();
for (int i = 0; i < neighbours.Count; i++)
{
next = neighbours[i];
newCost = costSoFar[current] + (next == finish || next.IsWalkable ? 1 : 10000);
if (!costSoFar.ContainsKey(next) || newCost < costSoFar[next])
{
if (costSoFar.ContainsKey(next))
{
costSoFar[next] = newCost;
}
else
{
costSoFar.Add(next, newCost);
}
priority = newCost + finish.GetHeuristic(next);
if (frontier.Contains(next))
{
frontier.UpdatePriority(next, priority);
}
else
{
frontier.Enqueue(next, priority);
}
cameFrom[next] = current;
}
}
}
var path = new List <IPathNode>();
if (cameFrom.ContainsKey(finish))
{
IPathNode last = finish;
path.Add(finish);
while (cameFrom[last] != start)
{
path.Add(cameFrom[last]);
last = cameFrom[last];
}
path.Reverse();
}
return(path);
}
/// <summary>
/// A* search
/// </summary>
/// <param name="allowReverse"></param>
/// <param name="carsToIgnore"></param>
/// <param name="consistLength"></param>
protected async System.Threading.Tasks.Task Astar(bool allowReverse, HashSet <string> carsToIgnore, double consistLength, List <TrackTransition> bannedTransitions)
{
await Await.BackgroundSyncContext();
cameFrom = new Dictionary <RailTrack, RailTrack>();
costSoFar = new Dictionary <RailTrack, double>();
//var queue = new PriorityQueue<RailTrack>();
var queue = new GenericPriorityQueue <RailTrackNode, double>(10000);
queue.Enqueue(new RailTrackNode(start), 0.0);
cameFrom.Add(start, start);
costSoFar.Add(start, 0.0);
RailTrack current = null;
while (queue.Count > 0)
{
current = queue.Dequeue().track;
RailTrack prev = null;
cameFrom.TryGetValue(current, out prev);
string debug = $"ID: {current.logicTrack.ID.FullID} Prev: {prev?.logicTrack.ID.FullID}";
List <RailTrack> neighbors = new List <RailTrack>();
if (current.outIsConnected)
{
neighbors.AddRange(current.GetAllOutBranches().Select(b => b.track));
}
if (current.inIsConnected)
{
neighbors.AddRange(current.GetAllInBranches().Select(b => b.track));
}
string branches = DumpNodes(neighbors, current);
debug += "\n" + $"all branches: {branches}";
#if DEBUG2
Terminal.Log(debug);
#endif
foreach (var neighbor in neighbors)
{
if (bannedTransitions != null && bannedTransitions.All(t => t.track == current && t.nextTrack == neighbor))
{
Terminal.Log($"{current.logicTrack.ID.FullID}->{neighbor.logicTrack.ID.FullID} banned");
continue;
}
//if non start/end track is not free omit it
if (neighbor != start && neighbor != goal && !neighbor.logicTrack.IsFree(carsToIgnore))
{
Terminal.Log($"{neighbor.logicTrack.ID.FullID} not free");
continue;
}
//if we could go through junction directly (without reversing)
bool isDirect = current.CanGoToDirectly(prev, neighbor);
if (!allowReverse && !isDirect)
{
Terminal.Log($"{neighbor.logicTrack.ID.FullID} reverse needed");
continue;
}
// compute exact cost
//double newCost = costSoFar[current] + neighbor.logicTrack.length;
double newCost = costSoFar[current] + neighbor.logicTrack.length / neighbor.GetAverageSpeed();
if (!isDirect)
{
// if we can't fit consist on this track to reverse, drop this neighbor
if (prev != null && !current.IsDirectLengthEnough(prev, consistLength))
{
Terminal.Log($"{neighbor.logicTrack.ID.FullID} not long enough to reverse");
continue;
}
//add penalty when we must reverse
//newCost += 2.0 * consistLength + 30.0;
}
// If there's no cost assigned to the neighbor yet, or if the new
// cost is lower than the assigned one, add newCost for this neighbor
if (!costSoFar.ContainsKey(neighbor) || newCost < costSoFar[neighbor])
{
// If we're replacing the previous cost, remove it
if (costSoFar.ContainsKey(neighbor))
{
costSoFar.Remove(neighbor);
cameFrom.Remove(neighbor);
}
//Terminal.Log($"neighbor {neighbor.logicTrack.ID.FullID} update {newCost}");
costSoFar.Add(neighbor, newCost);
cameFrom.Add(neighbor, current);
double priority = newCost + Heuristic(neighbor, goal)
/ 20.0f; //convert distance to time (t = s / v)
queue.Enqueue(new RailTrackNode(neighbor), priority);
}
}
}
await Await.UnitySyncContext();
}
private void Update()
{
int width = Width;
AdjacencyRule adjacencyRule = _baseMap.DistanceMeasurement;
var mapBounds = _goalMap.Bounds();
var walkable = _baseMap.Walkable;
for (int i = 0; i < walkable.Count; i++)
{
var point = walkable[i];
// Value won't be null as null only happens for non-walkable squares
var newPoint = _baseMap[point] !.Value * -Magnitude;
_goalMap[point] = newPoint;
_openSet.Enqueue(_nodes[point], newPoint);
}
_edgeSet.Clear();
_closedSet.SetAll(false);
while (_openSet.Count > 0) // Multiple runs are needed to deal with islands
{
var minNode = _openSet.Dequeue();
_closedSet[minNode.Position.ToIndex(width)] = true;
for (int i = 0; i < adjacencyRule.DirectionsOfNeighborsCache.Length; i++)
{
var openPoint = minNode.Position + adjacencyRule.DirectionsOfNeighborsCache[i];
if (!mapBounds.Contains(openPoint))
{
continue;
}
if (!_closedSet[openPoint.ToIndex(width)] && _baseMap.BaseMap[openPoint] != GoalState.Obstacle)
{
_edgeSet.Enqueue(openPoint);
}
}
while (_edgeSet.Count > 0)
{
var point = _edgeSet.Dequeue();
var pointIndex = point.ToIndex(width);
if (!mapBounds.Contains(point) || _closedSet[pointIndex])
{
continue;
}
var current = _goalMap[point] !.Value; // Never added non-nulls so this is fine
for (int j = 0; j < adjacencyRule.DirectionsOfNeighborsCache.Length; j++)
{
var openPoint = point + adjacencyRule.DirectionsOfNeighborsCache[j];
if (!mapBounds.Contains(openPoint))
{
continue;
}
if (_closedSet[openPoint.ToIndex(width)] || _baseMap.BaseMap[openPoint] == GoalState.Obstacle)
{
continue;
}
var neighborValue = _goalMap[openPoint] !.Value; // Never added non-nulls so this is fine
var newValue = current + _baseMap.DistanceMeasurement.Calculate(point, openPoint);
if (newValue < neighborValue)
{
_goalMap[openPoint] = newValue;
_openSet.UpdatePriority(_nodes[openPoint], newValue);
_edgeSet.Enqueue(openPoint);
}
}
_closedSet[pointIndex] = true;
_openSet.Remove(_nodes[point]);
}
}
}