Source: https://www2.ift.ulaval.ca/~pgiguere/cours/IntroRobotique/notes/10-PlanifA.pdf