public static bool Search(GraphNode source, GraphNode destination, ref List <GraphNode> path, int maxSteps)
{
bool found = false;
path = new List <GraphNode>();
//A* algorithm
SimplePriorityQueue <GraphNode> nodes = new SimplePriorityQueue <GraphNode>();
source.Cost = 0;
source.Heuristic = Vector3.Distance(source.transform.position, destination.transform.position);
nodes.Enqueue(source, source.Cost + source.Heuristic);
int steps = 0;
while (!found && nodes.Count > 0 && steps++ < maxSteps)
{
GraphNode node = nodes.Dequeue();
if (node == destination)
{
found = true;
continue;
}
foreach (GraphNode.Edge edge in node.Edges)
{
float cost = node.Cost + Vector3.Distance(edge.nodeA.transform.position, edge.nodeB.transform.position);
if (cost < edge.nodeB.Cost)
{
edge.nodeB.Cost = cost;
edge.nodeB.Parent = node;
edge.nodeB.Heuristic = Vector3.Distance(edge.nodeB.transform.position, destination.transform.position);
nodes.EnqueueWithoutDuplicates(edge.nodeB, edge.nodeB.Cost + edge.nodeB.Heuristic);
}
}
}
if (found)
{
GraphNode node = destination;
while (node != null)
{
path.Add(node);
node = node.Parent;
}
path.Reverse();
}
else
{
while (nodes.Count > 0)
{
path.Add(nodes.Dequeue());
}
}
return(found);
}
/// <summary>
/// Update the distance from the source to the concerned vertex. </summary>
/// <param name="vertex"> </param>
private void _improve_to_vertex(BaseVertex vertex, bool is_source2sink)
{
// 1. get the neighboring vertices
HashSet <BaseVertex> neighbor_vertex_list = is_source2sink ? _graph.get_adjacent_vertices(vertex) : _graph.get_precedent_vertices(vertex);
// 2. update the distance passing on current vertex
foreach (BaseVertex cur_adjacent_vertex in neighbor_vertex_list)
{
// 2.1 skip if visited before
if (_determined_vertex_set.Contains(cur_adjacent_vertex))
{
continue;
}
// 2.2 calculate the new distance
double distance = _start_vertex_distance_index.ContainsKey(vertex) ? _start_vertex_distance_index[vertex] : Graph.DISCONNECTED;
distance += is_source2sink ? _graph.get_edge_weight(vertex, cur_adjacent_vertex) : _graph.get_edge_weight(cur_adjacent_vertex, vertex);
// 2.3 update the distance if necessary
if (!_start_vertex_distance_index.ContainsKey(cur_adjacent_vertex) || _start_vertex_distance_index[cur_adjacent_vertex] > distance)
{
_start_vertex_distance_index[cur_adjacent_vertex] = distance;
_predecessor_index[cur_adjacent_vertex] = vertex;
cur_adjacent_vertex.set_weight(distance);
_vertex_candidate_queue.EnqueueWithoutDuplicates(cur_adjacent_vertex, (float)distance);// Lớp PriorytyQueue quan trong can thuc hien đổi lại
}
}
}
public static List <Point> AStar(Point start, Point goal, Func <Point, List <Point> > neighbours)
{
SimplePriorityQueue <Point, double> open = new SimplePriorityQueue <Point, double>();
Dictionary <Point, Point> parent = new Dictionary <Point, Point>();
Dictionary <Point, double> cost = new Dictionary <Point, double>();
open.EnqueueWithoutDuplicates(start, 0);
parent[start] = null;
cost[start] = 0;
while (open.Count > 0)
{
Point current = open.Dequeue();
if (current == goal)
{
break;
}
foreach (var next in neighbours(current))
{
double newcost = cost[current] + 1;
if (!cost.ContainsKey(next) || newcost < cost[next])
{
cost[next] = newcost;
open.Enqueue(next, Heuristic(next, goal) + newcost);
parent[next] = current;
}
}
}
List <Point> result = new List <Point>();
Point end = goal;
while (true)
{
result.Add(end);
if (parent[end] != null)
{
end = parent[end];
}
else
{
break;
}
}
result.Reverse();
return(result);
}
public static bool Search(GraphNode source, GraphNode destination, ref List <GraphNode> path, int maxSteps)
{
bool found = false;
//A* algorithm
SimplePriorityQueue <GraphNode> nodes = new SimplePriorityQueue <GraphNode>();
source.Cost = 0;
source.Heuristic = Vector3.Distance(source.transform.position, destination.transform.position);
nodes.Enqueue(source, (source.Cost + source.Heuristic));
// set the current number of steps
int steps = 0;
while (!found && nodes.Count > 0 && steps++ < maxSteps)
{
// <dequeue node>
GraphNode node = nodes.Dequeue();
if (node == destination) //(< check if node is the destination node >)
{
// <set found to true>
found = true;
// <continue, do not execute the rest of this loop>
continue;
}
foreach (GraphNode.Edge edge in node.Edges)
{
// calculate cost to nodeB = node cost + edge distance (nodeA to nodeB)
float cost = node.Cost + Vector3.Distance(edge.nodeA.transform.position, edge.nodeB.transform.position); //< Vector3.Distance nodeA <->nodeB >;
// if cost < nodeB cost, add to priority queue
if (cost < edge.nodeB.Cost) //(< cost is less than nodeB.cost >))
{
// <set nodeB cost to cost>
edge.nodeB.Cost = cost;
// <set nodeB parent to node>
edge.nodeB.Parent = node;
// calculate heuristic = Vector3.Distance (nodeB <-> destination)
edge.nodeB.Heuristic = Vector3.Distance(edge.nodeB.transform.position, destination.transform.position); //< Vector3.Distance(nodeB <->destination) >
// <enqueue without duplicates nodeB with nodeB cost + nodeB heuristics as priority>
nodes.EnqueueWithoutDuplicates(edge.nodeB, (edge.nodeB.Cost + edge.nodeB.Heuristic));
}
}
}
// create a list of graph nodes (path)
path = new List <GraphNode>();
// if found is true
if (found)
{
GraphNode node = destination; //< set node to destination >
// while node not null
while (node != null)
{
// <add node to path list>
path.Add(node);
// <set node to node.Parent>
node = node.Parent;
}
// reverse path
path.Reverse();
}
else
{
// add all nodes to path
path = nodes.ToList();
while (nodes.Count > 0)
{
// <add (dequeued node) to path>
GraphNode node = nodes.Dequeue();
path.Add(node);
}
}
return(found);
}
public static bool Search(GraphNode source, GraphNode destination, ref List <GraphNode> path, int maxSteps)
{
// set found bool flag and the current number of steps
bool found = false;
// A* algroritm
SimplePriorityQueue <GraphNode> nodes = new SimplePriorityQueue <GraphNode>();
source.Cost = 0;
source.Heuristic = Vector3.Distance(source.transform.position, destination.transform.position);
nodes.Enqueue(source, source.Heuristic + source.Cost);
// set the current number of steps
int steps = 0;
while (!found && nodes.Count > 0 && steps++ < maxSteps)
{
// <dequeue node>
GraphNode node = nodes.Dequeue();
if (node == destination)
{
// <set found to true>
found = true;
// continue, do not execute the rest of this loop
continue;
}
// search node edges for unvisited node
foreach (GraphNode.Edge edge in node.Edges)
{
// calculate cost to nodeB = node cost + edge distance (nodeA to nodeB)
float cost = node.Cost + Vector3.Distance(edge.nodeA.transform.position, edge.nodeB.transform.position);
// if cost < nodeB cost, add to priority queue
if (cost < edge.nodeB.Cost)
{
// <set nodeB cost to cost>
edge.nodeB.Cost = cost;
// <set nodeB parent to node>
edge.nodeB.Parent = node;
// <enqueue without duplicates nodeB with cost as priority>
edge.nodeB.Heuristic = Vector3.Distance(edge.nodeB.transform.position, destination.transform.position);
nodes.EnqueueWithoutDuplicates(edge.nodeB, edge.nodeB.Cost + edge.nodeB.Heuristic);
}
}
}
// create a list of graph nodes (path)
path = new List <GraphNode>();
// if found is true
if (found)
{
GraphNode node = destination;
// while node not null
while (node != null)
{
// <add node to path list>
path.Add(node);
// <set node to node.Parent>
node = node.Parent;
}
// reverse path
path.Reverse();
}
else
{
GraphNode node = nodes.Dequeue();
path.Add(node);
}
return(found);
}
public static Stack <string> doSearch(Vector3 a, Vector3 t, float yAngle)
{
bool[,] visited = new bool[131, 101];
State[,] parent = new State[131, 101];
State S = new State();
S.setTranslatedValues(a.x, a.z, "", yAngle, 0, 0);
State T = new State();
T.setTranslatedValues(t.x, t.z, "", 0, 0, 0);
print(S.X() + " , " + S.Z());
print(T.X() + " , " + T.Z());
if (obstacle(T.X(), T.Z()))
{
print("Target is obstructed");
return(new Stack <string>());
}
//State nullstate = new State (-1, -1, "", -1, 0, 0);
//movement : 1.0 L, 2.0 - R, 3.0 - G
bool goalReached = false;
//make a pq
SimplePriorityQueue <State> pq = new SimplePriorityQueue <State>();
pq.Enqueue(S, 0);
State current = S;
while (pq.Count > 0)
{
//print ("current: " + current.X () + " , " + current.Z ());
current = pq.Dequeue();
if (current.X() == T.X() && current.Z() == T.Z())
{
print("goal reached " + current.X() + "," + current.Z());
goalReached = true;
break;
}
else
{
visited [current.X(), current.Z()] = true;
List <State> successors = getSuccessors(current);
foreach (State child in successors)
{
State par = parent[child.X(), child.Z()];
if (!visited[child.X(), child.Z()] && par == null)
{
//add it to queue if not visited;
child.setCost(child.getCost() + current.getCost());
child.setHC(heuristic(child, T) + child.getCost());
pq.Enqueue(child, child.getHC());
parent[child.X(), child.Z()] = current;
}
else
{
//if already in queue; if lower cost add the node again with new parent
//this node would be poped from pq first and marked as visited so duplicate would not be a problem
if (!visited[child.X(), child.Z()])
{
float oldcost = -99.0f;
if (par != null)
{
if (par.X() != child.X() && par.Z() != child.Z())
{
oldcost = 1.4f;
}
else
{
oldcost = 1.0f;
}
oldcost += par.getCost();
}
if (oldcost > -99 && current.getCost() + child.getCost() < oldcost)
{
child.setCost(child.getCost() + current.getCost());
child.setHC(heuristic(child, T) + child.getCost());
parent[child.X(), child.Z()] = current;
if (!pq.EnqueueWithoutDuplicates(child, child.getHC()))
{
pq.UpdatePriority(child, child.getHC());
}
}
}
}
}
}
}
//List<string> path = new List<string> ();
Stack <string> path = new Stack <string>();
path.Push(current.Dir());
if (goalReached)
{
//traverse in reverse to reach start state
while (!matchState(current, S))
{
State par = parent [current.X(), current.Z()];
if (par == null)
{
throw new UnityException("NULL parent found");
}
path.Push(par.Dir());
current = par;
}
}
return(path);
}
public double EagerDijkstraShortPath(List <KeyValuePair <int, double> >[] graphAdjList,
int startNode, int endNode, out List <int> path)
{
int numOfVertices = graphAdjList.Length;
if (endNode < 0 || endNode >= numOfVertices)
{
throw new ArgumentOutOfRangeException("Invalid node index");
}
if (startNode < 0 || startNode >= numOfVertices)
{
throw new ArgumentOutOfRangeException("Invalid node index");
}
bool[] visited = new bool[numOfVertices];
double[] distance = new double[numOfVertices];
List <int> resultPath = new List <int>();//path you visit
int[] prevNode = new int[numOfVertices];
for (int i = 0; i < numOfVertices; i++)
{
distance[i] = double.PositiveInfinity;
}
distance[startNode] = 0;
SimplePriorityQueue <int, double> priorityQueue =
new SimplePriorityQueue <int, double>();
priorityQueue.Enqueue(startNode, 0.0);
while (priorityQueue.Count != 0)
{
int indexofNode = priorityQueue.Dequeue();
visited[indexofNode] = true;
foreach (var keyValuePair in graphAdjList[indexofNode])
{
double minValue = keyValuePair.Value;
if (distance[keyValuePair.Key] < minValue)
{
continue;
}
if (visited[keyValuePair.Key])
{
continue;
}
double distNew = distance[indexofNode] + keyValuePair.Value;
if (distNew < distance[keyValuePair.Key])
{
prevNode[keyValuePair.Key] = indexofNode;
distance[keyValuePair.Key] = distNew;
if (!priorityQueue.EnqueueWithoutDuplicates(keyValuePair.Key, distNew))
{
priorityQueue.UpdatePriority(keyValuePair.Key, distNew);
}
}
}
if (indexofNode == endNode)
{
for (int at = endNode; at != startNode; at = prevNode[at])
{
resultPath.Add(at);
}
if (!resultPath.Contains(startNode))
{
resultPath.Add(startNode);
}
if (!resultPath.Contains(endNode))
{
resultPath.Add(endNode);
}
resultPath.Reverse();
path = resultPath;
return(distance[endNode]);
}
}
path = resultPath;
return(double.PositiveInfinity);
}
public static bool Search(GraphNode source, GraphNode destination, ref List <GraphNode> path, int maxSteps)
{
bool found = false;
// create priority queue
SimplePriorityQueue <GraphNode> nodes = new SimplePriorityQueue <GraphNode>();
// set source cost to 0
source.Cost = 0;
// enqueue source node with the source cost as the priority
nodes.Enqueue(source, source.Cost);
// set the current number of steps
int steps = 0;
while (!found && nodes.Count > 0 && steps++ < maxSteps)
{
// dequeue node
GraphNode node = nodes.Dequeue();
// check if node is the destination node
if (node == destination)
{
// set found to true
found = true;
// continue, do not execute the rest of this loop
continue;
}
// search node edges for unvisited node
foreach (GraphNode.Edge edge in node.Edges)
{
// calculate cost to nodeB = node cost + edge distance (nodeA to nodeB)
float cost = node.Cost + Vector3.Distance(edge.nodeA.transform.position, edge.nodeB.transform.position);
// if cost < nodeB cost, add to priority queue
if (cost < edge.nodeB.Cost)
{
// set nodeB cost to cost
edge.nodeB.Cost = cost;
// set nodeB parent to node
edge.nodeB.Parent = node;
// enqueue without duplicates nodeB with cost as priority
nodes.EnqueueWithoutDuplicates(edge.nodeB, edge.nodeB.Cost);
}
}
}
// create a list of graph nodes (path)
path = new List <GraphNode>();
// if found is true
if (found)
{
// set node to destination
GraphNode node = destination;
// while node not null
while (node != null)
{
// add node to list path
path.Add(node);
// set node to node parent
node = node.Parent;
}
// reverse path
path.Reverse();
}
else
{
while (nodes.Count > 0)
{
path.Add(nodes.Dequeue());
}
}
return(found);
}
//the algorithm eliminates less likely patterns from the graph
//only searches on heuristicList and returns the path
public double AStarShortestPathAndDistance(List <KeyValuePair <int, double> >[] graphAdjList,
int startNode, int endNode, out List <int> path)
{
int numOfVertices = graphAdjList.Length;
//this must be precalculated in dynamic systems
//a list which holds shortest distance from start to end , for every node
List <double> heuristicList = ShortestDistanceListHeuristic(graphAdjList, endNode);
if (endNode < 0 || endNode >= numOfVertices)
{
throw new ArgumentOutOfRangeException("Invalid node index");
}
if (startNode < 0 || startNode >= numOfVertices)
{
throw new ArgumentOutOfRangeException("Invalid node index");
}
bool[] visited = new bool[numOfVertices];
double[] distance = new double[numOfVertices];
//a similiar list like distance, which has double.PositiveInfinity value by default
double[] heuristicNewDistance = new double[numOfVertices];
List <int> resultPath = new List <int>();//path you visit
int[] prevNode = new int[numOfVertices];
for (int i = 0; i < numOfVertices; i++)
{
distance[i] = double.PositiveInfinity;
heuristicNewDistance[i] = double.PositiveInfinity;
}
distance[startNode] = 0;
SimplePriorityQueue <int, double> priorityQueue =
new SimplePriorityQueue <int, double>();
priorityQueue.Enqueue(startNode, heuristicList[startNode]);
while (priorityQueue.Count != 0)
{
int indexofNode = priorityQueue.Dequeue();
visited[indexofNode] = true;
foreach (var keyValuePair in graphAdjList[indexofNode])
{
double heuristicMinValue = heuristicList[keyValuePair.Key];
if (heuristicNewDistance[keyValuePair.Key] < heuristicMinValue)
{
continue;
}
if (visited[keyValuePair.Key])
{
continue;
}
double distNew = distance[indexofNode] + keyValuePair.Value;
double heurDistNew = distNew + heuristicList[keyValuePair.Key];
if (heurDistNew < heuristicNewDistance[keyValuePair.Key])
{
prevNode[keyValuePair.Key] = indexofNode;
distance[keyValuePair.Key] = distNew;
heuristicNewDistance[keyValuePair.Key] = heurDistNew;
//Priority Queue for heristicList values
if (!priorityQueue.EnqueueWithoutDuplicates(keyValuePair.Key, heurDistNew))
{
priorityQueue.UpdatePriority(keyValuePair.Key, heurDistNew);
}
}
}
if (indexofNode == endNode)
{
for (int at = endNode; at != startNode; at = prevNode[at])
{
resultPath.Add(at);
}
if (!resultPath.Contains(startNode))
{
resultPath.Add(startNode);
}
if (!resultPath.Contains(endNode))
{
resultPath.Add(endNode);
}
resultPath.Reverse();
path = resultPath;
return(distance[endNode]);
}
}
path = resultPath;
return(double.PositiveInfinity);
}
public IPath FindBestPath(AStarNode startingNode, AStarNode endingNode, TimeSpan departureTime)
{
var paths = new SimplePriorityQueue <IPath>();
var visitedNodes = new HashSet <AStarNode>();
var nodesToVisit = new SimplePriorityQueue <AStarNode>();
var temporaryArrivalTimes = new Dictionary <INode, TimeSpan>();
nodesToVisit.Enqueue(startingNode, 0);
temporaryArrivalTimes[startingNode] = departureTime;
startingNode.Weight = 0;
startingNode.HeuristicWeight = 0;
while (nodesToVisit.Count > 0)
{
var currentNode = nodesToVisit.Dequeue();
if (currentNode == endingNode)
{
break;
}
foreach (var transit in currentNode.Transits)
{
var endNode = (AStarNode)transit.EndNode;
var pathWithStatus = FindDirectPath(transit, endingNode, temporaryArrivalTimes[transit.StartNode]);
if (pathWithStatus.Item1)
{
paths.Enqueue(pathWithStatus.Item2, (float)pathWithStatus.Item2.Value);
}
var potentialWeight = _weightCalculator.CalculateWeight(transit, temporaryArrivalTimes[currentNode]) + currentNode.Weight;
var potentialHeuristic = _heuristicCalculator.CalculateWeight(transit, temporaryArrivalTimes[currentNode]);
var weightUpdated = false;
if (potentialWeight < endNode.Weight)
{
weightUpdated = true;
endNode.Weight = potentialWeight;
endNode.HeuristicWeight = potentialHeuristic;
endNode.FastestTransit = transit;
var intermediateDepartureTime = transit.Transport.GetClosestDepartureTime(currentNode.Location, temporaryArrivalTimes[currentNode]);
temporaryArrivalTimes[endNode] = intermediateDepartureTime + transit.Transport.TravelTime(transit.StartNode.Location, transit.EndNode.Location, intermediateDepartureTime);
transit.DepartureTime = intermediateDepartureTime;
transit.ArrivalTime = temporaryArrivalTimes[endNode];
}
if (!visitedNodes.Contains(endNode))
{
var isInVisitList = nodesToVisit.Contains(endNode);
if (!isInVisitList)
{
nodesToVisit.EnqueueWithoutDuplicates(endNode, (float)endNode.TotalWeight);
}
else if (isInVisitList && weightUpdated)
{
nodesToVisit.UpdatePriority(endNode, (float)endNode.TotalWeight);
}
}
}
visitedNodes.Add(currentNode);
}
var path = BacktrackPath(endingNode);
paths.Enqueue(path, (float)path.Value);
return(paths.Dequeue());
}
