fixes and optimizations in Rkdtree.cpp