public AStarNodeRecord(AStarNodeRecord record)
{
this.node = record.node;
this.connection = record.connection;
this.costSoFar = record.costSoFar;
this.estimatedTotalCost = record.estimatedTotalCost;
}
AStarNodeRecord SmallestElement(List <AStarNodeRecord> list)
{
AStarNodeRecord result = new AStarNodeRecord();
result = list[0];
for (int i = 0; i < list.Count; i++)
{
if (list[i].estimatedTotalCost < result.estimatedTotalCost)
{
result = list[i];
}
}
return(result);
}
List <Vector2> AStar(Vector2 start, Vector2 end)
{
//Criar nova heuristica
Heuristic heuristic = new Heuristic(end);
List <AStarNodeRecord> open = new List <AStarNodeRecord>();
List <AStarNodeRecord> closed = new List <AStarNodeRecord>();
AStarNodeRecord startRecord = new AStarNodeRecord();
startRecord.node = start;
startRecord.connection = null;
startRecord.costSoFar = 0f;
startRecord.estimatedTotalCost = heuristic.GetEstimatedCost(start);
open.Add(startRecord);
AStarNodeRecord current = null;
while (open.Count > 0)
{
//procurar no com menor custo
current = open.OrderBy(arg => arg.estimatedTotalCost).First();
//chegamos ao fim?
if (current.node == end)
{
break;
}
//obter conexões
List <Connection> connections = getConnections(current.node);
//foreach connection
foreach (var connection in connections)
{
Vector2 endNode = connection.to;
//calcular custo acumulado desta ligação
float endNodeCost = current.costSoFar + connection.cost;
//ja processamos este nodo?
AStarNodeRecord endNodeRecord = null;
endNodeRecord = closed.Find(x => x.node == endNode);
if (endNodeRecord != null)
{
if (endNodeRecord.costSoFar < endNodeCost)
{
continue;
}
closed.Remove(endNodeRecord);
}
bool wasFound = false;
endNodeRecord = open.Find(a => a.node == endNode);
if (endNodeRecord != null)
{
//esta na lista
wasFound = true;
if (endNodeRecord.costSoFar <= endNodeCost)
{
continue;
}
}
else
{
endNodeRecord = new NodeRecord();
endNodeRecord.node = endNode;
}
endNodeRecord.costSoFar = endNodeCost;
endNodeRecord.connection = connection;
if (!wasFound)
{
open.Add(endNodeRecord);
}
}
open.Remove(current);
closed.Add(current);
}
if (current.node != end)
{
return(null);
}
List <Connection> path = new List <Connection>();
while (current.node != start)
{
path.Add(current.connection);
current = closed.Find(x => x.node == current.connection.from);
}
path.Reverse();
List <Vector2> points = path.Select <Connection, Vector2>(c => c.to).ToList();
points.Add(end);
return(points);
}
public List <AStarConnection> AStarPathFind(AStarGraph graph, AStarNode start, AStarNode end)
{
List <AStarConnection> path = new List <AStarConnection>();
AStarNodeRecord current = new AStarNodeRecord();
List <AStarConnection> currentConnections = new List <AStarConnection>();
AStarNodeRecord endNode = new AStarNodeRecord();
AStarNodeRecord endNodeRecord = new AStarNodeRecord();
float endNodeHeuristic = 0;
AStarNodeRecord startRecord = new AStarNodeRecord();
startRecord.node = start;
startRecord.connection = null;
startRecord.costSoFar = 0;
startRecord.estimatedTotalCost = Heuristic(start, end);
openList = new List <AStarNodeRecord>();
openList.Add(startRecord);
closedList = new List <AStarNodeRecord>();
while (openList.Count > 0)
{
current = SmallestElement(openList);
if (current.node == end)
{
break;
}
currentConnections = current.node.GetConnections();
foreach (AStarConnection conect in currentConnections)
{
endNode.node = conect.ToNode;
endNode.connection = conect;
endNode.costSoFar = current.costSoFar + conect.Cost;
if (closedList.Contains(endNode))
{
endNodeRecord = closedList.Find(x => closedList.Contains(endNode));
if (endNodeRecord.costSoFar <= endNode.costSoFar)
{
//continue
}
else
{
closedList.Remove(endNodeRecord);
endNodeHeuristic = endNodeRecord.estimatedTotalCost - endNodeRecord.costSoFar;
}
}
else if (openList.Contains(endNode))
{
endNodeRecord = openList.Find(x => openList.Contains(endNode));
if (endNodeRecord.costSoFar <= endNode.costSoFar)
{
//continue
}
else
{
endNodeHeuristic = endNodeRecord.estimatedTotalCost - endNodeRecord.costSoFar;
}
}
else
{
endNodeRecord = new AStarNodeRecord(endNode);
endNodeHeuristic = Heuristic(endNode.node, end);
}
endNodeRecord.costSoFar = endNode.costSoFar;
endNodeRecord.connection = endNode.connection;
endNodeRecord.estimatedTotalCost = endNodeRecord.costSoFar + endNodeHeuristic;
if (!openList.Contains(endNode))
{
openList.Add(endNodeRecord);
}
}
openList.Remove(current);
closedList.Add(current);
}
if (current.node != end)
{
//return none
}
else
{
AStarNode temp = new AStarNode();
while (current.node != start)
{
path.Add(current.connection);
temp = current.connection.FromNode;
for (int i = closedList.Count - 1; i >= 0; i--)
{
if (closedList[i].node == temp)
{
current = closedList[i];
}
}
}
}
path.Reverse();
return(path);
}
