public override IEnumerable <Node> AStar(Node from, Node to)
{
var openSet = new FastPriorityQueue <Node>(Width * Height);
openSet.Enqueue(from, 0f);
while (openSet.Count > 0)
{
var currentNode = openSet.Dequeue();
if (currentNode == to)
{
var path = ReconstructPath(currentNode);
foreach (var node in this)
{
node.Reset();
}
return(path);
}
currentNode.InClosedSet = true;
foreach (var neighbor in currentNode.Neighbors)
{
if (neighbor.InClosedSet)
{
continue;
}
var tentativeGScore = currentNode.GScore + currentNode.Distance(neighbor);
if (openSet.Contains(neighbor) && tentativeGScore >= neighbor.GScore)
{
continue;
}
neighbor.CameFrom = currentNode;
neighbor.GScore = tentativeGScore;
var fs = tentativeGScore + HeuristicCostEstimate(neighbor, to);
if (openSet.Contains(neighbor))
{
openSet.UpdatePriority(neighbor, fs);
}
else
{
openSet.Enqueue(neighbor, fs);
}
}
}
foreach (var node in this)
{
node.Reset();
}
return(new List <Node>());
}
public void AddSeed(int id, float seed_dist)
{
GraphNode g = get_node(id);
Debug.Assert(Queue.Contains(g) == false);
enqueue_node(g, seed_dist);
Seeds.Add(id);
}
void FindPath(Vector3 a_StartPos, Vector3 a_TargetPos)
{
Node StartNode = nodeMesh.NodeFromWorldPoint(a_StartPos); //Gets the node closest to the starting position
Node TargetNode = nodeMesh.NodeFromWorldPoint(a_TargetPos); //Gets the node closest to the target position
if (StartNode != null && TargetNode != null)
{
FastPriorityQueue <Node> OpenList = new FastPriorityQueue <Node>(200);
//SimplePriorityQueue<Node> OpenList = new FastPriorityQueue<Node>(maxN_Calculations);//List of nodes for the open list
HashSet <Node> ClosedList = new HashSet <Node>(); //Hashset of nodes for the closed list
OpenList.Enqueue(StartNode, StartNode.FCost); //Add the starting node to the open list to begin the program
while (OpenList.Count > 0) //Whilst there is something in the open list
{
Node CurrentNode = OpenList.Dequeue(); //Create a node and set it to the first item in the open list
OpenList.ResetNode(CurrentNode);
//OpenList.Remove(CurrentNode);//Remove that from the open list
ClosedList.Add(CurrentNode); //And add it to the closed list
if (CurrentNode == TargetNode) //If the current node is the same as the target node
{
GetFinalPath(StartNode, TargetNode); //Calculate the final path
}
//Debug.Log("el nodo en cuestion tiene x vecinos" + CurrentNode.NeighbNodes.Count);
Node[] nNodes = nodeMesh.GetNeighbNodes(CurrentNode);
for (int i = 0; i < nNodes.Length; i++)
{
Node NeighborNode = nNodes[i];
if (NeighborNode == null)
{
continue;
}
if (NeighborNode.BIsWall || ClosedList.Contains(NeighborNode))
{
continue; //If the neighbor is a wall or has already been checked // Skip it
}
int MoveCost = CurrentNode.IgCost + GetManhattenDistance(CurrentNode, NeighborNode); //Get the F cost of that neighbor
if (MoveCost < NeighborNode.IgCost || !OpenList.Contains(NeighborNode)) //If the f cost is greater than the g cost or it is not in the open list
{
NeighborNode.IgCost = MoveCost; //Set the g cost to the f cost
NeighborNode.IhCost = GetManhattenDistance(NeighborNode, TargetNode); //Set the h cost
NeighborNode.ParentNode1 = CurrentNode; //Set the parent of the node for retracing steps
if (!OpenList.Contains(NeighborNode)) //If the neighbor is not in the openlist
{
//Add it to the list
OpenList.Enqueue(NeighborNode, NeighborNode.FCost);
}
}
}
}
//while (OpenList.Count > 0)
//{
// OpenList.ResetNode(OpenList.Dequeue());
//}
}
}
public override IEnumerable <Node> AStar(Node from, Node to)
{
var closedSet = new HashSet <Node>();
var openSet = new FastPriorityQueue <Node>(Width * Height);
openSet.Enqueue(@from, 0f);
var cameFrom = new Dictionary <Node, Node>();
var gScore = new Dictionary <Node, float> {
{ from, 0f }
};
while (openSet.Count > 0)
{
var currentNode = openSet.Dequeue();
if (currentNode == to)
{
return(ReconstructPath(cameFrom, currentNode));
}
closedSet.Add(currentNode);
foreach (var neighbor in currentNode.Neighbors)
{
if (closedSet.Contains(neighbor))
{
continue;
}
var tentativeGScore = gScore[currentNode] + currentNode.Distance(neighbor);
if (openSet.Contains(neighbor) && tentativeGScore >= gScore[neighbor])
{
continue;
}
cameFrom[neighbor] = currentNode;
gScore[neighbor] = tentativeGScore;
var fs = tentativeGScore + HeuristicCostEstimate(neighbor, to);
if (openSet.Contains(neighbor))
{
openSet.UpdatePriority(neighbor, fs);
}
else
{
openSet.Enqueue(neighbor, fs);
}
}
}
return(new List <Node>());
}
protected void AddToFastOpen(Node child, Node parent, FastPriorityQueue <Node> list)
{
double newFValue, gValue;
if (!useThetaStar)
{
newFValue = CalculateFValue(child, parent, out gValue);
}
else
{
newFValue = CalculateFValue(child, parent, out gValue, out parent);
}
if (newFValue == Mathf.Infinity)
{
return;
}
if (!list.Contains(child)) //if child is not already in open list
{
list.Enqueue(child, (float)newFValue); //add to open list
SetValues(child, parent, gValue);
}
else //child is already in open list
{
float oldFValue = child.Priority;
if (newFValue < oldFValue) //if f-value of child in open list is greather than current f-value
{
list.UpdatePriority(child, (float)newFValue);
SetValues(child, parent, gValue);
}
}
}
public override bool Find(DirectionGraph graph, Node start, Node end)
{
priorityQueue = new FastPriorityQueue <Node>(graph.VerticeCount());
searchSteps = new Queue <Node>();
System.Diagnostics.Stopwatch watch = new System.Diagnostics.Stopwatch();
watch.Start();
costForStartToTarget = new Dictionary <Node, int>();
costForStartToTarget[start] = 0;
priorityQueue.Enqueue(start, 0);
while (priorityQueue.Count > 0)
{
Node min = priorityQueue.Dequeue();
if (min == end)
{
watch.Stop();
executionTimes = watch.Elapsed.TotalMilliseconds;
CalculateCostAndGeneratePath(start, end, graph);
return(true);
}
List <DirectionEdge> minNeighbors = graph.GetNeighborEdge(min);
int costForStartToMin = costForStartToTarget[min];
for (int i = 0, count = minNeighbors.Count; i < count; i++)
{
Node neighbor = minNeighbors[i].to;
int costForStartToNeighbor = int.MaxValue;
if (costForStartToTarget.ContainsKey(neighbor))
{
costForStartToNeighbor = costForStartToTarget[neighbor];
}
int newCostSoFar = costForStartToMin + minNeighbors[i].weight;
if (newCostSoFar < costForStartToNeighbor)
{
costForStartToTarget[neighbor] = newCostSoFar;
nodesComeFrom[neighbor] = min;
if (priorityQueue.Contains(neighbor))
{
priorityQueue.UpdatePriority(neighbor, newCostSoFar);
}
else
{
priorityQueue.Enqueue(neighbor, newCostSoFar);
}
}
}
if (min != start)
{
searchSteps.Enqueue(min);
}
}
watch.Stop();
executionTimes = watch.Elapsed.TotalMilliseconds;
searchSteps.Clear();
return(false);
}
/// <summary>
/// Finds a shortest path from the <paramref name="start"/> to the <paramref name="end"/>.
/// It uses a Dijkstra's algorithm with a priority queue.
/// </summary>
string Solve(Map map, Cell start, Cell end)
{
var vertices = new FastPriorityQueue <Cell>(map.TotalMapSize);
var distances = new Dictionary <Cell, float>();
var previous = new Dictionary <Cell, Cell>();
foreach (var cell in map.AllCells())
{
if (!cell.Equals(start))
{
distances[cell] = float.PositiveInfinity;
previous[cell] = null;
vertices.Enqueue(cell, float.PositiveInfinity);
}
}
vertices.Enqueue(start, 0);
distances[start] = 0;
int oneTenth = map.TotalMapSize > 10 ? map.TotalMapSize / 10 : 1;
while (vertices.Any())
{
var u = vertices.Dequeue();
if (reportProgress && vertices.Count % oneTenth == 0)
{
Console.WriteLine($"Remaining nodes {vertices.Count}/{map.TotalMapSize}");
}
foreach (var v in map.GetNeighbors(u))
{
if (!vertices.Contains(v))
{
continue;
}
var currentDistance = distances[v];
var newDistance = distances[u] + v.Difficulty;
if (newDistance < currentDistance)
{
vertices.UpdatePriority(v, newDistance);
distances[v] = newDistance;
previous[v] = u;
if (v.Equals(end))
{
vertices.Clear();
break;
}
}
}
}
return(GeneratePath(end, previous, map));
}
/// <inheritdoc/>
public void ImmediateEvent(IEvent eventToSchedule)
{
if (eventToSchedule == null)
{
throw new ArgumentNullException(nameof(eventToSchedule));
}
if (!(eventToSchedule is BaseEvent castedEvent))
{
throw new ArgumentException($"Argument must be of type {nameof(BaseEvent)}.", nameof(eventToSchedule));
}
lock (_eventsAvailableLock) {
lock (_queueLock) {
if (_priorityQueue.Contains(castedEvent))
{
throw new ArgumentException($"The event is already scheduled.", nameof(eventToSchedule));
}
_priorityQueue.Enqueue(castedEvent, 0);
}
// check and add event attribution to the requestor
if (castedEvent.RequestorId > 0)
{
lock (_eventsPerRequestorLock) {
if (!_eventsPerRequestor.ContainsKey(castedEvent.RequestorId))
{
_eventsPerRequestor.Add(castedEvent.RequestorId, new HashSet <string>());
}
try {
_eventsPerRequestor[castedEvent.RequestorId].Add(castedEvent.EventId);
} catch {
// ignore clashes, it was our goal to add it anyways.
}
}
}
Monitor.Pulse(_eventsAvailableLock);
}
}
/// <summary>
/// Benchmark
/// </summary>
public bool EnqueueContains()
{
Enqueue();
bool ret = true; // to ensure the compiler doesn't optimize the contains calls out of existence
for (int i = 0; i < QueueSize; i++)
{
ret &= Queue.Contains(Nodes[i]);
}
return(ret);
}
void GenerateChunkByTargetPosition()
{
if (target == null)
{
return;
}
Vector3Int targetPosition = VoxelUtil.WorldToChunk(target.position, chunkSize);
if (lastTargetChunkPosition == targetPosition)
{
return;
}
foreach (ChunkNode chunkNode in generateChunkQueue)
{
Vector3Int deltaPosition = targetPosition - chunkNode.chunkPosition;
if (chunkSpawnSize.x < Mathf.Abs(deltaPosition.x) || chunkSpawnSize.y < Mathf.Abs(deltaPosition.y) || chunkSpawnSize.z < Mathf.Abs(deltaPosition.z))
{
generateChunkQueue.Remove(chunkNode);
continue;
}
generateChunkQueue.UpdatePriority(chunkNode, (targetPosition - chunkNode.chunkPosition).sqrMagnitude);
}
for (int x = targetPosition.x - chunkSpawnSize.x; x <= targetPosition.x + chunkSpawnSize.x; x++)
{
for (int y = targetPosition.y - chunkSpawnSize.y; y <= targetPosition.y + chunkSpawnSize.y; y++)
{
for (int z = targetPosition.z - chunkSpawnSize.z; z <= targetPosition.z + chunkSpawnSize.z; z++)
{
Vector3Int chunkPosition = new Vector3Int(x, y, z);
if (chunks.ContainsKey(chunkPosition))
{
continue;
}
ChunkNode newNode = new ChunkNode {
chunkPosition = chunkPosition
};
if (generateChunkQueue.Contains(newNode))
{
continue;
}
generateChunkQueue.Enqueue(newNode, (targetPosition - chunkPosition).sqrMagnitude);
}
}
}
lastTargetChunkPosition = targetPosition;
}
private void AddToQueue(FastPriorityQueue <ChunkNode> queue, int3 position, float priority)
{
ChunkNode node = new ChunkNode(position);
#if DEBUG
if (queue.Contains(node))
{
Debug.LogWarning("Wants to enqueue generator job but it already exists.");
return;
}
#endif
if (queue.Count + 1 >= queue.MaxSize)
{
queue.Resize(queue.Count + 32);
}
queue.Enqueue(node, priority);
}
protected void AddToFastOpen(Node child)
{
newKeyValue = CalculateKeyValue(child);
if (newKeyValue[0] >= float.MaxValue)
{
return;
}
child.priorityKey = newKeyValue;
if (!fastOpenListPrim.Contains(child))
{
fastOpenListPrim.Enqueue(child, (float)newKeyValue[0]);
}
else
{
fastOpenListPrim.UpdatePriority(child, (float)newKeyValue[0]);
}
if (isUpdatingNodes && visualize)
{
child.isInOpenAfterChange = true; //DEBUG
}
}
public int[] Work(bool print = true, int startIndex = 0)
{
int NumberOfNodes = Graph.ListOfNodes.Count;
var queue = new FastPriorityQueue <Node>(100 * 1000);
Node u;
int du;
int dv;
int wuv;
int[] d = new int[NumberOfNodes];
for (int i = 0; i < NumberOfNodes; i++)
{
d[i] = Int32.MaxValue / 10;
}
if (startIndex + 1 > NumberOfNodes)
{
startIndex = 0;
}
d[startIndex] = 0;
for (int i = 0; i < NumberOfNodes; i++)
{
queue.Enqueue(Graph.GetNode(i), 0);
}
while (queue.Any()) // while queue is not empty
{
u = queue.Dequeue();
foreach (var v in u.ListOfEdges)
{
du = d[u.Id];
dv = d[v.Target.Id];
wuv = v.Weight;
if (du + wuv < dv)
{
d[v.Target.Id] = du + wuv;
if (queue.Contains(v.Target))
{
queue.UpdatePriority(v.Target, d[v.Target.Id]);
}
else
{
queue.Enqueue(v.Target, d[v.Target.Id]);
}
}
}
}
if (print)
{
for (int i = 0; i < NumberOfNodes; i++)
{
Console.WriteLine("Id: {0}, distance: {1}", i, d[i]);
}
}
return(d);
}
public virtual T FindPath()
{
var startNode = CreateNode(Start);
startNode.StartCost = 0;
OpenSet.Enqueue(startNode, startNode.TotalCost);
_loopLimiter.Reset();
while (_loopLimiter.Advance())
{
if (OpenSet.Count == 0)
{
break;
}
var centerNode = GetLowest();
if (centerNode.Value.Equals(End))
{
return(centerNode);
}
ClosedSet.Add(centerNode.Value);
//OpenSet.Remove(centerNode);
var surrounding = GetSurrounding(centerNode);
for (int i = 0; i < surrounding.Count; i++)
{
var neighborPos = surrounding[i];
if (neighborPos == null)
{
continue;
}
if (ClosedSet.Contains(neighborPos))
{
continue;
}
if (!CheckWalkable(centerNode.Value, neighborPos))
{
continue;
}
var neighbor = Get(neighborPos);
if (neighbor == null)
{
//if (CheckWalkable(centerNode.Value, neighborPos)) {
neighbor = CreateNode(neighborPos);
//}
// else {
// ClosedSet.Add(neighborPos);
// }
}
if (neighborPos.Equals(End))
{
if (neighbor != null)
{
neighbor.Parent = centerNode;
return(neighbor);
}
if (IgnoreEndWalkable)
{
return(centerNode);
}
}
if (neighbor == null)
{
continue;
}
var newStartCost = centerNode.StartCost + neighbor.GetTravelCost(centerNode);
if (newStartCost < neighbor.StartCost)
{
neighbor.Parent = centerNode;
neighbor.StartCost = newStartCost;
if (OpenSet.Contains(neighbor))
{
OpenSet.UpdatePriority(neighbor, neighbor.TotalCost);
}
else
{
OpenSet.Enqueue(neighbor, neighbor.TotalCost);
}
}
}
}
return(null);
}
/// <summary>
/// FindPathReverse, finds the fastes route between a and b
/// </summary>
/// <param name="a">stop number of the departuring station</param>
/// <param name="b">stop number of the arrival station</param>
/// <param name="finalArrivalTime">ArrivalTime in UTC format.</param>
/// <returns>List of TimeTableId, describing the route for the path</returns>
private List <PathItem> FindPathReverse(int a, int b, long finalArrivalTime)
{
// the path we find as shortest
var cameFrom = new Dictionary <int, TraceBackNode>();
// gScore is the actual UTCtime to get to a stop
var gScore = new Dictionary <int, long>();
gScore[b] = finalArrivalTime;
// a min heap, now 'turned' into a max heap
var heap = new FastPriorityQueue <QueueNode>(5000);
heap.Enqueue(new QueueNode {
StopId = b, Time = finalArrivalTime, TripId = -1
}, -DistanceTime(a, b));
while (heap.Count != 0)
{
var currentNode = heap.Dequeue();
// is arrival station found?
if (currentNode.StopId == a)
{
return(ReconstructPath(cameFrom, currentNode.StopId));
}
// get the neighbours to the currentNode
var neighbours = ts.Edges
.Where(e => e.ToStop == currentNode.StopId)
.Select(e => new { e.EdgeId, e.FromStop, e.DistanceWalk });
foreach (var n in neighbours)
{
Iterations += 1;
long departureTime; // utctime when departing from n.ToStop
int timeTableId = -1;
int tripId = -1;
if (n.DistanceWalk > 0)
{
if ((TimeToWalkSeconds(n.DistanceWalk) > cachedGlobal.MaxWalkingtime) & (currentNode.TripId == -1))
{
continue; // to long to walk, or 2 sections in a row is walking!
}
departureTime = gScore[currentNode.StopId] - TimeToWalkSeconds(n.DistanceWalk);
}
else
{
long gScoreTemp = gScore[currentNode.StopId];
TimeTable timeTableEntry;
timeTableEntry = GetNextArrival(n.EdgeId, gScoreTemp);
if (timeTableEntry == null)
{
continue;
}
// now chek if previous node is not walking, and change of transport and changetime is available
// othwise add changeTime, and search again.
if ((currentNode.TripId != -1)
& (currentNode.TripId != timeTableEntry.TripId)
& (timeTableEntry.ArrivalTime < gScoreTemp - cachedGlobal.TimeToChangeTransport))
{
gScoreTemp -= cachedGlobal.TimeToChangeTransport;
timeTableEntry = GetNextArrival(n.EdgeId, gScoreTemp);
if (timeTableEntry == null) // no path found
{
continue;
}
}
departureTime = timeTableEntry.DepartureTime;
timeTableId = timeTableEntry.TimeTableId;
tripId = timeTableEntry.TripId;
}
if (departureTime > gScore.TryGetValueOrMin(n.FromStop)) // quicker way found, with a smaller value
{
cameFrom[n.FromStop] = new TraceBackNode {
ToStop = currentNode.StopId, EdgeId = n.EdgeId, TimeTableId = timeTableId
};
gScore[n.FromStop] = departureTime;
QueueNode qn = new QueueNode {
StopId = n.FromStop, Time = departureTime, TripId = tripId
};
if (!heap.Contains(qn))
{
int heuristik = DistanceTime(n.FromStop, a); // estimated traveltime from this node to the start
heap.Enqueue(qn, heuristik - departureTime);
}
}
}
}
// no path found!
return(null);
// Local: Get the next timeTable entry in respect to arrival time given
TimeTable GetNextArrival(int edgeId, long arrivalTime)
{
return(ts.TimeTables
.Where(t => t.EdgeId == edgeId && (t.ArrivalTime <= arrivalTime))
.OrderByDescending(t => t.ArrivalTime)
.FirstOrDefault());
}
}
private static List <Vector2> ShortestPath(Graph graph, Vertex start, Vertex target, bool strict = true)
{
var queue = new FastPriorityQueue <Vertex>(graph.VertexCount);
var predecessor = new Dictionary <Vertex, Vertex>();
var gValue = new Dictionary <Vertex, float>();
var processed = new HashSet <Vertex>();
// Initialize all necessary components
gValue[start] = 0;
queue.Enqueue(start, HeuristicCost(start, target));
// Calculate shortest path
while (queue.Count != 0)
{
var current = queue.Dequeue();
if (current == target)
{
return(ReconstructPath(predecessor, target));
}
processed.Add(current);
foreach (var edge in current.Edges)
{
if (strict && edge.Weight > Vector2.Distance(current.Position, edge.TargetVertex.Position))
{
continue;
}
var neighbour = edge.TargetVertex;
if (!gValue.ContainsKey(neighbour))
{
gValue[neighbour] = float.PositiveInfinity;
}
if (processed.Contains(neighbour))
{
continue;
}
var tentativeValue = gValue[current] + edge.Weight;
try
{
if (tentativeValue >= gValue[neighbour])
{
continue;
}
}
catch
{
Debug.WriteLine("Graph vertex count and number of vertices match: " + graph.CheckGraph());
}
predecessor[neighbour] = current;
gValue[neighbour] = tentativeValue;
if (queue.Contains(neighbour))
{
queue.UpdatePriority(neighbour, tentativeValue + HeuristicCost(neighbour, target));
}
else
{
queue.Enqueue(neighbour, tentativeValue + HeuristicCost(neighbour, target));
}
}
}
return(new List <Vector2> {
start.Position, start.Position
});
}
/// <summary>
/// FindPathForward, finds the fastes route between a and b
/// </summary>
/// <param name="a">stop number of the departuring station</param>
/// <param name="b">stop number of the arrival station</param>
/// <param name="utcStart">Departuretime in UTC format.</param>
/// <returns>List of TimeTableId, describing the route for the path</returns>
private List <PathItem> FindPathForward(int a, int b, long utcStart)
{
// the path we find as shortest
var cameFrom = new Dictionary <int, TraceBackNode>();
// gScore is the actual linuxtime to arrive at a stop: <stopId, arrival time>
var gScore = new Dictionary <int, long>();
gScore[a] = utcStart;
// a min heap
var heap = new FastPriorityQueue <QueueNode>(5000);
heap.Enqueue(new QueueNode {
StopId = a, Time = utcStart, TripId = -1
}, DistanceTime(a, b));
while (heap.Count != 0)
{
var currentNode = heap.Dequeue();
// is arrival station found?
if (currentNode.StopId == b)
{
var path = ReconstructPath(cameFrom, currentNode.StopId);
path.Reverse();
return(path);
}
// get the neighbours to the currentNode
var neighbours = ts.Edges
.Where(e => e.FromStop == currentNode.StopId)
.Select(e => new { e.EdgeId, e.ToStop, e.DistanceWalk });
foreach (var n in neighbours)
{
Iterations += 1;
long arrivalTime; // time when arriving to n.ToStop
int timeTableId = -1;
int tripId = -1;
if (n.DistanceWalk > 0)
{
if ((TimeToWalkSeconds(n.DistanceWalk) > cachedGlobal.MaxWalkingtime) | (currentNode.TripId == -1))
{
continue; // to long to walk, or 2 sections in a row is walking!
}
arrivalTime = gScore[currentNode.StopId] + TimeToWalkSeconds(n.DistanceWalk);
}
else
{
long gScoreTemp = gScore[currentNode.StopId]; // TryGetValueOrMax (currentNode.StopId)??
TimeTable timeTableEntry;
timeTableEntry = GetNextDeparture(n.EdgeId, gScoreTemp);
if (timeTableEntry == null) // no path found
{
continue;
}
// now chek if previous node is not walking, and change of transport and changetime is available
// othwise add changeTime, and search again.
if ((currentNode.TripId != -1)
& (currentNode.TripId != timeTableEntry.TripId)
& (timeTableEntry.DepartureTime < gScoreTemp + cachedGlobal.TimeToChangeTransport))
{
gScoreTemp += cachedGlobal.TimeToChangeTransport;
timeTableEntry = GetNextDeparture(n.EdgeId, gScoreTemp);
if (timeTableEntry == null) // no path found
{
continue;
}
}
arrivalTime = timeTableEntry.ArrivalTime;
timeTableId = timeTableEntry.TimeTableId;
tripId = timeTableEntry.TripId;
}
if (arrivalTime < gScore.TryGetValueOrMax(n.ToStop)) // quicker way found
{
cameFrom[n.ToStop] = new TraceBackNode {
ToStop = currentNode.StopId, EdgeId = n.EdgeId, TimeTableId = timeTableId
};
gScore[n.ToStop] = arrivalTime;
QueueNode qn = new QueueNode {
StopId = n.ToStop, Time = arrivalTime, TripId = tripId
};
if (!heap.Contains(qn))
{
int heuristik = DistanceTime(n.ToStop, b); // estimated traveltime from this node to the end
heap.Enqueue(qn, heuristik + arrivalTime);
}
}
}
}
return(null);
// Local: Get the next timetable entry in respect to departuetime given
TimeTable GetNextDeparture(int edgeId, long departureTime)
{
return(ts.TimeTables
.Where(t => t.EdgeId == edgeId && (t.DepartureTime >= departureTime))
.OrderBy(t => t.DepartureTime)
.FirstOrDefault());
}
}
public static void FlowFieldQuadTree(QuadTree quadTree, QuadTree end, out Dictionary <QuadTree, QuadTree> flowField)
{
#if LOG_TIME
var bench = new Stopwatch( );
var avgNeighbourLookup = new List <long> ( );
#endif
#if DEBUG
var timeout = new Stopwatch( );
timeout.Start( );
#endif
flowField = new Dictionary <QuadTree, QuadTree> ( );
var openset = new FastPriorityQueue <QuadTree> (quadTree.SubdivisionCount( ));
var neighbours = new List <QuadTree> ( );
openset.Enqueue(end, 0f);
flowField[end] = end;
QuadTree current = null;
float newcost = 0f;
while (openset.Count > 0)
{
#if DEBUG
if (timeout.ElapsedMilliseconds > 1000)
{
throw new System.OperationCanceledException("flowField Generation Timed Out");
}
#endif
current = openset.Dequeue( );
#if LOG_TIME
bench.Restart( );
#endif
quadTree.Neighbours(current, ref neighbours);
#if LOG_TIME
bench.Stop( );
avgNeighbourLookup.Add(bench.ElapsedTicks);
#endif
foreach (var neighbour in neighbours)
{
if (neighbour.Count != 0)
{
continue;
}
newcost = current.Priority + (current.center - neighbour.center).sqrMagnitude + (neighbour.center - end.center).sqrMagnitude;
if (newcost < neighbour.Priority)
{
if (!openset.Contains(neighbour))
{
openset.Enqueue(neighbour, newcost);
}
else
{
openset.UpdatePriority(neighbour, newcost);
}
flowField[neighbour] = current;
}
}
neighbours.Clear( );
}
#if LOG_TIME
Debug.Log("Average Neighbour Lookup: " + avgNeighbourLookup.Average( ) + " total " + avgNeighbourLookup.Sum( ));
#endif
}
/// <summary>
/// Run the A* algorithm and put the result in path.
/// If no path was found, return 'forever' and put only the destination in path.
/// Returns the total path time.
/// </summary>
public float FindPath(
List <PathNode> path,
Vector3 start,
Vector3 destination,
MobilityData mobility,
float unitRadius,
MoveCommandType command)
{
path.Clear();
path.Add(new PathNode(destination, false));
PathNode cameFromDest = null;
float gScoreDest = Pathfinder.FindLocalPath(this,
start,
destination,
mobility,
unitRadius);
if (gScoreDest < Pathfinder.FOREVER)
{
if (command == MoveCommandType.NORMAL || command == MoveCommandType.REVERSE)
{
return(gScoreDest);
}
}
// Initialize with all nodes accessible from the starting point
// (this can be optimized later by throwing out some from the start)
_openSet.Clear();
List <PathNode> graph = _graphRegularMove;
float neighborSearchDistance = Pathfinder.FOREVER;
// if we are in fast mode our graph is much more extensive and we have to
// limit our neighor distance to use that extensive network of nodes
if (command == MoveCommandType.FAST)
{
graph = _graphFastMove;
neighborSearchDistance = ARC_MAX_DIST;
}
// find the nearest neighbor start A* search
foreach (PathNode neighbor in graph)
{
neighbor.IsClosed = false;
neighbor.CameFrom = null;
neighbor.GScore = Pathfinder.FOREVER;
Vector3 neighborPos = Position(neighbor);
// and optimal search
if ((start - neighborPos).magnitude < neighborSearchDistance)
{
float gScoreNew = Pathfinder.FindLocalPath(this,
start,
neighborPos,
mobility,
unitRadius);
if (gScoreNew < Pathfinder.FOREVER)
{
neighbor.GScore = gScoreNew;
float fScoreNew = gScoreNew + TimeHeuristic(neighborPos, destination, mobility);
_openSet.Enqueue(neighbor, fScoreNew);
}
}
}
// generic A* algorithm based on distance to destination and arc time's as hueristic function weights
while (_openSet.Count > 0)
{
PathNode current = _openSet.Dequeue();
current.IsClosed = true;
if (gScoreDest < current.Priority)
{
break;
}
foreach (PathArc arc in current.Arcs)
{
PathNode neighbor = arc.Node1 == current ? arc.Node2 : arc.Node1;
if (neighbor.IsClosed)
{
continue;
}
float arcTime = arc.Time[mobility.Index];
if (arcTime >= Pathfinder.FOREVER)
{
continue;
}
float gScoreNew = current.GScore + arcTime;
if (gScoreNew >= neighbor.GScore)
{
continue;
}
float fScoreNew = gScoreNew + TimeHeuristic(Position(neighbor),
destination,
mobility);
if (!_openSet.Contains(neighbor))
{
_openSet.Enqueue(neighbor, fScoreNew);
}
else
{
_openSet.UpdatePriority(neighbor, fScoreNew);
}
neighbor.GScore = gScoreNew;
neighbor.CameFrom = current;
}
float arcTimeDest = Pathfinder.FOREVER;
// checks can we get to the last mile without more pathfinding
if (Vector3.Distance(Position(current), destination) < neighborSearchDistance)
{
arcTimeDest = Pathfinder.FindLocalPath(this, Position(current), destination, mobility, unitRadius);
}
// Debug.Log(openSet.Count + " " + Position(current) + " " + current.isRoad + " " + Vector3.Distance(Position(current), destination) + " " + (current.gScore + arcTimeDest) + " " + gScoreDest);
if (arcTimeDest >= Pathfinder.FOREVER)
{
continue;
}
if (arcTimeDest < Pathfinder.FOREVER && command == MoveCommandType.NORMAL)
{
arcTimeDest = 0f;
}
float gScoreDestNew = current.GScore + arcTimeDest;
if (gScoreDestNew < gScoreDest)
{
gScoreDest = gScoreDestNew;
cameFromDest = current;
}
}
// Reconstruct best path
PathNode node = cameFromDest;
while (node != null)
{
path.Add(node);
node = node.CameFrom;
}
return(gScoreDest);
}
void EikonalUpdateFormula(fastLocation l)
{
float phi_proposed = Mathf.Infinity;
int xInto, yInto;
// cycle through directions to check all neighbors and perform the eikonal
// update cycle on them
for (int d = 0; d < DIR_ENWS.Length; d++)
{
xInto = l.x + (int)DIR_ENWS[d].x;
yInto = l.y + (int)DIR_ENWS[d].y;
neighbor = new fastLocation(xInto, yInto);
if (isEikonalLocationValidAsNeighbor(neighbor))
{
// The point is valid. Now, we pull values from THIS location's
// 4 neighbors and use them in the calculation
int xIInto, yIInto;
float phi_mx, phi_my, C_mx, C_my;
Vector4 phi_m;
phi_m = Vector4.one * Mathf.Infinity;
// track cost of moving into each nearby space
for (int dd = 0; dd < DIR_ENWS.Length; dd++)
{
xIInto = neighbor.x + (int)DIR_ENWS[dd].x;
yIInto = neighbor.y + (int)DIR_ENWS[dd].y;
if (isEikonalLocationValidToMoveInto(new fastLocation(xIInto, yIInto)))
{
phi_m[dd] = Phi[xIInto, yIInto] + C[neighbor.x, neighbor.y][dd];
}
}
// select out cheapest
phi_mx = Math.Min(phi_m[0], phi_m[2]);
phi_my = Math.Min(phi_m[1], phi_m[3]);
// now assign C_mx based on which direction was chosen
if (phi_mx == phi_m[0])
{
C_mx = C[neighbor.x, neighbor.y][0];
}
else
{
C_mx = C[neighbor.x, neighbor.y][2];
}
// now assign C_mx based on which direction was chosen
if (phi_my == phi_m[1])
{
C_my = C[neighbor.x, neighbor.y][1];
}
else
{
C_my = C[neighbor.x, neighbor.y][3];
}
// solve for our proposed Phi[neighbor] value using the quadratic solution to the
// approximation of the eikonal equation
float C_mx_Sq = C_mx * C_mx;
float C_my_Sq = C_my * C_my;
float phi_mDiff_Sq = (phi_mx - phi_my) * (phi_mx - phi_my);
float valTest;
// valTest = C_mx_Sq + C_my_Sq - 1f/(C_mx_Sq*C_my_Sq);
valTest = C_mx_Sq + C_my_Sq - 1f;
// test the quadratic
if (phi_mDiff_Sq > valTest)
{
// use the simplified solution for phi_proposed
float phi_min = Math.Min(phi_mx, phi_my);
float cost_min;
if (phi_min == phi_mx)
{
cost_min = C_mx;
}
else
{
cost_min = C_my;
}
phi_proposed = cost_min + phi_min;
}
else
{
// solve the quadratic
float radical = (float)Math.Sqrt((double)(C_mx_Sq * C_my_Sq * (C_mx_Sq + C_my_Sq - phi_mDiff_Sq)));
float soln1 = (C_my_Sq * phi_mx + C_mx_Sq * phi_my + radical) / (C_mx_Sq + C_my_Sq);
float soln2 = (C_my_Sq * phi_mx + C_mx_Sq * phi_my - radical) / (C_mx_Sq + C_my_Sq);
phi_proposed = Math.Max(soln1, soln2);
}
// we now have a phi_proposed
// we are re-writing the phi-array real time, so we simply compare to the current slot
if (phi_proposed < Phi[neighbor.x, neighbor.y])
{
// save the value of the lower phi
Phi[neighbor.x, neighbor.y] = phi_proposed;
if (considered.Contains(neighbor))
{
// re-write the old value in the queue
considered.UpdatePriority(neighbor, phi_proposed);
}
else
{
// -OR- add this value to the queue
considered.Enqueue(neighbor, Phi[neighbor.x, neighbor.y]);
}
}
}
}
}
/// <summary>
/// A* forward search for a path that reaches the given goal.
/// </summary>
/// <returns>Returns null if a path could not be found, or a list of the
/// transitions that must be followed, in order.</returns>
public static Queue <ITransition> Search(
ISearchContext agent,
IState worldState,
IGoal goal,
bool reversePath = false)
{
// This is used to get the nodes from the state we're currently exploring.
var exploredNodes = new Dictionary <IState, Node>(worldState.GetComparer());
var closedSet = new HashSet <IState>(worldState.GetComparer());
var openSet = new FastPriorityQueue <Node>(MAX_FRINGE_NODES);
var currentNode = Node.Borrow(null, 0f, worldState, null);
openSet.Enqueue(currentNode, 0f);
IState nextState;
Node nextNode;
while (openSet.Count > 0)
{
// Examine the next node.
currentNode = openSet.Dequeue();
// Check if the node satisfies the goal.
if (currentNode.state.IsGoalState(goal, false))
{
DebugUtils.Log("Selected path with cost: " + currentNode.Score + " Visited nodes: " + (closedSet.Count + openSet.Count));
var plan = UnwrapPath(currentNode, reversePath);
// Return all nodes.
Node.ReturnAll();
// Monitor nodes pool size to see if there are problems in the search.
Node.ReportLeaks();
return(plan);
}
// Mark this node as closed - don't explore it again.
closedSet.Add(currentNode.state);
exploredNodes.Remove(currentNode.state);
// Go over all possible transitions.
var possibleTransitions = currentNode.state.GetPossibleTransitions(agent);
foreach (var transition in possibleTransitions)
{
var cost = transition.CalculateCost(currentNode.state);
// Apply the transition to get the next state.
nextState = transition.ApplyToState(currentNode.state);
//DebugUtils.LogError("Checking transition: " + currentNode.state + " -(" + transition + ")-> " + nextState);
//UnityEngine.Debug.Break();
// Now that we have the new state, we can check if it was
// already evaluated and if so, we ignore it. We can ignore it
// thanks to the priority queue - we always check the state whose
// path to the starting state is the cheapest and so all the next
// states can only add to this cost.
if (closedSet.Contains(nextState))
{
transition.ReturnSelf();
nextState.ReturnSelf();
continue;
}
float newRunningCost = currentNode.runningCost + cost;
// Check if we explored this node in the past.
if (!exploredNodes.TryGetValue(nextState, out nextNode))
{
// Found a new node.
nextNode = Node.Borrow(currentNode, newRunningCost, nextState, transition);
// Cache the node for later.
exploredNodes[nextState] = nextNode;
DebugUtils.Assert(!openSet.Contains(nextNode), "Open set contains new node... How can this be?");
// This is a new node.
nextNode.CalculateHeuristicCost(agent, goal);
openSet.Enqueue(nextNode, nextNode.Score);
//DebugUtils.LogError("New node cost: " + newRunningCost + " " + nextNode.PathToString());
}
else if (newRunningCost < nextNode.runningCost)
{
// Found the best path so far to this node.
nextNode.parent = currentNode;
nextNode.runningCost = newRunningCost;
nextNode.transition.ReturnSelf();
nextNode.transition = transition;
nextState.ReturnSelf();
//DebugUtils.LogError("!!! Best cost: " + newRunningCost + " " + nextNode.PathToString());
}
else
{
// Transition is not needed as it is known to be sub optimal.
transition.ReturnSelf();
nextState.ReturnSelf();
}
}
}
DebugUtils.Log("NO PLAN. Closed nodes: " + closedSet.Count);
Node.ReturnAll();
return(null);
}
// Run the A* algorithm and put the result in path
// If no path was found, return 'forever' and put only the destination in path
// Returns the total path time
public float FindPath(
List <PathNode> path,
Vector3 start, Vector3 destination,
MobilityType mobility, float unitRadius,
MoveCommandType command)
{
path.Clear();
path.Add(new PathNode(destination, false));
PathNode cameFromDest = null;
float gScoreDest = Pathfinder.FindLocalPath(this, start, destination, mobility, unitRadius);
if (gScoreDest < Pathfinder.Forever)
{
if (command == MoveCommandType.Slow || command == MoveCommandType.Reverse)
{
return(gScoreDest);
}
}
// Initialize with all nodes accessible from the starting point
// (this can be optimized later by throwing out some from the start)
openSet.Clear();
foreach (PathNode neighbor in graph)
{
neighbor.isClosed = false;
neighbor.cameFrom = null;
neighbor.gScore = Pathfinder.Forever;
Vector3 neighborPos = Position(neighbor);
if ((start - neighborPos).magnitude < ArcMaxDist)
{
float gScoreNew = Pathfinder.FindLocalPath(this, start, neighborPos, mobility, unitRadius);
if (gScoreNew < Pathfinder.Forever)
{
neighbor.gScore = gScoreNew;
float fScoreNew = gScoreNew + TimeHeuristic(neighborPos, destination, mobility);
openSet.Enqueue(neighbor, fScoreNew);
}
}
}
while (openSet.Count > 0)
{
PathNode current = openSet.Dequeue();
current.isClosed = true;
if (gScoreDest < current.Priority)
{
break;
}
foreach (PathArc arc in current.arcs)
{
PathNode neighbor = arc.node1 == current ? arc.node2 : arc.node1;
if (neighbor.isClosed)
{
continue;
}
float arcTime = arc.time[mobility.Index];
if (arcTime >= Pathfinder.Forever)
{
continue;
}
float gScoreNew = current.gScore + arcTime;
if (gScoreNew >= neighbor.gScore)
{
continue;
}
float fScoreNew = gScoreNew + TimeHeuristic(Position(neighbor), destination, mobility);
if (!openSet.Contains(neighbor))
{
openSet.Enqueue(neighbor, fScoreNew);
}
else
{
openSet.UpdatePriority(neighbor, fScoreNew);
}
neighbor.gScore = gScoreNew;
neighbor.cameFrom = current;
}
float arcTimeDest = Pathfinder.Forever;
if (Vector3.Distance(Position(current), destination) < ArcMaxDist)
{
arcTimeDest = Pathfinder.FindLocalPath(this, Position(current), destination, mobility, unitRadius);
}
// Debug.Log(openSet.Count + " " + Position(current) + " " + current.isRoad + " " + Vector3.Distance(Position(current), destination) + " " + (current.gScore + arcTimeDest) + " " + gScoreDest);
if (arcTimeDest >= Pathfinder.Forever)
{
continue;
}
if (arcTimeDest < Pathfinder.Forever && command == MoveCommandType.Slow)
{
arcTimeDest = 0f;
}
float gScoreDestNew = current.gScore + arcTimeDest;
if (gScoreDestNew < gScoreDest)
{
gScoreDest = gScoreDestNew;
cameFromDest = current;
}
}
// Reconstruct best path
PathNode node = cameFromDest;
while (node != null)
{
path.Add(node);
node = node.cameFrom;
}
return(gScoreDest);
}
//public int getCostBetweenPoints(Vector2Int pointOne, Vector2Int pointTwo)
//{
//}
private List <PathNode> AStar(MapTile[,] mapTiles, Vector2Int start, Vector2Int goal, float maxCost, bool pathMode, MapUnit c)//True: return path, False, return all possibilities
{
List <PathNode> nodesInRange = new List <PathNode>();
aStarNodeArray = new AStarNode[mapTiles.GetLength(0), mapTiles.GetLength(1)];
for (int x = 0; x < mapTiles.GetLength(0); x++)
{
for (int y = 0; y < mapTiles.GetLength(1); y++)
{
aStarNodeArray[x, y] = new AStarNode(new Vector2Int(x, y));
aStarNodeArray[x, y].gScore = float.PositiveInfinity;
if (pathMode)
{
aStarNodeArray[x, y].hScore = Distance(new Vector2Int(x, y), goal);
}
else
{
aStarNodeArray[x, y].hScore = 0;
}
}
}
aStarNodeArray[start.x, start.y].gScore = 0;
FastPriorityQueue <AStarNode> openSet = new FastPriorityQueue <AStarNode>(aStarNodeArray.GetLength(0) * aStarNodeArray.GetLength(1));
openSet.Enqueue(aStarNodeArray[start.x, start.y], aStarNodeArray[start.x, start.y].fScore);
//worldMap.SetDebugTile(start, Color.cyan);
//worldMap.SetDebugTile(start, Color.red);
if (pathMode && (!mapTiles[start.x, start.y].GetPassable(c) || !mapTiles[goal.x, goal.y].GetPassable(c)))
{
return(ReconstructPath(start, goal, aStarNodeArray, maxCost));
}
while (openSet.Count != 0)
{
AStarNode currentNode = openSet.Dequeue();
if (currentNode.gScore > maxCost && !pathMode)
{
return(nodesInRange);
}
if (pathMode && currentNode.pos == goal)
{
//worldMap.SetDebugTile(goal, Color.green);
return(ReconstructPath(start, goal, aStarNodeArray, maxCost));
}
currentNode.closed = true;
// worldMap.SetDebugTile(currentNode.pos, Color.blue);
if (!pathMode)
{
nodesInRange.Add(new PathNode(currentNode.pos, currentNode.gScore));
}
foreach (Vector2Int v in GetNeighbors(currentNode, mapTiles, c))
{
AStarNode currentNeighbor = aStarNodeArray[v.x, v.y];
if (currentNeighbor.closed)
{
continue;
}
if (!openSet.Contains(currentNeighbor))
{
openSet.Enqueue(currentNeighbor, currentNeighbor.fScore);
//worldMap.SetDebugTile(currentNeighbor.pos, Color.magenta);
currentNeighbor.prevPos = currentNode.pos;
}
float tentative_gScore = currentNode.gScore + Cost(mapTiles, currentNode.pos, currentNeighbor.pos, c);
if (tentative_gScore < currentNeighbor.gScore)
{
// This path to neighbor is better than any previous one. Record it!
currentNeighbor.prevPos = currentNode.pos;
currentNeighbor.gScore = tentative_gScore;
openSet.UpdatePriority(currentNeighbor, currentNeighbor.fScore);
}
}
}
if (pathMode)
{
return(ReconstructPath(start, goal, aStarNodeArray, maxCost));
}
else
{
return(nodesInRange);
}
}
public static List <Tile> GetPath(Vector2Int from, Vector2Int to)
{
finalPath.Clear();
int fromIndex = gridController.TilePosToIndex(from.x, from.y);
int toIndex = gridController.TilePosToIndex(to.x, to.y);
Tile initialTile = gridController.Tiles[fromIndex];
Tile destinationTile = gridController.Tiles[toIndex];
openListPriorityQueue.Enqueue(initialTile, 0);
Tile currentTile = null;
while (openListPriorityQueue.Count > 0)
{
currentTile = openListPriorityQueue.Dequeue();
closeDictionary.Add(currentTile.Index, currentTile);
if (Equals(currentTile, destinationTile))
{
break;
}
UpdateNeighbors(currentTile);
for (int i = neighbors.Length - 1; i >= 0; --i)
{
Tile neighbourPathTile = neighbors[i];
if (neighbourPathTile == null)
{
continue;
}
if (closeDictionary.ContainsKey(neighbourPathTile.Index))
{
continue;
}
bool isAtOpenList = openListPriorityQueue.Contains(neighbourPathTile);
float movementCostToNeighbour = currentTile.GCost + GetDistance(currentTile, neighbourPathTile);
if (movementCostToNeighbour < neighbourPathTile.GCost || !isAtOpenList)
{
neighbourPathTile.SetGCost(movementCostToNeighbour);
neighbourPathTile.SetHCost(GetDistance(neighbourPathTile, destinationTile));
neighbourPathTile.SetParent(currentTile);
if (!isAtOpenList)
{
openListPriorityQueue.Enqueue(neighbourPathTile,
neighbourPathTile.FCost);
}
else
{
openListPriorityQueue.UpdatePriority(neighbourPathTile, neighbourPathTile.FCost);
}
}
}
}
while (currentTile.Parent != null && !Equals(currentTile, initialTile))
{
finalPath.Add(currentTile);
currentTile = currentTile.Parent;
}
finalPath.Add(initialTile);
openListPriorityQueue.Clear();
closeDictionary.Clear();
return(finalPath);
}
public List <Vector2Int> search(MazeManager maze)
{
Vector2Int start = maze.getStart();
int openSetSize = maze.rows * maze.cols;
FastPriorityQueue <openSetElement> openSet = new FastPriorityQueue <openSetElement>(openSetSize);
//HashSet<Vector2Int> openSet = new HashSet<Vector2Int>();
HashSet <Vector2Int> closedSet = new HashSet <Vector2Int>();
List <Vector2Int> goals = maze.getGoals();
Vector2Int goal = goals[0];
if (maze.isObstacle(ref goal))
{
// Debug.Log("FAIL!");
return(new List <Vector2Int>());
}
Dictionary <Vector2Int, Vector2Int> predecessor = new Dictionary <Vector2Int, Vector2Int>();
Dictionary <Vector2Int, double> gScore = new Dictionary <Vector2Int, double>();
Dictionary <Vector2Int, double> fScore = new Dictionary <Vector2Int, double>();
gScore.Add(start, 0);
fScore.Add(start, heuristicDistance(start, goal));
openSetElement startInOpenSet = new openSetElement(start);
openSet.Enqueue(startInOpenSet, (float)fScore[start]);
Vector2Int curPoint = new Vector2Int();
double nei_gscore = 0.0;
while (openSet.Count > 0)
{
/*
* double minfScore = Math.Sqrt(Math.Pow(maze.rows, 2.0) + Math.Pow(maze.cols, 2.0));
* foreach(Vector2Int point in openSet)
* {
* double curfScore = (double)fScore[point];
* if (minfScore > curfScore)
* {
* curPoint = point;
* minfScore = curfScore;
* }
* }
*/
curPoint = openSet.Dequeue().pos_;
double minfScore = fScore[curPoint];
if (curPoint == goal)
{
List <Vector2Int> zig_zag_path = reconstructZigZagPath(predecessor, goal);
//Debug.Log("zig_zag_SUCCESS!!!");
List <Vector2Int> straight_path = reconstructStraightPath(ref maze, ref zig_zag_path);
//Debug.Log("straight_SUCCESS!!!");
return(straight_path);
}
closedSet.Add(curPoint);
// openSet.Remove(curPoint);
foreach (KeyValuePair <Vector2Int, bool> neighbor in getNeighbors(ref maze, curPoint))
{
if (closedSet.Contains(neighbor.Key))
{
continue;
}
if (neighbor.Value)
{
// Short Edges
nei_gscore = gScore[curPoint] + 1.0;
}
else
{
// Long Edges
nei_gscore = gScore[curPoint] + Math.Sqrt(2.0);
}
if (!gScore.ContainsKey(neighbor.Key) || (nei_gscore < gScore[neighbor.Key]))
{
predecessor[neighbor.Key] = curPoint;
gScore[neighbor.Key] = nei_gscore;
fScore[neighbor.Key] = nei_gscore + heuristicDistance(neighbor.Key, goal);
openSetElement neighborInOpenSet = new openSetElement(neighbor.Key);
if (!openSet.Contains(neighborInOpenSet))
{
openSet.Enqueue(neighborInOpenSet, (float)fScore[neighbor.Key]);
}
}
}
}
// Debug.Log("FAIL!!!");
return(new List <Vector2Int>());
}
public static Solution Solve(Planet planet)
{
var areas = planet.Areas;
var currentArea = areas[planet.Sugar];
currentArea.Duration = 0;
currentArea.DurationFromStart = 0;
var endArea = planet.Areas[planet.Astroants];
endArea.Duration = 0;
var areaQueue = new FastPriorityQueue <Area>(areas.Length);
while (true)
{
if (currentArea == endArea)
{
return(GetSolution(planet));
}
currentArea.Visited = true;
for (var i = 0; i < currentArea.Neighbors.Length; i++)
{
var direction = currentArea.Neighbors[i];
var neighbor = areas.GetNeighbor(currentArea.Coords, direction);
if (neighbor.Visited)
{
continue;
}
var durationFromStart = currentArea.DurationFromStart + neighbor.Duration;
if (durationFromStart >= neighbor.DurationFromStart)
{
continue;
}
neighbor.CameFrom = direction;
neighbor.DurationFromStart = durationFromStart;
if (areaQueue.Contains(neighbor))
{
areaQueue.UpdatePriority(neighbor, neighbor.TotalEstimatedDuration);
}
else
{
neighbor.SetEstimatedDurationTo(endArea.Coords);
areaQueue.Enqueue(neighbor, neighbor.TotalEstimatedDuration);
}
}
if (areaQueue.Count == 0)
{
return(null);
}
currentArea = areaQueue.Dequeue();
}
}
/// <summary>
/// The EikonalUpdateFormula computes the actual solution potential
/// </summary>
/// <param name="l"></param>
private void EikonalUpdateFormula(FastLocation l)
{
float phi_proposed = Mathf.Infinity;
// cycle through directions to check all neighbors and perform the eikonal
// update cycle on them
for (int d = 0; d < DIR_ENWS.Length; d++)
{
int xInto = l.x + (int)DIR_ENWS[d].x;
int yInto = l.y + (int)DIR_ENWS[d].y;
neighbor = new FastLocation(xInto, yInto);
if (isEikonalLocationValidAsNeighbor(neighbor))
{
// The point is valid. Now, we pull values from THIS location's
// 4 neighbors and use them in the calculation
Vector4 phi_m;
phi_m = Vector4.one * Mathf.Infinity;
// track cost of moving into each nearby space
for (int dd = 0; dd < DIR_ENWS.Length; dd++)
{
int xIInto = neighbor.x + (int)DIR_ENWS[dd].x;
int yIInto = neighbor.y + (int)DIR_ENWS[dd].y;
if (isEikonalLocationValidToMoveInto(new FastLocation(xIInto, yIInto)))
{
phi_m[dd] = Phi[xIInto, yIInto] + C[neighbor.x, neighbor.y][dd];
}
}
// select out cheapest
float phi_mx = Math.Min(phi_m[0], phi_m[2]);
float phi_my = Math.Min(phi_m[1], phi_m[3]);
// now assign C_mx based on which direction was chosen
float C_mx = phi_mx == phi_m[0] ? C[neighbor.x, neighbor.y][0] : C[neighbor.x, neighbor.y][2];
// now assign C_my based on which direction was chosen
float C_my = phi_my == phi_m[1] ? C[neighbor.x, neighbor.y][1] : C[neighbor.x, neighbor.y][3];
// solve for our proposed Phi[neighbor] value using the quadratic solution to the
// approximation of the eikonal equation
float C_mx_Sq = C_mx * C_mx;
float C_my_Sq = C_my * C_my;
float phi_mDiff_Sq = (phi_mx - phi_my) * (phi_mx - phi_my);
float valTest = C_mx_Sq + C_my_Sq - 1f / (C_mx_Sq * C_my_Sq);
//float valTest = C_mx_Sq + C_my_Sq - 1f;
// test the quadratic
if (phi_mDiff_Sq > valTest)
{
// use the simplified solution for phi_proposed
float phi_min = Math.Min(phi_mx, phi_my);
float cost_min = phi_min == phi_mx ? C_mx : C_my;
phi_proposed = cost_min + phi_min;
}
else
{
// solve the quadratic
var radical = Math.Sqrt(C_mx_Sq * C_my_Sq * (C_mx_Sq + C_my_Sq - phi_mDiff_Sq));
var soln1 = (C_my_Sq * phi_mx + C_mx_Sq * phi_my + radical) / (C_mx_Sq + C_my_Sq);
var soln2 = (C_my_Sq * phi_mx + C_mx_Sq * phi_my - radical) / (C_mx_Sq + C_my_Sq);
// max - prefers diagonals
//phi_proposed = (float)Math.Max(soln1, soln2);
// min - prefers cardinals
//phi_proposed = (float)Math.Min(soln1, soln2);
// mean - better mix but still prefer diagonals
//phi_proposed = (float)(soln1 + soln2) / 2;
// geometric mean - seems identical to mean
//phi_proposed = (float)Math.Sqrt(soln1 * soln2);
// weighted mean - appears to offer the best compromise
var max = (float)Math.Max(soln1, soln2);
var min = (float)Math.Min(soln1, soln2);
float wMax = CCValues.S.maxWeight;
float wMin = CCValues.S.minWeight;
phi_proposed = (float)(max * wMax + min * wMin) / (wMax + wMin);
}
// we now have a phi_proposed
// we are re-writing the phi-array real time, so we simply compare to the current slot
if (phi_proposed < Phi[neighbor.x, neighbor.y])
{
// save the value of the lower phi
Phi[neighbor.x, neighbor.y] = phi_proposed;
if (considered.Contains(neighbor))
{
// re-write the old value in the queue
considered.UpdatePriority(neighbor, phi_proposed);
}
else
{
// -OR- add this value to the queue
considered.Enqueue(neighbor, Phi[neighbor.x, neighbor.y]);
}
}
}
}
}
private Point Move(Person person, Point pos)
{
// Start by adding p to visited
var visited = new HashSet <Point> {
pos
};
var nodes = new FastPriorityQueue <VisitedNode>(64);
var adjacentNodes = GetAdjacentNodes(pos);
foreach (var node in adjacentNodes)
{
node.InitialVel = new Point(node.Pos.X - pos.X, node.Pos.Y - pos.Y);
nodes.Enqueue(node, node.GetPriority());
visited.Add(node.Pos);
}
while (nodes.Any())
{
var nextNodes = new FastPriorityQueue <VisitedNode>(64);
while (nodes.Any())
{
var node = nodes.Dequeue();
visited.Add(node.Pos);
if (this._people.TryGetValue(node.Pos, out var enemy) && enemy.GetType() != person.GetType())
{
// Enemy in this pos - stop searching
var newPos = new Point(pos.X, pos.Y);
newPos.Offset(node.InitialVel);
return(newPos);
}
// Non-empty square - cannot move here, do nothing
if (this._people.ContainsKey(node.Pos) || !this._emptySquares.Contains(node.Pos))
{
continue;
}
// This square is empty - add all adjacent non-visited nodes
adjacentNodes = GetAdjacentNodes(node.Pos);
foreach (var adjNode in adjacentNodes)
{
if (!visited.Contains(adjNode.Pos))
{
adjNode.InitialVel = node.InitialVel;
visited.Add(adjNode.Pos);
nextNodes.Enqueue(adjNode, adjNode.GetPriority());
}
else if (nextNodes.Contains(adjNode))
{
var queueNode = nextNodes.Single(nds => nds.Equals(adjNode));
if (adjNode.InitialVel.Y < queueNode.InitialVel.Y)
{
queueNode.InitialVel = adjNode.InitialVel;
}
else if (adjNode.InitialVel.Y == queueNode.InitialVel.Y &&
adjNode.InitialVel.X < queueNode.InitialVel.X)
{
queueNode.InitialVel = adjNode.InitialVel;
}
}
}
}
nodes = nextNodes;
}
return(pos);
}
