Constraint algorithm (mechanics)