public List <PathFindNode> GetRoad(int numCheckpoints, System.Random rnd) { List <PathFindNode> path = new List <PathFindNode>(); path.Clear(); PathFindNode start = GetNewRoadGoal(null, path, rnd, false); PathFindNode target = null; for (int i = 0; i < numCheckpoints; i++) { if (i == numCheckpoints - 1) { target = path[0]; } else { target = GetNewRoadGoal(start, path, rnd, i % 2 == 0); } LinkedList <PathFindNode> p = PathFind(start, target); if (p == null) { if (i == 0) { start = target; } i--; continue; } start = target; AddRoadSegment(path, p); } return(path); }
public float Distance(PathFindNode other) { float dx = x - other.x; float dy = y - other.y; return(Mathf.Sqrt(dx * dx + dy * dy)); }
private void PathFindTryPath(PathFindNode from, PathFindNode to, PathFindNode target, LinkedList <PathFindNode> queue) { if (to == null || to.visited) { return; } float distance = from.distance + ((from.height - to.height) * heightPenalty) * ((from.height - to.height) * heightPenalty) + from.Distance(to); if (distance + 0.1f < to.distance) { to.distance = distance; to.prevNode = from; to.estimation = to.distance + to.Distance(target); if (to.queueNode != null) { queue.Remove(to.queueNode); } if (queue.Count == 0) { queue.AddFirst(to); } var pos = queue.First; while (pos.Next != null && to.estimation > pos.Next.Value.estimation) { pos = pos.Next; } to.queueNode = queue.AddAfter(pos, to); } }
public void Remove(PathFindNode node) { if (node.QueueIndex == numNodes) { nodes[numNodes--] = null; return; } var formerLastNode = nodes[numNodes]; Swap(node, formerLastNode); nodes[numNodes--] = null; var parentIndex = formerLastNode.QueueIndex / 2; var parentNode = nodes[parentIndex]; if (parentIndex > 0 && CompareTo(formerLastNode, parentNode) >= 0) { SortUp(formerLastNode); } else { SortDown(formerLastNode); } }
public int SqrDistance(PathFindNode other) { int dx = x - other.x; int dy = y - other.y; return(dx * dx + dy * dy); }
public void Reset() { distance = 99999f; prevNode = null; queueNode = null; visited = false; estimation = float.PositiveInfinity; }
public LinkedList <PathFindNode> PathFind(PathFindNode start, PathFindNode target) { int width = nodes.GetLength(0); int height = nodes.GetLength(1); if (!Utils.InMargin(start.x, start.y, width, height, 1) || !Utils.InMargin(target.x, target.y, width, height, 1) || target == null || start == null) { return(null); } for (int i = 0; i < width; i++) { for (int j = 0; j < height; j++) { if (nodes[i, j] != null) { nodes[i, j].Reset(); } } } int jumpDistance = 1; LinkedList <PathFindNode> queue = new LinkedList <PathFindNode>(); start.distance = 0; queue.AddFirst(start); while (queue.Count > 0) { PathFindNode current = queue.First.Value; queue.RemoveFirst(); current.queueNode = null; current.visited = true; if (current == target) { LinkedList <PathFindNode> path = new LinkedList <PathFindNode>(); while (target != null) { path.AddFirst(target); target = target.prevNode; } return(path); } PathFindTryPath(current, nodes[current.x + jumpDistance, current.y], target, queue); PathFindTryPath(current, nodes[current.x - jumpDistance, current.y], target, queue); PathFindTryPath(current, nodes[current.x, current.y + jumpDistance], target, queue); PathFindTryPath(current, nodes[current.x, current.y - jumpDistance], target, queue); PathFindTryPath(current, nodes[current.x + jumpDistance, current.y + jumpDistance], target, queue); PathFindTryPath(current, nodes[current.x + jumpDistance, current.y - jumpDistance], target, queue); PathFindTryPath(current, nodes[current.x - jumpDistance, current.y + jumpDistance], target, queue); PathFindTryPath(current, nodes[current.x - jumpDistance, current.y - jumpDistance], target, queue); if (queue.Count > nodes.Length / 8) { return(null); } } return(null); }
private void Swap(PathFindNode node1, PathFindNode node2) { nodes[node1.QueueIndex] = node2; nodes[node2.QueueIndex] = node1; var temp = node1.QueueIndex; node1.QueueIndex = node2.QueueIndex; node2.QueueIndex = temp; }
private static int GetDistance(PathFindNode nodeA, PathFindNode nodeB) { var dstX = Math.Abs(nodeA.X - nodeB.X); var dstZ = Math.Abs(nodeA.Z - nodeB.Z); var dstY = Math.Abs(nodeA.Position.y - nodeB.Position.y); if (dstX > dstZ) { return(14 * dstZ + 10 * (dstX - dstZ) + (int)(10 * dstY)); } return(14 * dstX + 10 * (dstZ - dstX) + (int)(10 * dstY)); }
private static List <Vector3> RetracePath(PathFindNode startNode, PathFindNode endNode) { var path = new List <Vector3>(); while (endNode != startNode) { path.Add(endNode.Position); endNode = endNode.Parent; } path.Reverse(); //path.RemoveAt(0); return(path); }
private void SortDown(PathFindNode node) { var finalQueueIndex = node.QueueIndex; while (true) { var newParent = node; var childLeftIndex = 2 * finalQueueIndex; if (childLeftIndex > numNodes) { node.QueueIndex = finalQueueIndex; nodes[finalQueueIndex] = node; break; } var childLeft = nodes[childLeftIndex]; if (CompareTo(childLeft, newParent) >= 0) { newParent = childLeft; } var childRightIndex = childLeftIndex + 1; if (childRightIndex <= numNodes) { var childRight = nodes[childRightIndex]; if (CompareTo(childRight, newParent) >= 0) { newParent = childRight; } } if (newParent != node) { nodes[finalQueueIndex] = newParent; var temp = newParent.QueueIndex; newParent.QueueIndex = finalQueueIndex; finalQueueIndex = temp; } else { node.QueueIndex = finalQueueIndex; nodes[finalQueueIndex] = node; break; } } }
private static PathFindNode FindPathNodeOrCreate(int x, int z, float y) { var node = Grid[x, z]; if (node != null) { return(node); } var halfGrid = Size / 2f; var groundPos = new Vector3(x - halfGrid, y, z - halfGrid); //groundPos.y = TerrainMeta.HeightMap.GetHeight(groundPos); FindRawGroundPosition(groundPos, out groundPos); Grid[x, z] = node = new PathFindNode(groundPos); return(node); }
private PathFindNode GetNeighbourIfExistOrClose(PathFindNode currentEndNode, Vector3 currentPosition, Vector3 targetPosition) { float distanceBetweenCurrentAndNode = MathUtils.GetDistance2D(currentPosition, currentEndNode.transform.position); if (currentEndNode.neighbours.Length > 0) { PathFindNode pathfindNode = currentEndNode.neighbours [0]; float distanceBetweenCurrentAndNeighbour = MathUtils.GetDistance2D(currentPosition, pathfindNode.transform.position); if (distanceBetweenCurrentAndNeighbour < (distanceBetweenCurrentAndNode * extraDistancePenalityForNonNeighbour)) { return(pathfindNode); } } return(null); }
private PathFindNode FindNodeClosestTo(List <PathFindNode> nodesToUse, Vector3 goalPosition) { float closestDistance = float.MaxValue; PathFindNode closestNode = null; foreach (PathFindNode pathFindNode in nodesToUse) { float distance = MathUtils.GetDistance2D(goalPosition, pathFindNode.transform.position); if (distance < closestDistance) { closestDistance = distance; closestNode = pathFindNode; } } return(closestNode); }
private void SortUp(PathFindNode node) { var parent = node.QueueIndex / 2; while (parent > 0) { var parentNode = nodes[parent]; if (CompareTo(parentNode, node) >= 0) { break; } Swap(node, parentNode); parent = node.QueueIndex / 2; } }
private PathFindNode FindNeighbourClosestToNode(PathFindNode currentEndNode, Vector3 currentPosition, Vector3 targetPosition) { float closestDistance = float.MaxValue; PathFindNode closestNode = null; float distanceBetweenGoalAndEndNode = MathUtils.GetDistance2D(currentPosition, currentEndNode.transform.position); foreach (PathFindNode pathfindNode in currentEndNode.neighbours) { float distanceBetweenCurrentAndNeighbour = MathUtils.GetDistance2D(currentPosition, pathfindNode.transform.position); if (distanceBetweenCurrentAndNeighbour < closestDistance && distanceBetweenCurrentAndNeighbour < distanceBetweenGoalAndEndNode) { closestDistance = distanceBetweenCurrentAndNeighbour; closestNode = pathfindNode; } } return(closestNode); }
private static int CompareTo(PathFindNode node, PathFindNode other) { if (node.F == other.F) { if (node.H == other.H) { return(0); } if (node.H > other.H) { return(-1); } return(1); } if (node.F > other.F) { return(-1); } return(1); }
private PathFindNode GetNewRoadGoal(PathFindNode start, List <PathFindNode> road, System.Random rnd, bool goalClose = false) { PathFindNode node = null; if (start == null) { while (node == null) { node = nodes[rnd.Next(1, nodes.GetLength(0) - 1), rnd.Next(1, nodes.GetLength(0) - 1)]; } return(node); } bool prev10 = true; do { node = nodes[rnd.Next(1, nodes.GetLength(0) - 1), rnd.Next(1, nodes.GetLength(0) - 1)]; if (node == null) { continue; } if (goalClose && node.SqrDistance(start) > nodes.Length / 16) { continue; } prev10 = false; for (int pn = Mathf.Max(0, road.Count - 10); pn < road.Count; pn++) { if (node.SqrDistance(road[pn]) <= 9) { prev10 = true; break; } } }while (prev10); return(node); }
public List <PathFindNode> FindBestPathToTargetFrom(List <PathFindNode> allNodes, Vector3 currentPosition, Vector3 target) { this.openNodes = allNodes; this.closedNodes = new List <PathFindNode> (); PathFindNode nodeClosestToTarget = FindNodeClosestTo(openNodes, target); PathFindNode currentStartNode = nodeClosestToTarget; closedNodes.Add(nodeClosestToTarget); while (currentStartNode != null) { currentStartNode = GetNeighbourIfExistOrClose(currentStartNode, currentPosition, target); if (currentStartNode) { closedNodes.Add(currentStartNode); } } closedNodes.Reverse(); return(closedNodes); }
public PathFindNode(TileSpawnData t, PathFindNode p) { tile = t; parent = p; }
public bool Contains(PathFindNode node) { return(nodes[node.QueueIndex] == node); }
public void Update(PathFindNode node) { SortUp(node); }
public List <Vector3> FindPath(Vector3 sourcePos, Vector3 targetPos) { //Interface.Oxide.LogInfo("Queue: {0} Grid: {1}", OpenList.MaxSize, Grid.Length); var closedList = new HashSet <PathFindNode>(); var targetNode = new PathFindNode(targetPos); if (targetNode.X < 0 || targetNode.X >= Size || targetNode.Z < 0 || targetNode.Z >= Size) { return(null); } Grid[targetNode.X, targetNode.Z] = targetNode; var startNode = new PathFindNode(sourcePos); if (startNode.X < 0 || startNode.X >= Size || startNode.Z < 0 || startNode.Z >= Size) { return(null); } Grid[startNode.X, startNode.Z] = startNode; OpenList.Enqueue(startNode); //PathFindNode closestNode = null; while (OpenList.Count > 0) { var currentNode = OpenList.Dequeue(); if (currentNode == targetNode) { Clear(); return(RetracePath(startNode, targetNode)); } closedList.Add(currentNode); for (var i = 0; i < 8; i++) { var dirX = Direction[i, 0]; var dirZ = Direction[i, 1]; var x = currentNode.X + dirX; var z = currentNode.Z + dirZ; if (x < 0 || x >= Size || z < 0 || z >= Size) { continue; } var neighbour = FindPathNodeOrCreate(x, z, currentNode.Position.y); //Interface.Oxide.LogInfo("Checking neighbour: {0} {1} {2} {3} {4}", x, z, neighbour.Position, closedList.Contains(neighbour), neighbour.Walkable); if (!neighbour.Walkable) { continue; } var newGScore = currentNode.G + GetDistance(currentNode, neighbour); if (newGScore >= neighbour.G && closedList.Contains(neighbour)) { continue; } if (newGScore < neighbour.G || !OpenList.Contains(neighbour)) { //foreach (var player in BasePlayer.activePlayerList) // player.SendConsoleCommand("ddraw.sphere", 30f, Color.black, neighbour.Position, .25f); neighbour.G = newGScore; neighbour.H = GetDistance(neighbour, targetNode); neighbour.F = newGScore + neighbour.H; neighbour.Parent = currentNode; if (!OpenList.Contains(neighbour)) { OpenList.Enqueue(neighbour); } else { OpenList.Update(neighbour); } //if (closestNode == null || newGScore < closestNode.G) // closestNode = neighbour; } } if (closedList.Count > MaxDepth) { //Interface.Oxide.LogWarning("[PathFinding] Hit MaxDepth!"); break; } } Clear(); //if (closestNode != null) // return RetracePath(startNode, closestNode); return(null); }
public void Enqueue(PathFindNode node) { nodes[++numNodes] = node; node.QueueIndex = numNodes; SortUp(node); }