Algorithms for Nonlinear Optimal Control Problems Based on the First and Second Order Necessary Conditions