Fix infeasible start condition in 3D

In 3d, we have to do the check for all of the obstacle points being on
the wrong side of the normal plane *after* we find the closest point
using the QP
Remove duplicate c-space obstacle points
add some slack tolerance
Sample from the computed C-space region and display the result