improve self collision algorithm