How to do the feasible check between two configurations? The Visible(p,q),a short segment of a path between p,q.