This version is executing plan in simulation correctly. Octree update is not correct though. This would still be correct search, but quite ineeficient because a track of free and occupied voxels is not happening correctly.