WIP: Occupied point in kdtree