Fix PlanAndOptimize class

  - remote method tryDirectPath that has been moved to PathPlanner,
  - implement startSolve and call internal PathPlanner implementation.
Add documentation
Include missing header.
Provide method to add path planner and path optimizer types in problem solver.
Add a reverse method in Path.
Store weak pointer to itself in PathPlanner class.
Added new connected components list to store information about reachable connected components
Addition of updateCCReachability function to adapt the roadmap to state space planning