Fixed a bug when "going somewhere" not from the origin

Signed-off-by: Pier-Luc Plante <pier-luc.plante.1@ulaval.ca>