public static int Comparison(Element e1, Element e2)
{
if (e1.intersection.Distance < e2.intersection.Distance)
return -1;
else if (e1.intersection.Distance == e2.intersection.Distance)
return 0;
else if (e1.intersection.Distance > e2.intersection.Distance)
return 1;
return 0;
}
private void UpdateLast()
{
while (last.Parent != null && last.Parent.Right == last)
last = last.Parent;
if (last.Parent != null)
{
last = last.Parent;
last = last.Right;
}
while (last.Left != null)
last = last.Left;
}
public Element(Intersection intersection, Element parent)
{
this.intersection = intersection;
this.leftE = null;
this.rightE = null;
this.parent = parent;
this.ID = globalID;
globalID++;
}
private void Heapify(Element element)
{
Element currentParent = element.Parent;
FixGrandParent(element);
if (element.Parent.Left.Equals(element))
{
FixLeft(element, element.Parent, false);
}
else
{
FixRight(element, element.Parent, false);
}
if (last.Equals(currentParent))
last = element;
else if (last.Equals(element))
last = currentParent;
if(element.Parent != null && element < element.Parent)
Heapify(element);
if (element.Parent == null)
head = element;
}
private Element PushDown(Element element)
{
if (element.Left != null && element.Left < element.Right)
{
if (element.Left < element)
{
Element newHead = element.Left;
FixGrandParent(element, element.Left);
FixLeft(element.Left, element, true);
PushDown(element);
return newHead;
}
else
{
return element;
}
}
else
{
if (element.Right != null && element.Right < element)
{
Element newHead = element.Right;
FixGrandParent(element, element.Right);
FixRight(element.Right, element, true);
PushDown(element);
return newHead;
}
else
{
return element;
}
}
}
public Intersection GetMin()
{
Intersection retVal = head.Intersection;
if (last.Right == null && last.Left == null)
{
if (last == head)
{
head = null;
return retVal;
}
else
{
GetPredecessor();
}
}
Element lastElement = null;
if (last.Right != null && last.Left != null)
{
if (last.Right > last.Left)
{
lastElement = last.Right;
last.Right = null;
}
else
{
lastElement = last.Left;
last.Left = last.Right;
last.Right = null;
}
}
else if (last.Right != null && last.Left == null)
{
lastElement = last.Right;
last.Right = null;
}
else if (last.Left != null && last.Right == null)
{
lastElement = last.Left;
last.Left = null;
}
lastElement.Parent = null;
lastElement.Left = head.Left;
lastElement.Right = head.Right;
if (lastElement.Left != null)
lastElement.Left.Parent = lastElement;
if (lastElement.Right != null)
lastElement.Right.Parent = lastElement;
bool resetLast = false;
if (last == head)
resetLast = true;
head = PushDown(lastElement);
if (resetLast)
{
last = head;
}
else
{
if (last.Equals(head))
{
Element current = head;
while (current.Left != null)
current = current.Left;
last = current.Parent;
}
}
return retVal;
}
private void GetPredecessor()
{
while (last.Parent != null && last.Parent.Left == last)
last = last.Parent;
if (last.Parent != null)
{
last = last.Parent;
last = last.Left;
}
while (last.Right != null)
last = last.Right;
last = last.Parent;
}
private void FixParents(Element node, Element origLeft, Element origRight, Element newHead)
{
if (node.Right != null)
node.Right.Parent = node;
if (node.Left != null)
node.Left.Parent = node;
if (origLeft != null)
origLeft.Parent = newHead;
if (origRight != null)
origRight.Parent = newHead;
}
private void FixRight(Element node, Element newRight, bool updateLast)
{
Element origLeft = node.Left;
Element origRight = node.Right;
node.Right = newRight;
node.Left = newRight.Left;
FixHelper(node, newRight, origLeft, origRight, updateLast);
}
private void FixLast(Element node, Element newHead)
{
if (last.Equals(node))
{
last = newHead;
}
else if (last.Equals(newHead))
{
last = node;
}
}
private void FixHelper(Element node, Element newHead, Element origLeft, Element origRight, bool updateLast)
{
node.Parent = newHead.Parent;
if (updateLast)
{
FixLast(node, newHead);
}
FixParents(node, origLeft, origRight, newHead);
newHead.Left = origLeft;
newHead.Right = origRight;
}
private void FixGrandParent(Element node, Element newChild)
{
if (node.Parent == null)
return;
if (node.Parent.Left.Equals(node))
{
node.Parent.Left = newChild;
}
else if(node.Parent.Right.Equals(node))
{
node.Parent.Right = newChild;
}
}
public IntersectionQueue()
{
head = null;
last = null;
}
public void Push(Intersection intersection)
{
if (head == null)
{
head = new Element(intersection, null);
last = head;
}
else
{
if (last.Right != null)
{
UpdateLast();
}
Element newElement = new Element(intersection, last);
if (last.Left == null)
last.Left = newElement;
else if (last.Right == null)
last.Right = newElement;
bool resetLast = false;
if (last == head)
resetLast = true;
if (newElement < newElement.Parent)
Heapify(newElement);
if (resetLast)
last = head;
}
}
