public static List <Vector2> FindPath(PathingMaster args)
{
var priority = args.Priority;
var startNode = args.Start;
var endNode = args.Target;
if (startNode == endNode)
{
return(null);
}
if (!args.ValidateNodeForPath(endNode))
{
return(null);
}
startNode.G = 0;
startNode.H = startNode.HDistance(endNode);
startNode.F = startNode.H;
priority.Add(startNode);
for (var attempts = 0; !priority.Empty && attempts < 10; attempts++)
{
var jumpNode = priority.Top();
if (jumpNode.Equals(endNode))
{
return(ExpandPath(PathingNode.Backtrace(jumpNode)));
}
IdentifySuccessors(args, jumpNode);
}
return(null);
}
private static bool IdentifySuccessors(PathingMaster args, PathingNode activeNode)
{
var priority = args.Priority;
var endNode = args.Target;
priority.Clear();
activeNode.IsClosed = true;
var any = false;
var neighbors = activeNode.GetNeighbors();
foreach (var neighbor in neighbors)
{
var checkNode = args.GetNode(neighbor.X, neighbor.Y);
if (checkNode == null)
{
continue;
}
if (args.ValidateNodeForPath(checkNode))
{
continue;
}
if (checkNode.IsClosed)
{
continue;
}
//get angle, active to end
var activeToEndAngle = activeNode.GetAngleRadian(endNode);
//check arc
if (activeNode.HasInArc(neighbor, activeToEndAngle, 2 * Math.PI / 3, 2.0f))
{
//distance(h) + active to end angle difference from active to neighbor * 2(g) = f
var activeToNeighborAngle = activeNode.GetRelativeAngleRadian(neighbor, activeToEndAngle);
checkNode.G = Math.Abs(activeToEndAngle - activeToNeighborAngle) * 2;
checkNode.H = checkNode.HDistance(endNode);
checkNode.F = checkNode.G + checkNode.H;
//push to priority stack
priority.Add(checkNode);
any = true;
}
}
return(any);
}
private static IEnumerable <Position> FindNeighbors(PathingMaster args, PathingNode jumpNode)
{
var parent = jumpNode.Parent;
var x = jumpNode.X;
var y = jumpNode.Y;
var neighbors = new List <Position>();
if (parent != null)
{
var px = parent.X;
var py = parent.Y;
var dx = (x - px) / Math.Max(Math.Abs(x - px), 1);
var dy = (y - py) / Math.Max(Math.Abs(y - py), 1);
if (dx != 0 && dy != 0)
{
if (args.ValidateNodeForPath(x, y + dy))
{
neighbors.Add(new Position(x, y + dy));
}
if (args.ValidateNodeForPath(x + dx, y))
{
neighbors.Add(new Position(x + dx, y));
}
if (args.ValidateNodeForPath(x + dx, y + dy) && (args.ValidateNodeForPath(x, y + dy) || args.ValidateNodeForPath(x + dx, y)))
{
neighbors.Add(new Position(x + dx, y + dy));
}
if (args.ValidateNodeForPath(x - dx, y + dy) && args.ValidateNodeForPath(x, y + dy) && !args.ValidateNodeForPath(x - dx, y))
{
neighbors.Add(new Position(x - dx, y + dy));
}
if (!args.ValidateNodeForPath(x + dx, y - dy))
{
return(neighbors);
}
if (args.ValidateNodeForPath(x + dx, y) && !args.ValidateNodeForPath(x, y - dy))
{
neighbors.Add(new Position(x + dx, y - dy));
}
}
else if (dx == 0)
{
if (!args.ValidateNodeForPath(x, y + dy))
{
return(neighbors);
}
neighbors.Add(new Position(x, y + dy));
if (args.ValidateNodeForPath(x + 1, y + dy) && !args.ValidateNodeForPath(x + 1, y))
{
neighbors.Add(new Position(x + 1, y + dy));
}
if (args.ValidateNodeForPath(x - 1, y + dy) && !args.ValidateNodeForPath(x - 1, y))
{
neighbors.Add(new Position(x - 1, y + dy));
}
}
else
{
if (!args.ValidateNodeForPath(x + dx, y))
{
return(neighbors);
}
neighbors.Add(new Position(x + dx, y));
if (args.ValidateNodeForPath(x + dx, y + 1) && !args.ValidateNodeForPath(x, y + 1))
{
neighbors.Add(new Position(x + dx, y + 1));
}
if (args.ValidateNodeForPath(x + dx, y - 1) && !args.ValidateNodeForPath(x, y - 1))
{
neighbors.Add(new Position(x + dx, y - 1));
}
}
}
else
{
neighbors.AddRange(from neighbor in jumpNode.GetNeighbors() where args.ValidateNodeForPath(neighbor) select new Position(neighbor.X, neighbor.Y));
}
return(neighbors);
}
private static bool Jump(PathingMaster args, int x, int y, int pX, int pY, ref int jX, ref int jY)
{
if (!args.ValidateNodeForPath(x, y))
{
return(false);
}
if (args.Target.Equals(x, y))
{
jX = x;
jY = y;
return(true);
}
var dx = x - pX;
var dy = y - pY;
if (dx != 0 && dy != 0)
{
if (args.ValidateNodeForPath(x - dx, y + dy) && !args.ValidateNodeForPath(x - dx, y) || args.ValidateNodeForPath(x + dx, y - dy) && !args.ValidateNodeForPath(x, y - dy))
{
jX = x;
jY = y;
return(true);
}
}
else if (dx != 0)
{
if (args.ValidateNodeForPath(x + dx, y + 1) && !args.ValidateNodeForPath(x, y + 1) || args.ValidateNodeForPath(x + dx, y - 1) && !args.ValidateNodeForPath(x, y - 1))
{
jX = x;
jY = y;
return(true);
}
}
else if (args.ValidateNodeForPath(x + 1, y + dy) && !args.ValidateNodeForPath(x + 1, y) || args.ValidateNodeForPath(x - 1, y + dy) && !args.ValidateNodeForPath(x - 1, y))
{
jX = x;
jY = y;
return(true);
}
if (dx != 0 && dy != 0)
{
//jump x and y for diagonal neighbors
var jx = Jump(args, x + dx, y, x, y, ref jX, ref jY);
var jy = Jump(args, x, y + dy, x, y, ref jX, ref jY);
if (jx || jy)
{
jX = x;
jY = y;
return(true);
}
}
if (args.ValidateNodeForPath(x + dx, y) || args.ValidateNodeForPath(x, y + dy))
{
return(Jump(args, x + dx, y + dy, x, y, ref jX, ref jY));
}
return(false);
}