public static List <Vector3> FindPathAstar(
Vector3 source,
Vector3 destination,
float search_boundary = 20f,
float grid_granularity = 1f,
float agent_size = 1f,
float near_stopping_distance = 3f)
{
var closed_set = new HashSet <FastVector3>();
//FastPriorityQueue<FastVector3> frontier_set = new FastPriorityQueue<FastVector3>(MAX_VECTORS_IN_QUEUE);
var frontier_set = new SimplePriorityQueue <FastVector3>();
var fast_source = new FastVector3(source);
var predecessor = new Dictionary <FastVector3, FastVector3>();
var g_scores = new Dictionary <FastVector3, float> {
{ fast_source, 0 }
};
frontier_set.Enqueue(
fast_source,
Heuristic(source, destination)); // Priority is distance, lowest distance highest priority
while (frontier_set.Count > 0)
{
var current_point = frontier_set.Dequeue();
closed_set.Add(current_point);
//Stopping condition and reconstruct path
var distance_to_destination = Heuristic(current_point.V, destination);
if (distance_to_destination < near_stopping_distance)
{
var current_trace_back_point = current_point;
var path = new List <Vector3> {
current_trace_back_point.V
};
while (predecessor.ContainsKey(current_trace_back_point))
{
current_trace_back_point = predecessor[current_trace_back_point];
path.Add(current_trace_back_point.V);
}
path.Reverse();
return(path);
}
//Get neighboring points
var neighbours = GetUnobstructedNeighbouringNodes(
current_point.V,
search_boundary,
grid_granularity,
agent_size);
//Calculate scores and add to frontier
foreach (var neighbour in neighbours)
{
/*foreach(FastVector3 candidate_point in frontier_set) { For FastPriorityQueue implementation this is check necessary
* if(neighbour == candidate_point) {
* neighbour.QueueIndex = candidate_point.QueueIndex;
* }
* }*/
if (closed_set.Contains(neighbour))
{
continue; // Skip if neighbour is already in closed set'
}
var temp_g_score = g_scores[current_point] + Heuristic(current_point.V, neighbour.V);
if (frontier_set.Contains(neighbour))
{
if (temp_g_score > g_scores[neighbour])
{
continue; // Skip if neighbour g_score is already lower
}
g_scores[neighbour] = temp_g_score;
}
else
{
g_scores.Add(neighbour, temp_g_score);
}
var f_score = g_scores[neighbour] + Heuristic(neighbour.V, destination);
if (frontier_set.Contains(neighbour))
{
frontier_set.UpdatePriority(neighbour, f_score);
predecessor[neighbour] = current_point;
}
else
{
/*if (frontier_set.Count > MAX_VECTORS_IN_QUEUE-1) { For FastPriorityQueue implementation this is check necessary
* return null;
* }*/
frontier_set.Enqueue(neighbour, f_score);
predecessor.Add(neighbour, current_point);
}
}
}
return(null);
}
public bool Equals(FastVector3 obj)
{
return(this.V == obj.V);
}
