/// <summary>
///
/// </summary>
/// <param name="aStarPath"></param>
/// <param name="nodeToProcess"> Poslední bod aka první od kterého začínáme. </param>
/// <param name="firstNode"></param>
/// <param name="finalPath"></param>
/// <returns></returns>
private List <Node> RefactorPath(Vector3 nodeToProcess, Vector3 firstNode, List <AstarPoint> aStarPath, List <Node> finalPath = null)
{
// Rekurzivně, rekurzivně složíme cestu :)
if (firstNode == null || nodeToProcess == null || aStarPath == null)
{
return(null);
}
/* if (nodeToProcess == firstNode)
* return finalPath;*/
// První kolo
if (finalPath == null)
{
finalPath = new List <Node>();
}
AstarPoint aStar = aStarPath.Find(o => o.pointPosition == nodeToProcess);
if (aStar != null)
{
aStarPath.Remove(aStar);
// Přidáme nový bod
finalPath.Add(nodes_data.Find(o => o.position == aStar.pointPosition));
return(RefactorPath(aStar.prewPointPosition, firstNode, aStarPath, finalPath));
}
else
{
return(finalPath);
}
}
public List <AstarPoint> GenerateAstarPath(Node node, Vector3 finalNodePos, List <AstarPoint> d_table = null, List <Vector3> visited_v = null, float closest_dist = float.PositiveInfinity)
{ // TODO: GC ALLOC!!!- MOC (cca. 24 KB)
if (node == null)
{
return(null);
}
if (node.position == finalNodePos)//A* modifikace
{
return(d_table);
}
// Navštívené body
if (visited_v == null)
{
visited_v = new List <Vector3>();
}
//Dijkskrova tabulka pro daný bod: BOD; vzdálenost; předchozí BOD
if (d_table == null)
{
d_table = new List <AstarPoint>(); //new Dictionary<Vector3, KeyValuePair<float, Vector3>>();
foreach (Node node_ in nodes_data)
{
d_table.Add(new AstarPoint()
{
pointPosition = node_.position,
weight = Vector3.Distance(node_.position, finalNodePos),
distance = (node_.Equals(node.position) ? 0 : float.PositiveInfinity),
prewPointPosition = new Vector3()
});
}
}
// Momentálně se nacházíme na bodě tzn. navštívili jsme ho tzn. přidáme do visited
visited_v.Add(node.position);
List <AstarPoint> unvisited_neighbour_nodes = new List <AstarPoint>();
foreach (KeyValuePair <Vector3, float> neighbour_to_node in node.neighbours)
{
if (!visited_v.Contains(neighbour_to_node.Key))
{
AstarPoint tablePointData = d_table.FirstOrDefault(o => o.pointPosition == neighbour_to_node.Key);
// Přidáme do pole, pro určení následujícího bodu
unvisited_neighbour_nodes.Add(tablePointData);
/*if (d_table.ContainsKey(neighbour_to_node.Key))// Pouze ověření, protože vždy bude obsahovat viz foreach nahoře
* {*/
if (float.IsInfinity(closest_dist)) // Pokud je vzdálenost nekonečná
{
closest_dist = 0 + neighbour_to_node.Value;
}
else
{
closest_dist = closest_dist + neighbour_to_node.Value;
}
// Pokud je nová vzdálenost (menší)
if ((tablePointData.distance > closest_dist) /*|| float.IsInfinity(tablePointData.distance)*/)
{
float weight = Vector3.Distance(neighbour_to_node.Key, finalNodePos) + closest_dist;
tablePointData.weight = weight;
tablePointData.distance = closest_dist;
tablePointData.prewPointPosition = node.position;
}
/* }
* else { Debug.LogError("WTF ?????"); }*/
}
}
// Nalezneme nejbližší uzel
Node closest_node = null;
// Získáme nejbližší uzel, pro další návštěvu
if (unvisited_neighbour_nodes.Count != 0)
{
var closest_node_data = unvisited_neighbour_nodes.OrderBy(kvp => kvp.weight).First();
closest_node = nodes_data.Find(o => o.position == closest_node_data.pointPosition);
}
else
{
return(d_table);
}
if (visited_v.Count != (nodes_data.Count - 1))
{
return(GenerateAstarPath(closest_node, finalNodePos, d_table, visited_v, closest_dist));
}
else
{
return(d_table);
}
}
