C++ implementation of k-dimensional tree