Fix constraint error correction in soft body simulation