Better handling of infeasible start condition

When d is inside the obstacle, we can detect this by looking at the
result of the QP (if norm(yi) == 0). Rather than worrying about finding
a perfectly feasible plane, just find a plane that will push d away from
the obstacle, and everything should work out okay
Fix number of samples