public void Enqueue(Node node)
{
if (array.Length == length)
Enlarge();
array[length] = node;
length++;
for (int i = length - 2 ; i >= 0; i--)
{
if (array[i].TotalCost > node.TotalCost)
{
Node ghostNode = array[i + 1];
array[i + 1] = array[i];
array[i] = ghostNode;
}
else if (array[i].TotalCost <= node.TotalCost)
break;
}
}
public void addNode(Node node)
{
_nodes.Add(node);
}
public bool Contains(Node node)
{
return array.Any(node1 => node1 == node);
}
private double heuristicCostEstimated(Node current, Node goal)
{
var currentX = current._p.X;
var currentY = current._p.Y;
var goalX = goal._p.X;
var goalY = goal._p.Y;
var differenceX = currentX - goalX;
var differenceY = currentY - goalY;
if (differenceX < 0)
differenceX *= -1;
if (differenceY < 0)
differenceY *= -1;
var total = (Math.Sqrt(Math.Pow(differenceX, 2) + Math.Pow(differenceY, 2)));
return total;
}
private List<Node> ReconstructPath(Node current)
{
var output = new List<Node>();
if (current.Previous != null)
output = ReconstructPath(current.Previous);
output.Add(current);
return output;
}
private int DistBetween(Node current, Node nextNode)
{
Edge edge = current._edges.First(e => e._nextNode == nextNode);
if (edge._cost > 0)
return edge._cost;
var currentX = current._p.X;
var currentY = current._p.Y;
var neighborX = nextNode._p.X;
var neighborY = nextNode._p.Y;
var differenceX = currentX - neighborX;
var differenceY = currentY - neighborY;
if (differenceX < 0)
differenceX *= -1;
if (differenceY < 0)
differenceY *= -1;
var cost = (int)Math.Sqrt(Math.Pow(differenceX, 2) + Math.Pow(differenceY, 2));
edge._cost = cost;
return cost;
}
public void CreateListAStar(Node start, Node target)
{
List<Node> closedSet = new List<Node>(); // Set of nodes that are already evaluated.
AStarQueue openSet = new AStarQueue(); //Set of tentative nodes to be evaluated, initially containing the start node
List<Node> cameFrom = new List<Node>(); //List of navigated nodes
int maxDistanceAtcf, distanceATCFLeft;
List<Node> path = new List<Node>();
openSet.Enqueue(start);
start.DistanceFromStart = 0;
start.DistanceToGoal = (int)heuristicCostEstimated(start, target);
start.TotalCost = start.DistanceFromStart + start.DistanceToGoal;
maxDistanceAtcf = start.DistanceToGoal;
distanceATCFLeft = start.DistanceToGoal;
int bestScore = 0;
while (openSet.GetLength() != 0)
{
var current = openSet.Dequeue();
closedSet.Add(current);
current.aStarVisited = true;
bool _tentativeIsBetter;
if (current == target)
{
AStarTargets = ReconstructPath(current);
break;
}
foreach (var edge in current._edges)
{
Node nextNode = edge._nextNode;
var tentativeGScore = current.DistanceFromStart + DistBetween(current, nextNode);
if (nextNode.aStarVisited && tentativeGScore >= nextNode.DistanceFromStart) continue;
if (!openSet.Contains(nextNode) || tentativeGScore < nextNode.DistanceFromStart)
{
nextNode.DistanceToGoal = (int) heuristicCostEstimated(nextNode, target);
nextNode.Previous = current;
nextNode.DistanceFromStart = (int) tentativeGScore;
nextNode.TotalCost = edge._nextNode.DistanceFromStart + edge._nextNode.DistanceToGoal;
if (!openSet.Contains(nextNode))
openSet.Enqueue(nextNode);
}
}
}
}
/*public int getProgressPercent()
{
return (int)Math.Round(getProgress() * 100);
}*/
private void UpdateXml(Node n, int v)
{
try
{
var x = new XmlDocument();
x.Load(filepath);
string query = String.Format(startnode+n);
var node = x.SelectSingleNode(query);
node.InnerText = v.ToString();
x.Save(filepath);
}
catch (Exception)
{
UI.Notify("Could not save progess.");
}
}
public Edge(Node nextNode)
{
_nextNode = nextNode;
_cost = 0;
}
public Edge(Node nextNode, int cost)
{
_nextNode = nextNode;
_cost = cost;
}