Removed useless assertion.
Prevent penetration from being fully corrected by the solver.
Refactoring of CSO sempling function and epsilon constrants.