C++ templated KD-Tree implementation