2D orthogonal range searching in C++