Re-implemented optimizeTrajectory() with continous isPathValid() collision checker