Post-stabilization for rigid body simulation with contact and constraints