public AStarNode(Point p, bool walkable)
{
Location = p;
IsWalkable = walkable;
G = 1;
ID = Guid.NewGuid().ToString();
}
float GetTraversal(Point from, Point to, Point destination)
{
// Prefer reading order, meaning U, L, R, D (N, W, E, S)
// This should also work for eight-direction or diagonals, though current intent is four cardinal directions.
float _ret = 1.00f;
if (to.Y < from.Y) // Up / North
{
_ret += 0f;
}
//_ret *= 1.0f; // No change in cost if up
if (to.X < from.X) // Left / West
{
_ret += 0.001f;
}
//_ret *= 1.1f; // Slight increase in cost if left
if (to.X > from.X) // Right / East
{
_ret += 0.002f;
}
//_ret *= 1.2f; // Slightly higher increase in cost if right
if (to.Y > from.Y) // Down / South
{
_ret += 0.003f;
}
// _ret *= 1.3f; // Even hight increase in cost if down
return(_ret);
}
public static int CheckManhattanDistance(Point p1, Point p2)
{
int _ret = 0;
_ret = Math.Abs(p1.X - p2.X) + Math.Abs(p1.Y - p2.Y);
return(_ret);
}
public static int CheckManhattanDistance(int x, int y, Point p)
{
int _ret = 0;
_ret = Math.Abs(x - p.X) + Math.Abs(y - p.Y);
return(_ret);
}
public Point Step()
{
List <Point> path = FindPath();
Point _ret = path.DefaultIfEmpty(Start.Location).FirstOrDefault();
//path.Clear();
return(_ret);
}
public Point AStar(char[,] grid, char[] walkable, Point start, Point finish)
{
Point _ret = new Point(0, 0);
AStarNode _start = new AStarNode(start);
AStarNode _finish = new AStarNode(finish);
return(_ret);
}
public AStarNav(char[,] grid, char[] walkable, Point start, Point finish)
{
this.Start = new AStarNode(start);
this.Finish = new AStarNode(finish);
this.Map = grid;
this.height = this.Map.GetLength(1);
this.width = this.Map.GetLength(0);
this.walkable = walkable;
InitNodes();
}
public AStarNode[,] InitNodes()
{
AStarNode[,] _ret = new AStarNode[this.width, this.height];
for (int y = 0; y < this.Map.GetLength(1); y++)
{
for (int x = 0; x < this.Map.GetLength(0); x++)
{
Point p = new Point(x, y);
if (this.Map[x, y] == '.')
{
_ret[x, y] = new AStarNode(p, true);
_ret[x, y].Cost = x + y;
//_ret[x, y].H = CoordinateGeometry.GetLinearDistance(p, this.Finish.Location);
_ret[x, y].H = CoordinateGeometry.CheckManhattanDistance(p, this.Finish.Location);
_ret[x, y].G = 99999;
_ret[x, y].State = AStarNodeState.Untested;
}
else if (this.Map[x, y] == 'G' || this.Map[x, y] == 'E')
{
_ret[x, y] = new AStarNode(p, false);
_ret[x, y].Cost = 99;
//_ret[x, y].H = CoordinateGeometry.GetLinearDistance(p, this.Finish.Location);
_ret[x, y].H = CoordinateGeometry.CheckManhattanDistance(p, this.Finish.Location);
_ret[x, y].G = 99999;
_ret[x, y].State = AStarNodeState.Untested;
}
else
{
_ret[x, y] = new AStarNode(p, false);
_ret[x, y].Cost = x + y;
//_ret[x, y].H = CoordinateGeometry.GetLinearDistance(p, this.Finish.Location);
_ret[x, y].H = CoordinateGeometry.CheckManhattanDistance(p, this.Finish.Location);
_ret[x, y].G = 99999;
}
}
}
this.nodes = _ret;
this.Start = nodes[Start.Location.X, Start.Location.Y];
this.Finish = nodes[Finish.Location.X, Finish.Location.Y];
this.Start.G = 0;
return(_ret);
}
IEnumerable <Point> IsWalkableFrom(AStarNode fromNode)
{
List <Point> _ret = new List <Point>();
Point p = fromNode.Location;
if (p.Y > 0)// && this.nodes[p.X, p.Y - 1].IsWalkable)
{
_ret.Add(new Point(p.X, p.Y - 1));
}
if (p.X < this.nodes.GetLength(0))// && this.nodes[p.X + 1, p.Y].IsWalkable)
{
_ret.Add(new Point(p.X + 1, p.Y));
}
if (p.Y < this.nodes.GetLength(1))// && this.nodes[p.X, p.Y + 1].IsWalkable)
{
_ret.Add(new Point(p.X, p.Y + 1));
}
if (p.X > 0)// && this.nodes[p.X - 1, p.Y].IsWalkable)
{
_ret.Add(new Point(p.X - 1, p.Y));
}
return(_ret);
}
public static float GetLinearDistance(Point p1, Point p2)
{
return((float)Math.Sqrt(Math.Pow(p2.X - p1.X, 2) + Math.Pow(p2.Y - p1.Y, 2)));
}
public AStarNode(Point p)
{
Location = p;
G = 1;
ID = Guid.NewGuid().ToString();
}
