public void Contains() { MinHeap<int> heap1 = new MinHeap<int>(new int[] { 1, 2, 3, 4, 5 }), heap2 = new MinHeap<int>(new int[] { 5, 4, 3, 2, 1 }), heap3 = new MinHeap<int>(new int[] { 2, 4, 5, 1, 3 }), heap4 = new MinHeap<int>(new int[] { 1, 2, 4, 5 }), heap5 = new MinHeap<int>(new int[] { 5, 4, 2, 1 }), heap6 = new MinHeap<int>(new int[] { 2, 4, 5, 1 }); for (int i = 1; i <= 5; i++) { Assert.IsTrue(heap1.Contains(i)); Assert.IsTrue(heap2.Contains(i)); Assert.IsTrue(heap3.Contains(i)); if (i != 3) { Assert.IsTrue(heap4.Contains(i)); Assert.IsTrue(heap5.Contains(i)); Assert.IsTrue(heap6.Contains(i)); } else { Assert.IsFalse(heap4.Contains(i)); Assert.IsFalse(heap5.Contains(i)); Assert.IsFalse(heap6.Contains(i)); } } }
public void TestContains() { int[] heapArr = { }; MinHeap <int> pq = new MinHeap <int>(heapArr); for (int i = 1; i <= 100; ++i) { Assert.IsFalse(pq.Contains(i)); } for (int i = 1; i <= 100; ++i) { pq.Insert(i); } for (int i = 1; i <= 100; ++i) { Assert.IsTrue(pq.Contains(i)); } for (int i = 101; i <= 200; ++i) { Assert.IsFalse(pq.Contains(i)); } Assert.IsTrue(pq.Contains(1)); Assert.AreEqual(1, pq.Peek()); pq.Pop(); Assert.IsFalse(pq.Contains(1)); Assert.AreEqual(2, pq.Peek()); }
IEnumerator FindAIPath(Vector3 startPos, Vector3 targetPos) { Vector3[] waypoints = new Vector3[0]; bool pathSuccess = false; Node startNode = grid.NodeFromWorldPoint(startPos); Node targetNode = grid.NodeFromWorldPoint(targetPos); if ((startNode.walkable && startNode.aiWalkable) && (targetNode.walkable && targetNode.aiWalkable)) { MinHeap <Node> openSet = new MinHeap <Node>(grid.MaxSize); HashSet <Node> closedSet = new HashSet <Node>(); openSet.Add(startNode); while (openSet.Count > 0) { Node currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode == targetNode) { pathSuccess = true; break; } foreach (Node neighbour in grid.GetNeighbours(currentNode)) { if (!(neighbour.walkable && neighbour.aiWalkable) || closedSet.Contains(neighbour)) { continue; } int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour); if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = newMovementCostToNeighbour; neighbour.hCost = GetDistance(neighbour, targetNode); neighbour.parent = currentNode; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } else { openSet.UpdateItem(neighbour); } } } } } yield return(null); if (pathSuccess) { waypoints = RetracePath(startNode, targetNode); } requestManager.FinishedProcessingPath(waypoints, pathSuccess); }
public void ClearShouldRemoveAllPreviousValues() { int[] testInts = { 3, 2, 1, 0, -1, -2, -3 }; foreach (int testInt in testInts) { IntMinHeap.Add(testInt); } IntMinHeap.Clear(); foreach (int testInt in testInts) { Assert.IsFalse(IntMinHeap.Contains(testInt)); } }
public IEnumerable <Node> AStar(Vector3Int start, Vector3Int end) { var startNode = grid.GetNodeByWorldPos(start); var endNode = grid.GetNodeByWorldPos(end); var openStack = new MinHeap <Node>(grid.grid.Length) { startNode }; var closeStack = new HashSet <Node>(); while (openStack.Count > 0) { var lowestFNode = openStack.Extract(); if (lowestFNode.Equals(endNode)) { Path = ReconstructThePath(startNode, lowestFNode); return(Path); } closeStack.Add(lowestFNode); foreach (Node neighbour in grid.GetNeighbours(lowestFNode)) { if (neighbour.IsObstacle || closeStack.Contains(neighbour)) { continue; } int newCostToNeighbour = lowestFNode.G + GetDistance(lowestFNode, neighbour); if (newCostToNeighbour < neighbour.G || !openStack.Contains(neighbour)) { neighbour.G = newCostToNeighbour; neighbour.H = GetDistance(neighbour, endNode); neighbour.CameFrom = lowestFNode; if (!openStack.Contains(neighbour)) { openStack.Add(neighbour); } else { openStack.UpdateItem(neighbour); } } } } return(null); }
public List <Cell> Process() { Cell startCell = grid[(int)startPos.x, (int)startPos.y]; Cell endCell = grid[(int)endPos.x, (int)endPos.y]; MinHeap openSet = new MinHeap(width * height); HashSet <Cell> closedSet = new HashSet <Cell>(); openSet.Add(startCell); int count = 0; while (!openSet.IsEmpty()) { count++; Cell current = openSet.Pop(); closedSet.Add(current); if (current == endCell) { break; } foreach (var neighbour in GetNeighbours(current)) { if (!neighbour.walkable || closedSet.Contains(neighbour)) { continue; } int newCostToNeighbour = current.gCost + GetDistance(current, neighbour); if (!openSet.Contains(neighbour) || newCostToNeighbour < neighbour.gCost) { neighbour.gCost = newCostToNeighbour; neighbour.hCost = GetDistance(neighbour, endCell); neighbour.parent = current; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } } } } if (openSet.IsEmpty()) { return(null); } return(RetracePath(startCell, endCell)); }
private double FindMedian(MaxHeap <double> maxHeap, MinHeap <double> minHeap, double currentMedian, double[] input, int index, int window) { var runningMedian = currentMedian; if (minHeap.Count + maxHeap.Count == window) { var toRemove = input[index - window]; if (minHeap.Contains(toRemove)) { minHeap.Remove(toRemove); } else { maxHeap.Remove(toRemove); } runningMedian = RebalanceAndCalculateMedian(minHeap, maxHeap); } var target = input[index]; if (target >= runningMedian) { minHeap.Add(target); } else { maxHeap.Add(target); } return(RebalanceAndCalculateMedian(minHeap, maxHeap)); }
private void CalculateIntegrateField() { InitGrid(); MinHeap <Node> open_set = new MinHeap <Node>(size_x * size_y); open_set.Add(World2Node(player_pos)); while (open_set.Count > 0) { Node cur_node = open_set.RemoveFirst(); cur_node.mem_closed_set = true; foreach (Node neighbour in GetNeighbours(cur_node)) { if (neighbour.IsWalkable() && !neighbour.mem_closed_set) { float cost = cur_node.cost_val + CalculateCost(cur_node.grid_x, cur_node.grid_y, neighbour.grid_x, neighbour.grid_y); if (cost < neighbour.cost_val) { neighbour.cost_val = cost; neighbour.parent = cur_node; } if (!open_set.Contains(neighbour)) { open_set.Add(neighbour); } else { open_set.UpdateItem(neighbour); } } } } //Debug.Log() }
public static Vector3[] PathTo(Node Location, Node Target) { MinHeap <Node> Opened = new MinHeap <Node>(grid.maxSize); // Nodes Found HashSet <Node> Closed = new HashSet <Node>(); // Nodes Explored Opened.Push(Location); // Open current location int i = 0; while (Opened.Count > 0 && i < 4) { // Move Current node into explored Node Current = Opened.Pop(); i++; Closed.Add(Current); // Stop if target node found if (Current == Target || i == 4) { return(ToWaypoints(ReversePath(Location, Current))); } Node[] Neighbors = grid.GetNeighbors(Current); foreach (Node Neighbor in Neighbors) { if (Neighbor.isJumpable) { return(null); } if (!Neighbor.isWalkable || Closed.Contains(Neighbor)) { continue; } int Cost = Current.gCost + grid.GetDistance(Location, Target); if (!Opened.Contains(Neighbor) || Cost < Neighbor.hCost) { Neighbor.Parent = Current; Neighbor.gCost = Cost; Neighbor.hCost = grid.GetHeuristic(Neighbor, Target); if (!Opened.Contains(Neighbor)) { Debug.DrawLine(Current.worldPosition, Neighbor.worldPosition); Opened.Push(Neighbor); } } } } return(null); }
// https://leetcode.com/problems/cheapest-flights-within-k-stops/solution/ public int FindCheapestPrice(int n, int[][] flights, int src, int dst, int K) { List <Tuple <int, int> >[] graph = new List <Tuple <int, int> > [n]; MinHeap <int> heap = new MinHeap <int>(); for (int i = 0; i < n; i++) { graph[i] = new List <Tuple <int, int> >(); } for (int i = 0; i < flights.Length; i++) { graph[flights[i][0]].Add(new Tuple <int, int>(flights[i][1], flights[i][2])); } heap.Add(src, 0); while (heap.Count != 0) { HeapNode <int> cur = heap.ExtractHeapNode(); if (cur.Key % 1000 == dst) { return(cur.Weight); } if (cur.Key / 1000 <= K) { foreach (var node in graph[cur.Key % 1000]) { if (heap.Contains((cur.Key / 1000 + 1) * 1000 + node.Item1) && heap[(cur.Key / 1000 + 1) * 1000 + node.Item1].Weight > node.Item2 + cur.Weight) { heap.Decrease((cur.Key / 1000 + 1) * 1000 + node.Item1, node.Item2 + cur.Weight); } else if (!heap.Contains((cur.Key / 1000 + 1) * 1000 + node.Item1)) { heap.Add((cur.Key / 1000 + 1) * 1000 + node.Item1, node.Item2 + cur.Weight); } } } } return(-1); }
private PathNode FindReversePath(Vector2 start, Vector2 end, NavTerrainTypes linkTypeMask) { NodePool pool = new NodePool(minX, maxX, minY, maxY, distBetweenNodes); MinHeap <PathNode> openNodes = new MinHeap <PathNode>(); PathNode endNode = pool.GetAt(start); PathNode startNode = new PathNode(end, -1, NavTerrainTypes.Floor); startNode.pathRemainderEstimate = Vector2.Distance(start, end); openNodes.Add(startNode); while (openNodes.size > 0) { PathNode node = openNodes.Pop(); if (node == endNode) { return(endNode); } node.isClosed = true; foreach (PathNode adjacent in pool.GetAdjacentNodes(node)) { if (adjacent.isClosed || (linkTypeMask & adjacent.terrainType) == 0) { continue; } float cost = node.knownCost + Vector2.Distance(node.position, adjacent.position); if (!openNodes.Contains(adjacent)) { adjacent.knownCost = cost; adjacent.pathRemainderEstimate = Vector2.Distance(adjacent.position, endNode.position); openNodes.Add(adjacent); adjacent.pathParent = node; } else if (cost < adjacent.knownCost) { adjacent.knownCost = cost; adjacent.pathParent = node; openNodes.PriorityLowered(adjacent); } } } return(endNode); }
/// <summary> /// Re-evaluates the Rhs-value of a State /// </summary> private void UpdateVertex(Vector2Int coordinates) { Node node = Map.GetNode(coordinates); if (!coordinates.Equals(m_goal)) { node.Rhs = MinCost(coordinates); } if (m_heap.Contains(coordinates)) // To prevent any copies { m_heap.Remove(coordinates); } if (node.CostFromStartingPoint != node.Rhs) { m_heap.Insert(coordinates, CalculatePriority(coordinates)); } }
private MinHeap FillMinHeap(int[] nums, int size) { var heap = new MinHeap(); for (int i = 0; i < nums.Length; i++) { var num = nums[i]; if (heap.Size < size) { heap.Add(num); } else if (!heap.Contains(num) && heap.Min < num) { heap.RemoveMin(); heap.Add(num); } } return(heap); }
/// <summary> /// try to use MinHeap - August 15, 2018 /// Requirement: /// 1. Remove third maximum number; /// 2. If total of distinct numbers is less than three, remove maximum number. /// Tips: /// 1. Design a minimum heap using C# SortedSet; /// 2. If there is less than three distinct numbers in the array, return maximum one; /// 3. Always keep minimum heap size on check, make sure that it is less and equal to 3; /// 4. More on step 3, if the size of heap is bigger than three, remove minimum one; /// 5. Go over all the numbers in the array, put them to heap; /// 6. Remove smallest one at the end. /// </summary> /// <param name="numbers"></param> /// <returns></returns> public static int ThirdMax(int[] numbers) { if (numbers == null || numbers.Length == 0) { return(-1); } int size = 0; var length = numbers.Length; var minimumHeap = new MinHeap(); var count = 0; for (int i = 0; i < length && size < 3; i++) { minimumHeap.Add(numbers[i]); size = minimumHeap.Count(); // caught by online judge, heap excludes duplicate count++; } // if the array has less than three distinct number, return maximum one if (count == length && size < 3) { return(minimumHeap.GetLast()); } for (int i = count; i < length; i++) { var current = numbers[i]; // if the current number is in the minimm heap or smaller than minimum value in the heap if (minimumHeap.Contains(current) || current < minimumHeap.GetMin()) { continue; } minimumHeap.RemoveMin(); minimumHeap.Add(current); } return(minimumHeap.GetMin()); }
public Dictionary <int, int> GetShortestPath(List <Tuple <int, int> >[] graph, int sourceVertex) { MinHeap <int> minHeap = new MinHeap <int>(); Dictionary <int, int> distance = new Dictionary <int, int>(); for (int i = 0; i < graph.Length; i++) { minHeap.Add(i, Int32.MaxValue); } minHeap.Decrease(sourceVertex, 0); distance.Add(sourceVertex, 0); while (minHeap.Count != 0) { HeapNode <int> curVertex = minHeap.ExtractHeapNode(); distance[curVertex.Key] = curVertex.Weight; foreach (var adjVertex in graph[curVertex.Key]) { if (!minHeap.Contains(adjVertex.Item1)) { continue; } int newDistance = distance[curVertex.Key] + adjVertex.Item2; if (minHeap[adjVertex.Item1].Weight > newDistance) { minHeap.Decrease(adjVertex.Item1, newDistance); } } } return(distance); }
private void testPathA1(Node start, Node goal, int code) { int size = 2; int count = 0; bool pathSuccess = false; foreach (Node n in Map.nodes) { n.revert(); if (n.occCode != -1 && n.occCode != code) { alteredNodes.Add(n); n.walkable = false; count++; } } Debug.Log("Count: " + count); Debug.Log("Unit size: " + size * (1 / Map.length)); if (start == null || goal == null) { Debug.Log("Start or Goal are null, pathfinding cant be completed"); return; } if (!start.walkable || !goal.walkable) { Debug.Log("Start or Goal are not walkable, pathfinding cant be completed"); return; } MinHeap <Node> openSet = new MinHeap <Node>(); HashSet <Node> closedSet = new HashSet <Node>(); openSet.addItem(start); long viewed = 0; while (openSet.size > 0) { Node currentNode = openSet.getFront(); closedSet.Add(currentNode); viewed++; if (currentNode == goal) { //Add callback pathSuccess = true; break; } if (size > getDist(currentNode) && (currentNode.occCode == -1 || currentNode.occCode == code)) { Debug.Log("Ignoring start node"); continue; } foreach (Node n in Map.getNeighbors(currentNode)) { if (!n.walkable || closedSet.Contains(n)) { continue; } if (size * (1 / Map.length) > getDist(n)) { Debug.Log("Ignoring node"); continue; } int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, n) + n.moveCost; if (newMovementCostToNeighbour <= n.gCost || !openSet.Contains(n)) { n.gCost = newMovementCostToNeighbour; n.hCost = GetDistance(n, goal); n.parent = currentNode; if (!openSet.Contains(n)) { openSet.addItem(n); } else { openSet.UpdateItem(n); } } } } if (pathSuccess) { path = new List <Node>(); Node currentNode = goal; while (currentNode != start) { path.Add(currentNode); currentNode = currentNode.parent; } path.Reverse(); //simplifyPath(); //path = simplified; //buildVPath(); foreach (Node n in path) { GameObject g = Instantiate(nodeMarker2); g.transform.position = n.position; g.transform.localScale = Vector3.one * length; } } }
// constraints should be the target object! // Plan path uses a heuristic function h(n) of manhattan distance // Just change to the Euclidian distance for better performance public void PlanPath(Vector3 startPos, Vector3 goalPos, out List <Vector3> path, GameObject obj, params object[] constraints) { Debug.Log("====== startPos ===== " + startPos); Debug.Log("====== goalPos ===== " + goalPos); // clear nodes nodes.Clear(); // init empty path List <PathNode> plannedPath = new List <PathNode>(); //List<PathNode> openSet = new List<PathNode> (); MinHeap <PathNode> openSet = new MinHeap <PathNode>(new BetterHeuristicOld()); List <PathNode> closedSet = new List <PathNode>(); PathNode endNode = null; path = new List <Vector3>(); Vector3 size = Helper.GetObjectWorldSize(obj).size; //increment = size.magnitude/2 > defaultIncrement.magnitude ? new Vector3(size.x/2, size.y/2, size.z/2) : defaultIncrement; increment = size.magnitude > defaultIncrement.magnitude ? size : defaultIncrement; var watch = Stopwatch.StartNew(); PathNode startNode = new PathNode(startPos); startNode.scoreFromStart = 0; // Manhattan distance startNode.heuristicScore = Mathf.Abs(goalPos.x - startNode.position.x) + Mathf.Abs(goalPos.y - startNode.position.y) + Mathf.Abs(goalPos.z - startNode.position.z); // Euclidan distance // startNode.heuristicScore = new Vector3 (goalPos.x - startNode.position.x, goalPos.y - startNode.position.y, goalPos.z - startNode.position.z).magnitude; nodes.Add(startNode); openSet.Add(startNode); QuantizeSpace(obj, embeddingSpaceBounds, increment, constraints); // set increment to moving object size, clean up after each run watch.Stop(); Debug.Log("========= Time to quantize space " + watch.ElapsedMilliseconds); // find closest node to goal // TODO: if goal is inside concave object (concave voxeme provided as constraint) // find closest node to goal such that arc from testNode to goal // does not intersect non-concave component of object // if constraints contain a voxeme Voxeme testTarget = constraints.OfType <Voxeme>().FirstOrDefault(); if (testTarget != null) { // if that object is concave (e.g. cup) // if goalPos is within the bounds of target (e.g. in cup) if (testTarget.voxml.Type.Concavity.Contains("Concave") && Helper.GetObjectWorldSize(testTarget.gameObject).Contains(goalPos)) { endNode = new PathNode(new Vector3(goalPos.x, Helper.GetObjectWorldSize(testTarget.gameObject).max.y + size.y, goalPos.z)); nodes.Add(endNode); //Debug.Break(); } else { float dist = Mathf.Infinity; foreach (PathNode node in nodes) { if ((node.position - goalPos).magnitude < dist) { // if dist from this node to goal < dstance from previous node in list to goal dist = (node.position - goalPos).magnitude; endNode = node; } } } } else { float dist = Mathf.Infinity; foreach (PathNode node in nodes) { if ((node.position - goalPos).magnitude < dist) { // if dist from this node to goal < dstance from previous node in list to goal dist = (node.position - goalPos).magnitude; endNode = node; } } } Debug.Log("========= endNode ========== " + endNode); //PathNode endNode = new PathNode(goalPos); //nodes.Add (endNode); watch = Stopwatch.StartNew(); PlotArcs(Helper.GetObjectWorldSize(obj)); watch.Stop(); Debug.Log("========= Time to plot arcs " + watch.ElapsedMilliseconds); //return; //path.Add(startPos); PathNode nextNode = new PathNode(embeddingSpaceBounds.max + Vector3.one); plannedPath.Add(new PathNode(startPos)); // starting with startNode, for each neighborhood node of last node, assess A* heuristic // using best node found until endNode reached while (openSet.Count > 0) { Debug.Log(" ======== openSet.Count ====== " + openSet.Count); // O(1) PathNode curNode = openSet.TakeMin(); // O(n) // PathNode curNode = null; // // float testHeuristicScore = Mathf.Infinity; // foreach (PathNode node in openSet) { // if (node.heuristicScore < testHeuristicScore) { // testHeuristicScore = node.heuristicScore; // curNode = node; // } // } //nextNode = embeddingSpaceBounds.max + Vector3.one; //dist = (nextNode - endNode).magnitude; //float dist = ((embeddingSpaceBounds.max + Vector3.one)-endNode.position).magnitude; // if reached end node if ((curNode.position - endNode.position).magnitude < Constants.EPSILON) { // extend path to goal node (goal position) PathNode goalNode = new PathNode(goalPos); goalNode.cameFrom = curNode; path = ReconstructPath(startNode, goalNode); Debug.Log("====== path ===== "); foreach (var point in path) { Debug.Log(point); } break; } closedSet.Add(curNode); var arcList = arcs.Where(n => ((n.Item1.position - curNode.position).magnitude < Constants.EPSILON) || (n.Item2.position - curNode.position).magnitude < Constants.EPSILON).ToList(); foreach (var arc in arcList) { float testScore; if ((arc.Item1.position - curNode.position).magnitude < Constants.EPSILON) { if (!closedSet.Contains(arc.Item2)) { testScore = curNode.scoreFromStart + (arc.Item2.position - curNode.position).magnitude; if (testScore < arc.Item2.scoreFromStart) { nextNode = arc.Item2; arc.Item2.cameFrom = curNode; arc.Item2.scoreFromStart = testScore; // Manhattan distance // arc.Item2.heuristicScore = arc.Item2.scoreFromStart + // (Mathf.Abs(goalPos.x - arc.Item2.position.x) + Mathf.Abs(goalPos.y - arc.Item2.position.y) + // Mathf.Abs(goalPos.z - arc.Item2.position.z)); // Euclidian distance arc.Item2.heuristicScore = arc.Item2.scoreFromStart + new Vector3(goalPos.x - arc.Item2.position.x, goalPos.y - arc.Item2.position.y, goalPos.z - arc.Item2.position.z).magnitude; if (!openSet.Contains(arc.Item2)) { openSet.Add(arc.Item2); } } } } else if ((arc.Item2.position - curNode.position).magnitude < Constants.EPSILON) { if (!closedSet.Contains(arc.Item1)) { testScore = curNode.scoreFromStart + (arc.Item1.position - curNode.position).magnitude; if (testScore < arc.Item1.scoreFromStart) { nextNode = arc.Item1; arc.Item1.cameFrom = curNode; arc.Item1.scoreFromStart = testScore; // Manhattan distance // arc.Item1.heuristicScore = arc.Item1.scoreFromStart + // (Mathf.Abs(goalPos.x - arc.Item1.position.x) + Mathf.Abs(goalPos.y - arc.Item1.position.y) + // Mathf.Abs(goalPos.z - arc.Item1.position.z)); // Euclidian distance arc.Item1.heuristicScore = arc.Item1.scoreFromStart + new Vector3(goalPos.x - arc.Item1.position.x, goalPos.y - arc.Item1.position.y, goalPos.z - arc.Item1.position.z).magnitude; if (!openSet.Contains(arc.Item1)) { openSet.Add(arc.Item1); } } } } } } }
/// <summary> /// Given an array of tiles and a starting coordinate, /// find all reachable tiles (and paths to get to those tiles). /// This is implemented using a modified form of Djikstra's algorithm. /// /// We return a dictionary of <option, tuple<cost-to-option, path-to-option>>. /// </summary> /// <param name="tiles"></param> /// <param name="startCoord"></param> public static Dictionary <Coordinate, Tuple <int, List <Coordinate> > > FindReachableTiles( Unit unit, Tile[,] tiles) { int rows = Util.GetRows(tiles); int cols = Util.GetCols(tiles); int[,] dist = new int[rows, cols]; Coordinate[,] prev = new Coordinate[rows, cols]; var frontier = new MinHeap <TileCoordinateNode>(); TileCoordinate startTileCoordinate = new TileCoordinate( tiles[unit.location.r, unit.location.c], unit.location); TileCoordinateNode startNode = new TileCoordinateNode(startTileCoordinate, 0); frontier.Insert(startNode); for (int r = 0; r < rows; ++r) { for (int c = 0; c < cols; ++c) { Coordinate coord = new Coordinate(r, c); if (!coord.Equals(unit.location)) { dist[coord.r, coord.c] = int.MaxValue; prev[coord.r, coord.c] = null; } else { dist[coord.r, coord.c] = 0; prev[coord.r, coord.c] = null; // doesn't matter for start coord } } } while (frontier.HeapSize() > 0) { TileCoordinateNode node = frontier.Pop(); TileCoordinate tileCoordinate = node.tileCoordinate; Coordinate coordinate = tileCoordinate.coordinate; foreach (TileCoordinate adjacentTileCoord in GetAllowableAdjacentTiles(tiles, coordinate, unit.board, unit.team)) { Coordinate adjacentCoord = adjacentTileCoord.coordinate; // watch for overflow here int calculatedDist = dist[coordinate.r, coordinate.c] + adjacentTileCoord.tile.movementCost; bool calculatedDistPreferrable = dist[adjacentCoord.r, adjacentCoord.c] == int.MaxValue || calculatedDist < dist[adjacentCoord.r, adjacentCoord.c]; if (calculatedDistPreferrable && calculatedDist <= unit.movementPoints) { dist[adjacentCoord.r, adjacentCoord.c] = calculatedDist; prev[adjacentCoord.r, adjacentCoord.c] = coordinate; TileCoordinateNode adjacentNode = new TileCoordinateNode(adjacentTileCoord, calculatedDist); if (!frontier.Contains(adjacentNode)) { frontier.Insert(adjacentNode); } else { frontier.DecreaseKey(adjacentNode, adjacentNode); } } } } // djikstra finished // now processing and adding to the return dict var answer = new Dictionary <Coordinate, Tuple <int, List <Coordinate> > >(); for (int r = 0; r < rows; ++r) { for (int c = 0; c < cols; ++c) { Coordinate targetCoord = new Coordinate(r, c); int distanceToTarget = dist[r, c]; // cell must also be empty, unless it is the starting coord if (distanceToTarget != int.MaxValue && (targetCoord.Equals(unit.location) || unit.board[targetCoord.r, targetCoord.c] is null)) { /* * Console.WriteLine($"Distance from {unit.currentLocation}" + * $" to {targetCoordinate}" + * $" is: {distanceToTarget}"); */ //string ans = targetCoordinate.ToString(); List <Coordinate> pathToTarget = new List <Coordinate>(); Coordinate currentCoord = targetCoord; // all paths lead to the starting coordinate // and the starting coordinate's prev is null while (prev[currentCoord.r, currentCoord.c] != null) { // ans = $"{prev[targetCoordinate.r, targetCoordinate.c]}, {ans}"; pathToTarget.Insert(0, prev[currentCoord.r, currentCoord.c]); currentCoord = prev[currentCoord.r, currentCoord.c]; } pathToTarget.Add(targetCoord); // Console.WriteLine($"path to {targetCoord}: {String.Join(", ", pathToTarget)}"); answer.Add( targetCoord, new Tuple <int, List <Coordinate> >( distanceToTarget, pathToTarget) ); } } } return(answer); }
IEnumerator FindPath(Vector3 startPos, Vector3 targetPos) { Stopwatch sw = new Stopwatch(); sw.Start(); Node[] waypoints = new Node[0]; bool pathFound = false; Node startNode = gridManager.getNodeFromWorld(startPos); Node targetNode = gridManager.getNodeFromWorld(targetPos); if (startNode.walkable && targetNode.walkable) { MinHeap <Node> openSet = new MinHeap <Node>(gridManager.Size); HashSet <Node> closeSet = new HashSet <Node>(); openSet.Add(startNode); while (openSet.Count > 0) { Node curr = openSet.Poll(); closeSet.Add(curr); if (curr == targetNode) { GetPath(startNode, targetNode); sw.Stop(); print("path found in: " + sw.ElapsedMilliseconds + "ms"); pathFound = true; break; } foreach (Node neighbour in gridManager.getNeighbours(curr)) { if (!neighbour.walkable || closeSet.Contains(neighbour)) { continue; } int moveCost = curr.gCost + GetDistance(curr, neighbour); if (moveCost < neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = moveCost; neighbour.hCost = GetDistance(neighbour, targetNode); neighbour.parent = curr; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } } } } } yield return(null); if (pathFound) { waypoints = GetPath(startNode, targetNode); } requestManager.ProcessingDone(waypoints, pathFound); }
public List <Node> AStar(Node s, Node t) { if (s == null) { Debug.Log("source is null"); return(null); } if (t == null) { Debug.Log("target is null"); return(null); } // call InSameComponent() to first to find ccs if (!InSameComponent(s, t)) { return(new List <Node>()); } // need a minheap to evaluate the node with lowest cost ! MinHeap open = new MinHeap(20000); // todo don't hardcode this? open.InsertKey(s); HashSet <Node> closed = new HashSet <Node>(); while (open.heapSize > 0) { Node current = open.ExtractMin(); closed.Add(current); if (current.Equals(t)) // path has been found { return(ReconstructPath(s, t)); } foreach (Node neighbor in current.neighList) { if (closed.Contains(neighbor)) { // todo: skip to the next neighbor continue; } // if new path to neighbor is shorter, or neighbor is not in open, // set f_cost of neighbor float newMvmtCostToNeigh = current.gCost + Vector3.Distance(current.position, neighbor.position); if (newMvmtCostToNeigh < neighbor.gCost || !open.Contains(neighbor)) { neighbor.gCost = newMvmtCostToNeigh; neighbor.hCost = Vector3.Distance(neighbor.position, t.position); // set parent of neighbor to current node neighbor.comesFrom = current; if (!open.Contains(neighbor)) { open.InsertKey(neighbor); } } } } return(null); }
// Run A* Search, Given This Pathfinder's World And A Query private void Pathfind(PathQuery q) { // Initialization isThoughtCollidable = new bool[World.numCells.X, World.numCells.Y]; start = HashHelper.Hash(q.Start, World.numCells, World.size); end = HashHelper.Hash(q.End, World.numCells, World.size); ; for(int y = 0; y < World.numCells.Y; y++) { for(int x = 0; x < World.numCells.X; x++) { fScore[x, y] = int.MaxValue; gScore[x, y] = int.MaxValue; } } // Precondition: Any Buildings In World Have Valid Centers var viewedBuildings = gameState.teams[q.FOWIndex].ViewedEnemyBuildings; foreach(var vb in viewedBuildings) { var vbData = gameState.teams[vb.Team].Race.Buildings[vb.Type]; Point p = vb.CellPoint; for(int y = 0; y < vbData.GridSize.Y; y++) { for(int x = 0; x < vbData.GridSize.X; x++) { isThoughtCollidable[p.X + x, p.Y + y] = true; } } } gScore[start.X, start.Y] = 0; fScore[start.X, start.Y] = Estimate(start.X, start.Y); var openSet = new MinHeap<Point>(Comparison, 30); openSet.Insert(start); // A* Loop List<Point> path = null; while(openSet.Count > 0) { Point p = openSet.Pop(); if(p.X == end.X && p.Y == end.Y) { path = new List<Point>(); BuildPath(path, end); break; } bool canMove = false; foreach(Point n in NeighborhoodAlign(p).Where(InGrid)) { int tgs = gScore[p.X, p.Y] + 10; canMove = CanMoveFrom(p, n, q.FOWIndex); if(canMove && tgs < gScore[n.X, n.Y]) { prev[n.X, n.Y] = p; gScore[n.X, n.Y] = tgs; fScore[n.X, n.Y] = gScore[n.X, n.Y] + Estimate(n.X, n.Y); if(!openSet.Contains(n)) { openSet.Insert(n); } } } foreach(Point n in NeighborhoodDiag(p).Where(InGrid)) { int tgs = gScore[p.X, p.Y] + 14; // To Move Diagonally, Destination Must Be Reachable By Horizontal & Vertical Moves As Well canMove = CanMoveFrom(p, n, q.FOWIndex); foreach(Point d in DiagDecomp(p, n)) { canMove &= CanMoveFrom(p, d, q.FOWIndex); } if(canMove && tgs < gScore[n.X, n.Y]) { prev[n.X, n.Y] = p; gScore[n.X, n.Y] = tgs; fScore[n.X, n.Y] = gScore[n.X, n.Y] + Estimate(n.X, n.Y); if(!openSet.Contains(n)) { openSet.Insert(n); } } } } // Check If We Need To Find The Nearest Point if(path == null) { int s; bool[,] ch = new bool[World.numCells.X, World.numCells.Y]; Array.Clear(ch, 0, ch.Length); Point cg = FindClosestGoal(end, fScore, ch, out s, 0); if(s == int.MaxValue) { // Impossible } else { path = new List<Point>(); BuildPath(path, cg); } } // A* Conclusion if(path != null) { foreach(Point wp in path) { q.waypoints.Add(new Vector2(((float)wp.X + 0.5f) * World.cellSize, ((float)wp.Y + 0.5f) * World.cellSize)); } } q.IsComplete = true; }
public List <Node> FindPath(Vector3 startposition, Vector3 goalposition, int algorithmindex, int heuristicindex, out string unitmessage) { ResetNodeParentgCostAndhCost(); ClearLists(); Node startNode = grid.GetNodeFromWorldPoint(startposition); Node goalNode = grid.GetNodeFromWorldPoint(goalposition); Node currentNode; Stopwatch timer = new Stopwatch(); int nodesExploredCount = 0; string pathfindingUsed = ""; startNode.gCost = 0; startNode.hCost = grid.GetNodeDistance(startNode, goalNode, heuristicindex) + (int)startNode.nodeType; openList.Add(startNode); timer.Start(); while (openList.Count() > 0) { currentNode = openList.RemoveFrontItem(); if (!closedList.Contains(currentNode)) { closedList.Add(currentNode); nodesExploredCount++; } switch (algorithmindex) { case 0: ExpandDijkstraOpenList(currentNode, heuristicindex); pathfindingUsed = "Dijkstra with "; break; case 1: ExpandBestFirstSearchOpenList(currentNode, goalNode, heuristicindex); pathfindingUsed = "Greedy Best First Search with "; break; case 2: ExpandAStarOpenList(currentNode, goalNode, heuristicindex); pathfindingUsed = "A* Pathfinding with "; break; default: break; } if (openList.Contains(goalNode)) { pathList = GetPathNodes(goalNode); timer.Stop(); unitmessage = ((pathfindingUsed) + (HeuristicUsed(heuristicindex)) + ("Elapsed time = ") + (timer.Elapsed.TotalMilliseconds).ToString() + " milliseconds , Nodes Explored = " + nodesExploredCount.ToString() + ", Nodes To Goal = " + pathList.Count.ToString()); return(pathList); } } unitmessage = ("Path is blocked, no path possible to goal"); return(null); }
private void AStar() { //Debug.Log("Unit size: " + size * (1 / Map.length)); if (start == null || goal == null) { Debug.Log("Start or Goal are null, pathfinding cant be completed"); failed = true; return; } if (!start.walkable || !goal.walkable) { Debug.Log("Start or Goal are not walkable, pathfinding cant be completed"); failed = true; return; } if (!nodesAreOK(getNodesFromLocation(start.position)) || !nodesAreOK(getNodesFromLocation(goal.position))) { Debug.Log("Start or goal is not fully walkable, pathfinding cannot be completed"); failed = true; return; } MinHeap <Node> openSet = new MinHeap <Node>(); HashSet <Node> closedSet = new HashSet <Node>(); openSet.addItem(start); long viewed = 0; while (openSet.size > 0) { Node currentNode = openSet.getFront(); closedSet.Add(currentNode); viewed++; if (currentNode == goal) { //Add callback pathSuccess = true; break; } /*if (size > getDist(currentNode)) * { * Debug.Log("Ignoring start node"); * continue; * }*/ if (!nodesAreOK(getNodesFromLocation(currentNode.position))) { continue; } foreach (Node n in Map.getNeighbors(currentNode)) { if (!n.walkable || closedSet.Contains(n)) { continue; } if (!nodesAreOK(getNodesFromLocation(n.position))) { //Debug.Log("Ignoring node"); continue; } int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, n) + n.moveCost; if (newMovementCostToNeighbour <= n.gCost || !openSet.Contains(n)) { n.gCost = newMovementCostToNeighbour; n.hCost = GetDistance(n, goal); n.parent = currentNode; if (!openSet.Contains(n)) { openSet.addItem(n); } else { openSet.UpdateItem(n); } } } } if (pathSuccess) { retracePath(); } }
/// <summary> /// Given an array of tiles and a starting coordinate, /// find all reachable tiles (and paths to get to those tiles). /// This is implemented using a modified form of Djikstra's algorithm. /// </summary> /// <param name="tiles"></param> /// <param name="startCoord"></param> public static void FindReachableTiles( int rows, int cols, int[,] dist, Coordinate[,] prev, Tile[,] tiles, Coordinate startCoord, int movementPoints) { var frontier = new MinHeap <TileCoordinateNode>(); TileCoordinate startTileCoordinate = new TileCoordinate( tiles[startCoord.r, startCoord.c], startCoord); TileCoordinateNode startNode = new TileCoordinateNode(startTileCoordinate, 0); frontier.Insert(startNode); for (int r = 0; r < rows; ++r) { for (int c = 0; c < cols; ++c) { Coordinate coord = new Coordinate(r, c); if (!coord.Equals(startCoord)) { dist[coord.r, coord.c] = int.MaxValue; prev[coord.r, coord.c] = null; } else { dist[coord.r, coord.c] = 0; prev[coord.r, coord.c] = null; // doesn't matter for start coord } } } while (frontier.HeapSize() > 0) { TileCoordinateNode node = frontier.Pop(); TileCoordinate tileCoordinate = node.tileCoordinate; Coordinate coordinate = tileCoordinate.coordinate; foreach (TileCoordinate adjacentTileCoord in GetAdjacentTiles(tiles, coordinate)) { Coordinate adjacentCoord = adjacentTileCoord.coordinate; // watch for overflow here int calculatedDist = dist[coordinate.r, coordinate.c] + adjacentTileCoord.tile.movementCost; bool calculatedDistPreferrable = dist[adjacentCoord.r, adjacentCoord.c] == int.MaxValue || calculatedDist < dist[adjacentCoord.r, adjacentCoord.c]; if (calculatedDistPreferrable && calculatedDist <= movementPoints) { dist[adjacentCoord.r, adjacentCoord.c] = calculatedDist; prev[adjacentCoord.r, adjacentCoord.c] = coordinate; TileCoordinateNode adjacentNode = new TileCoordinateNode(adjacentTileCoord, calculatedDist); if (!frontier.Contains(adjacentNode)) { frontier.Insert(adjacentNode); } else { frontier.DecreaseKey(adjacentNode, adjacentNode); } } } } }
private void AStar() { if (start == null || goal == null) { Debug.Log("Start or Goal are null, pathfinding cant be completed"); failed = true; return; } if (!start.walkable || !goal.walkable) { Debug.Log("Start or Goal are not walkable, pathfinding cant be completed"); failed = true; return; } /*if (start.cushion < width || goal.cushion < width) * { * Debug.Log("Start or Goal is to close to obstacle, pathfinding cant be completed"); * failed = true; * return; * }*/ MinHeap <Node> openSet = new MinHeap <Node>(); HashSet <Node> closedSet = new HashSet <Node>(); openSet.addItem(start); long viewed = 0; while (openSet.size > 0) { Node currentNode = openSet.getFront(); closedSet.Add(currentNode); viewed++; if (currentNode == goal) { //Add callback pathSuccess = true; break; } foreach (Node n in Map.getNeighbors(currentNode)) { if (!n.walkable || closedSet.Contains(n)) { continue; } int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, n) + n.moveCost; if (newMovementCostToNeighbour <= n.gCost || !openSet.Contains(n)) { n.gCost = newMovementCostToNeighbour; n.hCost = GetDistance(n, goal); n.parent = currentNode; if (!openSet.Contains(n)) { openSet.addItem(n); } else { openSet.UpdateItem(n); } } } } if (pathSuccess) { retracePath(); } }
void FindPath(Vector3 start, Vector3 end) { Node startNode = m_Grid.NodeFromWorldPos(start); Node endNode = m_Grid.NodeFromWorldPos(end); //List<Node> openSet = new List<Node>(); MinHeap <Node> openSet = new MinHeap <Node>(m_Grid.GridSize); List <Node> closedSet = new List <Node>(); openSet.Add(startNode); //int fCost = 0; while (openSet.Count > 0) { Node _currentNode = openSet.RemoveFirstItem(); //The following was replaced by heap //Node _currentNode = openSet[0]; //for (int i = 1; i < openSet.Count; i++) //{ // if (openSet[i].m_ifCost < _currentNode.m_ifCost || openSet[i].m_ifCost == _currentNode.m_ifCost) // { // _currentNode = openSet[i]; // } //} //openSet.Remove(_currentNode); closedSet.Add(_currentNode); if (_currentNode == endNode) { RetracePath(startNode, endNode); return; } foreach (Node neighbour in m_Grid.GetNeighbour(_currentNode)) { if (neighbour.m_isBlocked || closedSet.Contains(neighbour)) { continue; } int NewMovementCost = _currentNode.m_iGCost + GetDistance(_currentNode, neighbour); if (NewMovementCost < neighbour.m_iGCost || !openSet.Contains(neighbour)) { neighbour.m_iGCost = NewMovementCost; neighbour.m_iHCost = GetDistance(neighbour, endNode); neighbour.m_Parent = _currentNode; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } else { // This was added when heap was implemented, Sorts upwards if openset contains neighbour node openSet.UpdateItem(neighbour); } } } } }