/// <summary>
/// Inserts an item in the Heap
/// </summary>
public void Insert(Vector2Int coordinates, PriorityKey key)
{
HeapElement e = new HeapElement(coordinates, key);
m_count++;
m_hash[coordinates] = m_count;
if (m_count == m_heap.Length)
{
IncreaseCap();
}
m_heap[m_count] = e;
MoveUp(m_count);
}
/// <summary>
/// Computes the shortest path from the current location to the goal location.
/// </summary>
private void ComputeShortestPath()
{
Node startNode = Map.GetNode(Start);
if (startNode.IsObstacle()) // Prevent from crashing when bot is inside a wall
{
return;
}
for (int i = 0; i < Map.Map.Length * Map.Map[0].Length; i++)
{
// Gets the priority key from the top
PriorityKey oldKey = m_heap.TopKey();
// Gets the node of the top
Vector2Int coordinates = m_heap.Pop();
if (coordinates == null)
{
break; // Heap is empty
}
Node node = Map.GetNode(coordinates);
// Gets new key based on current position that is being calculated
PriorityKey newKey = CalculatePriority(coordinates);
if (oldKey.CompareTo(newKey) < 0) // The node has a lower priority than before
{
m_heap.Insert(coordinates, newKey);
}
else if (node.CostFromStartingPoint > node.Rhs) // The g-value wasn't optimally calculated
{
node.CostFromStartingPoint = node.Rhs; // Set the g-value to the calculated Rhs-value
foreach (Vector2Int surroundingCoordinates in Map.GetSurroundingOpenSpaces(coordinates)) // Update the Rhs-value of the surrounding nodes
{
UpdateVertex(surroundingCoordinates);
}
}
else // Re-calculate the Rhs-value of the current node and its surrounding nodes
{
node.CostFromStartingPoint = double.PositiveInfinity;
UpdateVertex(coordinates);
foreach (Vector2Int surroundingNodes in Map.GetSurroundingOpenSpaces(coordinates)) // Update the Rhs-value of the surrounding nodes
{
UpdateVertex(surroundingNodes);
}
}
// íf the top state of the Heap does not have a higher priority than the start state AND start.rhs is the cost of start
if (!(m_heap.TopKey().CompareTo(CalculatePriority(Start)) < 0 || startNode.Rhs != startNode.CostFromStartingPoint))
{
break;
}
}
}
public int CompareTo(PriorityKey that)
{
if (Key1 < that.Key1)
{
return(-1);
}
else if (Key1 > that.Key1)
{
return(1);
}
if (Key2 > that.Key2)
{
return(1);
}
else if (Key2 < that.Key2)
{
return(-1);
}
return(0);
}
public HeapElement(Vector2Int coordinates, PriorityKey key)
{
Coordinates = coordinates;
Key = key;
}
public void EnableInput(PriorityKey key)
{
priorityKeyEnabledMap_[(int)key] = true;
RefreshEnabledStatus();
}
public void ClearInput(PriorityKey key)
{
priorityKeyEnabledMap_.Remove((int)key);
RefreshEnabledStatus();
}
public override string ToString()
{
return(PriorityKey.ToString() + " " + PriorityValue.ToString());
}
