public WaypointMinHeap(Waypoint root) : this()
{
WaypointMinHeapNode node = new WaypointMinHeapNode(root);
minHeap.Add(node);
waypointToNodeMap.Add(root, node);
}
public WaypointMinHeapNode setPredecessor(WaypointMinHeapNode predecessorNode)
{
this.predecessorNode = predecessorNode;
if (predecessorNode != null)
{
pathLength = predecessorNode.pathLength + (waypoint.transform.position - predecessorNode.waypoint.transform.position).magnitude;
}
return(this);
}
private WaypointMinHeapNode add(Waypoint waypoint, WaypointMinHeapNode predecessorNode)
{
// create new node
WaypointMinHeapNode node = new WaypointMinHeapNode(waypoint, predecessorNode);
// add node to map
waypointToNodeMap[waypoint] = node;
minHeap.Add(node);
bubbleUp(node);
return(node);
}
public WaypointMinHeapNode getFirst()
{
if (minHeap.Count > 0)
{
WaypointMinHeapNode node = (WaypointMinHeapNode)minHeap[0];
minHeap.RemoveAt(0);
return(node);
}
else
{
return(null);
}
}
private void bubbleUp(WaypointMinHeapNode node)
{
int currIdx = minHeap.IndexOf(node);
int parentIdx = getParentIndex(currIdx);
// termination condition
if (parentIdx < 0)
{
return;
}
WaypointMinHeapNode parentNode = (WaypointMinHeapNode)minHeap[parentIdx];
if (parentNode.pathLength > node.pathLength)
{
swap(currIdx, parentIdx);
bubbleUp(node);
}
}
public WaypointMinHeapNode update(Waypoint waypoint, WaypointMinHeapNode predecessorNode)
{
// if waypoint was not added already, add it
if (!contains(waypoint))
{
return(add(waypoint, predecessorNode));
}
else
// otherwise find corresponding node and update it
{
float pathLength = predecessorNode.pathLength + (waypoint.transform.position - predecessorNode.waypoint.transform.position).magnitude;
WaypointMinHeapNode node = waypointToNodeMap[waypoint];
if (pathLength < node.pathLength)
{
node.setPredecessor(predecessorNode);
bubbleUp(node);
}
return(node);
}
}
public WaypointMinHeapNode(Waypoint waypoint, WaypointMinHeapNode predecessorNode)
{
this.waypoint = waypoint;
setPredecessor(predecessorNode);
}
