public static List<Vector2f> CalcPath(Vector2f start, Vector2f end)
{
var astar = new AStar();
var goalNode = new AStarNode2D(null,null,0, (int)end.X, (int)end.Y);
var startNode = new AStarNode2D(null,goalNode, 0, (int)start.X, (int)start.Y) {GoalNode = goalNode};
astar.FindPath(startNode, goalNode);
return astar.Solution.ToArray().ToList().Select(i => new Vector2f(((AStarNode2D) i).X, ((AStarNode2D) i).Y)).ToList();
}
private void AddSuccessor(ArrayList aSuccessors, int ax, int ay)
{
if (ax < 0)
{
ax = 31;
}
if (ay < 0)
{
ay = 31;
}
if (ax > 31)
{
ax = 0;
}
if (ay > 31)
{
ay = 0;
}
var currentCost = Safety.Danger[ax, ay];
if (currentCost == -1)
{
return;
}
var newNode = new AStarNode2D(this, GoalNode, Cost + currentCost, ax, ay);
if (newNode.IsSameState(Parent))
{
return;
}
aSuccessors.Add(newNode);
}