Working kdtree. On to optimization