Fixed some minor bugs in the kd tree to get it working