Adding logic for leaving a floor

Maths for finding parameters of next decision point