Path optimization algorithm based on collision constraints, automatically built between bodies close to collision.