PWTO: A Heuristic Approach for Trajectory Optimization in Complex Terrains