Untidy Fast Marching Method added