Various Small Changes to SPARS implementation

I'm not sure what the rule is on suggesting cosmetic changes to other's algorithms, I won't be offended if these are not good ideas.

- Made clearQuery() also clear solutions paths - this is the most important change. This function claims it allows you to use the same sparse graph for different planning problems, but it neglects to remove past solutions from the ProblemDefinition
- Added debug output function
- Made reachedFailureLimits() test public so that the graph can be pre-generated without a plan and still have a stopping condition