Three-Dimensional Trajectory Optimization In Constrained Airspace