Xiang Ma, David A. Castañón: Numerical algorithms for continuous optimal trajectories. CDC 2004: 5427-5432