Method MoveDown

src/simulator/local_sensing/include/ikd-Tree/ikd_Tree.cpp:1384–1397

}

void MANUAL_HEAP::MoveDown(int heap_index){
    int l = heap_index * 2 + 1;
    PointType_CMP tmp = heap[heap_index];
    while (l < heap_size){
        if (l + 1 < heap_size && heap[l] < heap[l+1]) l++;
        if (tmp < heap[l]){
            heap[heap_index] = heap[l];
            heap_index = l;
            l = heap_index * 2 + 1;
        } else break;
    }
    heap[heap_index] = tmp;
    return;
}
        
void MANUAL_HEAP::FloatUp(int heap_index){
    int ancestor = (heap_index-1)/2;