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);
}
public static List <Position> Backtrace(PathingNode traceNode)
{
var path = new List <Position>
{
new Position(traceNode)
};
while (traceNode.Parent != null)
{
traceNode = traceNode.Parent;
path.Add(new Position(traceNode));
}
path.Reverse();
return(path);
}
private static void IdentifySuccessors(PathingMaster args, PathingNode activeNode)
{
activeNode.IsClosed = true;
activeNode.IsOpen = false;
var priority = args.Priority;
var neighbors = FindNeighbors(args, activeNode);
foreach (var neighbor in neighbors)
{
int jX = 0, jY = 0;
if (!Jump(args, neighbor.X, neighbor.Y, activeNode.X, activeNode.Y, ref jX, ref jY))
{
continue;
}
var jumpNode = args.GetNode(jX, jY);
if (jumpNode.IsClosed)
{
continue;
}
var newG = activeNode.G + jumpNode.EuclideanDistance(activeNode);
if ((jumpNode.IsOpen || jumpNode.IsClosed) && newG >= jumpNode.G)
{
continue;
}
jumpNode.G = newG;
jumpNode.H = jumpNode.HDistance(args.Target);
jumpNode.F = jumpNode.H + jumpNode.G;
jumpNode.Parent = activeNode;
if (jumpNode.IsOpen || jumpNode.IsClosed)
{
priority.Update(jumpNode);
}
else
{
priority.Add(jumpNode);
}
jumpNode.IsOpen = true;
jumpNode.IsClosed = false;
}
}
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);
}
public PathingNode GetNode(int x, int y)
{
if (Map.Width < x || Map.Height < y)
{
return(null);
}
var id = x | y << 16;
PathingNode node;
if (Nodes.TryGetValue(id, out node))
{
return(node);
}
Nodes.Add(id, node = new PathingNode((ushort)x, (ushort)y));
return(node);
}
public PathingMaster(Map map, IPosition startPos, IPosition targetPos) : this(map)
{
Start = new PathingNode(startPos.X, startPos.Y);
Target = new PathingNode(targetPos.X, targetPos.Y);
}
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);
}
