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