void BinaryHeap<T>::heapifyDown(int nodeIndex) {
int leftChildIndex = getLeftChildIndex(nodeIndex);
int rightChildIndex = getRightChildIndex(nodeIndex);
T leftChildElem, rightChildElem, currentNodeElem;
while(leftChildIndex != -1) {
if(rightChildIndex == -1) {
rightChildIndex = leftChildIndex;
}
leftChildElem = heap[leftChildIndex]->payload;
rightChildElem = heap[rightChildIndex]->payload;
currentNodeElem = heap[nodeIndex]->payload;
int higherPriorityChildIndex = compare(leftChildElem, rightChildElem) ? leftChildIndex : rightChildIndex;
T higherPriorityChildElem = compare(leftChildElem, rightChildElem) ? leftChildElem : rightChildElem;
if(compare(higherPriorityChildElem, currentNodeElem)) {
swap(higherPriorityChildIndex, nodeIndex);
nodeIndex = higherPriorityChildIndex;
}
else
break;
leftChildIndex = getLeftChildIndex(nodeIndex);
rightChildIndex = getRightChildIndex(nodeIndex);
}
}
void StatusStructure::move(size_t from, size_t to) {
if (to >= m_tree.size()) {
return;
}
// Movement must be recursive and breath-first instead of depth-first (otherwise we would overwrite elements before they are moved)
std::queue<std::pair<size_t, size_t>> moveQueue;
moveQueue.push(std::make_pair(from, to));
while (!moveQueue.empty()) {
from = moveQueue.front().first;
to = moveQueue.front().second;
moveQueue.pop();
auto toMoveFrag = getAt(from);
insertAt(to, toMoveFrag);
if (toMoveFrag != nullptr) {
// move the children of the moved element
insertAt(from, nullptr); // clear the element
moveQueue.push(std::make_pair(getLeftChildIndex(from), getLeftChildIndex(to))); // move left children
moveQueue.push(std::make_pair(getRightChildIndex(from), getRightChildIndex(to))); // move right children
}
}
}
void StatusStructure::remove(const LineSegmentPtr& fragment) {
bool valid;
size_t index = indexOf(fragment, valid);
if (!valid) {
return;
}
m_index_map.erase(fragment);
if (leftChild(index) == nullptr) {
move(getRightChildIndex(index), index);
return;
} else if (rightChild(index) == nullptr) {
move(getLeftChildIndex(index), index);
return;
}
// get direct left neighbour of index (most right child in left subtree)
size_t directLeftNeighbourIndex = getLeftChildIndex(index);
while (rightChild(directLeftNeighbourIndex) != nullptr) {
directLeftNeighbourIndex = getRightChildIndex(directLeftNeighbourIndex);
}
insertAt(index, getAt(directLeftNeighbourIndex));
move(getLeftChildIndex(directLeftNeighbourIndex), directLeftNeighbourIndex);
}
size_t StatusStructure::getClosestTo(const Point& point, float& outDist) const {
size_t index = 0;
auto currentFrag = getAt(index);
if (currentFrag == nullptr) {
return index;
}
float dist = getPositionOnScanLine(currentFrag, point);
size_t closestIndex = index;
outDist = dist;
for (;;) {
if (dist > 0 && leftChild(index) != nullptr) {
index = getLeftChildIndex(index);
} else if (dist < 0 && rightChild(index) != nullptr) {
index = getRightChildIndex(index);
} else {
break;
}
currentFrag = getAt(index);
dist = getPositionOnScanLine(currentFrag, point);
if (std::fabs(outDist) > std::fabs(dist)) {
closestIndex = index;
outDist = dist;
}
}
return closestIndex;
}
void StatusStructure::insert(LineSegmentPtr fragment, const Point& scanLinePosition) {
size_t currentIndex = 0;
auto currentFrag = getAt(currentIndex);
while (currentFrag != nullptr) {
if (currentFrag == fragment) {
return;
}
if (isALeftOfBOnScanLine(*fragment, *currentFrag, scanLinePosition)) {
currentIndex = getLeftChildIndex(currentIndex);
} else {
currentIndex = getRightChildIndex(currentIndex);
}
currentFrag = getAt(currentIndex);
}
insertAt(currentIndex, fragment);
}
size_t StatusStructure::getLeftNeighbourIndex(size_t index) const {
size_t currentIndex = index;
if (leftChild(currentIndex) != nullptr) {
// if there is a left sub tree find the most right leaf in that tree
currentIndex = getLeftChildIndex(currentIndex);
while (rightChild(currentIndex) != nullptr) {
currentIndex = getRightChildIndex(currentIndex);
}
return currentIndex;
}
// the root element can't be the right child of another element
if (currentIndex == 0) {
return index;
}
if (!isLeftChild(currentIndex)) {
// current element is the right child of its parent... hence the parent is the left neighbour
return getParentIndex(currentIndex);
}
// current element is the left child of its parent... traverse upwards to the first node that is not a left child or the root node
while (currentIndex > 0 && isLeftChild(currentIndex)) {
currentIndex = getParentIndex(currentIndex);
}
if (currentIndex == 0) {
return index;
}
// the fragment at currentIndex is right of its parent and the fragment that
// we started with is the most left child of this fragment... hence the parent is
// the direct left neighbour
currentIndex = getParentIndex(currentIndex);
return currentIndex;
}
size_t StatusStructure::getRightNeighbourIndex(size_t index) const {
size_t currentIndex = index;
if (rightChild(currentIndex) != nullptr) {
// if there is a right sub tree find the most left leaf in that tree
currentIndex = getRightChildIndex(currentIndex);
while (leftChild(currentIndex) != nullptr) {
currentIndex = getLeftChildIndex(currentIndex);
}
return currentIndex;
}
// the root element can't be the left child of another element
if (currentIndex == 0) {
return index;
}
if (isLeftChild(currentIndex)) {
// current element is the left child of its parent... hence the parent is the right neighbour
return getParentIndex(currentIndex);
}
// current element is the right child of its parent... traverse upwards to the first node that is not a right child or the root node
while (currentIndex > 0 && !isLeftChild(currentIndex)) {
currentIndex = getParentIndex(currentIndex);
}
if (currentIndex == 0) {
return index;
}
currentIndex = getParentIndex(currentIndex);
return currentIndex;
}
LineSegmentPtr StatusStructure::rightChild(size_t index) const {
return getAt(getRightChildIndex(index));
}
