I have implemented a constraint dynamics physics simulation as proposed by Andrew Witkin et al 1990, but I cannot get the initial constraint “snapping” correctly.

I implemented JWJ^T λ = −Jq − JWQ − k_s C − k_d C as:

# f = dJ @ dq + J @ W @ Q + ks * C + kd * dC
# g = J @ W @ J.T
f, g, J, c = SimulationFunctions.matrices(self.ks, self.kd, self.particles, self.constraints)

# Solve for λ in g λ = -f, minimizing ||g λ + f||, where f = dJ dq + J W Q + ks C + kd dC and g = J W J.T

r: scipy.optimize.OptimizeResult = scipy.optimize.least_squares(lambda l: g @ l + f, np.zeros_like(f),
                                                                jac=lambda _: g, method='trf')
l: np.ndarray = r.x
self.error = f"constraint {c} solve {np.linalg.norm(g * l + f)}"

I want to replace the “ks * C + kd * dC” term with a better cost that allows the system to smoothly transition into satisfying the constraint, like a “critically damped system”. I also get cases where the system simply cannot reach the constraint and explodes or cases where the constraint is satisfied but acts as a spring, causing the system to accelerate.

How should I go about getting a “critically damped system” behaviour when the constraint is not satisfied?

My code is here https://github.com/EmmanuelMess/Interactive-Dynamics-Physics-Simulations/tree/master.