Fast numerical solution of optimal control problems for switched systems: An application to path planning.