Got KdTree collision mostly working. Still need to fix a bug. Saving progress.