Fixing portions of kdtree implementation - creation and some traversals