Emergency navigation, energy optimisation and cooperative algorithms for motion and evacuation