A Simple KD-Tree in C++