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); }