added kdtree

created simple kdtree template class to drastically speed things up