Feasible Direction Algorithm for Optimal Control Problems with State and Control Constraints: Implementation