Создайте структуру кучи (для второго маркера) и поместите каждый из ее узлов на карту (для первого маркера).
EDIT:
Реализация минимальной кучи, которую я делал в прошлом
#ifndef MINHEAP_H
#define MINHEAP_H
//////////////////////// MinHeap with Map for Data ////////////////////////
template <class T, class M = int> class MinHeap {
T* array;
unsigned const int arr_max;
unsigned int elements;
M map;
void percolate_down(unsigned int i=0) {
unsigned int n = elements-1, min;
do {
unsigned int l = 2*i + 1, r = 2*i + 2;
if (l <= n && array[i] > array[l]) min = l;
else min = i;
if (r <= n && array[i] > array[r] && array[l] > array[r]) min = r;
if (i != min) {
T temp = array[i];
array[i] = array[min];
array[min] = temp;
map.update(array[i], i);
map.update(array[min], min);
i = min;
} else break;
} while (i < n);
}
void percolate_up(unsigned int i) {
while (i && array[(i-1)/2] > array[i]) {
T temp = array[i];
array[i] = array[(i-1)/2];
array[(i-1)/2] = temp;
map.update(array[i], i);
map.update(array[(i-1)/2], (i-1)/2);
i = (i-1)/2;
}
}
public:
MinHeap(const int max) : array(new T[max]), arr_max(max), elements(0), map(max) {}
~MinHeap(void) { delete[] array; }
bool empty(void) const { return elements == 0; }
unsigned int capacity(void) const { return arr_max; }
unsigned int size(void) const { return elements; }
const M& heapmap(void) const { return map; }
const T& peek(unsigned int i=0) const { return array[i]; }
bool insert(T& element) {
if (arr_max == elements) return false;
unsigned int k = elements++;
map.update(element, k);
array[k] = element;
percolate_up(k);
return true;
}
unsigned int mass_insert(T copy[], unsigned int n) {
unsigned int i = 0;
for( ; i < n ; i++) if (!insert(copy[i])) break;
return i;
}
bool delete_min(void) {
if (elements == 0) return false;
map.update(array[0], arr_max+1);
array[0] = array[--elements];
map.update(array[0], 0);
percolate_down();
return true;
}
bool delete_element(unsigned int i) {
if (i > elements) return false;
map.update(array[i], arr_max+1);
T temp = array[i];
array[i] = array[--elements];
map.update(array[i], i);
if (array[i] > temp) percolate_down(i);
else if (temp > array[i]) percolate_up(i);
return true;
}
bool update(unsigned int i, T& element) {
if (i > elements) return false;
map.update(array[i], arr_max+1);
T temp = array[i];
array[i] = element;
map.update(array[i], i);
if (array[i] > temp) percolate_down(i);
else if (temp > array[i]) percolate_up(i);
return true;
}
// void print() { using namespace std; for (unsigned int i=0 ; i < elements ; i++) cout << array[i] << " | "; cout << endl; }
// Iterators
/*
friend class Const_Iterator;
class Const_Iterator {
MinHeap<T>* heap;
unsigned int index;
Const_Iterator(MinHeap<T>* h, unsigned int i) : heap(h), index(i) {}
public:
Const_Iterator(const Const_Iterator& clone) : heap(clone.heap), index(clone.index) {}
friend Const_Iterator MinHeap<T>::begin(void);
};
Const_Iterator begin(void) { return Const_Iterator(this, 0); }
*/
};
//////////////////////////////////////////////////////////////////////////////
//////////////////////// MinHeap without Map for Data ////////////////////////
template <class T> class MinHeap <T, int> {
T* array;
unsigned const int arr_max;
unsigned int elements;
void percolate_down(unsigned int i=0) {
unsigned int n = elements-1, min;
do {
unsigned int l = 2*i + 1, r = 2*i + 2;
if (l <= n && array[i] > array[l]) min = l;
else min = i;
if (r <= n && array[i] > array[r] && array[l] > array[r]) min = r;
if (i != min) {
T temp = array[i];
array[i] = array[min];
array[min] = temp;
i = min;
} else break;
} while (i < n);
}
void percolate_up(unsigned int i) {
while (i && array[(i-1)/2] > array[i]) {
T temp = array[i];
array[i] = array[(i-1)/2];
array[(i-1)/2] = temp;
i = (i-1)/2;
}
}
public:
MinHeap(const int max) : array(new T[max]), arr_max(max), elements(0) {}
~MinHeap(void) { delete[] array; }
bool empty(void) const { return elements == 0; }
unsigned int capacity(void) const { return arr_max; }
unsigned int size(void) const { return elements; }
const T& peek(unsigned int i=0) const { return array[i]; }
bool insert(T& element) {
if (arr_max == elements) return false;
unsigned int k = elements++;
array[k] = element;
percolate_up(k);
return true;
}
unsigned int mass_insert(T copy[], unsigned int n) {
unsigned int i = 0;
for( ; i < n ; i++) if (!insert(copy[i])) break;
return i;
}
bool delete_min(void) {
if (elements == 0) return false;
array[0] = array[--elements];
percolate_down();
return true;
}
bool delete_element(unsigned int i) {
if (i > elements) return false;
T temp = array[i];
array[i] = array[--elements];
if (array[i] > temp) percolate_down(i);
else if (temp > array[i]) percolate_up(i);
return true;
}
bool update(unsigned int i, T& element) {
if (i > elements) return false;
T temp = array[i];
array[i] = element;
if (array[i] > temp) percolate_down(i);
else if (temp > array[i]) percolate_up(i);
return true;
}
// void print() { using namespace std; for (unsigned int i=0 ; i < elements ; i++) cout << array[i] << " | "; cout << endl; }
};
//////////////////////////////////////////////////////////////////////////////
#endif // MINHEAP_H