Stable door pass

New algorith for door approaching and passing.