Function add_constraint_lagrange 

pub fn add_constraint_lagrange(
    geometry: &Geometry,
    forces: DVector<f64>,
    constraints: &[Constraint],
    lambdas: &mut Vec<f64>,
) -> Result<(DVector<f64>, DVector<f64>), ConstraintError>

Applies constraint forces using the Lagrange multiplier method.

This function implements compatible constraint handling by:

1. First step: Diagonal λ initialization: λ = - (c·g)/(c·c)
2. Subsequent steps: Reuse previous λ values
3. Apply constraint forces: F_new = F_old + C^T * λ
4. Return violations for extended gradient optimization

Arguments

• geometry - Current molecular geometry
• forces - Original forces from QM calculation (negative gradient)
• constraints - List of geometric constraints to enforce
• lambdas - Mutable reference to Lagrange multipliers (updated in-place)

Returns

Returns a tuple of (modified_forces, violations) where:

• modified_forces: Forces with constraint contributions (F + Cᵀλ)
• violations: Current constraint violation values for extended gradient

The constraint equation solved is: C * C^T * λ = -g(x) where C is the Jacobian, λ are the multipliers, and g(x) are violations.

The modified forces are: F_new = F_old + C^T * λ

Arguments

• geometry - Current molecular geometry
• forces - Original forces from QM calculation (negative gradient)
• constraints - List of geometric constraints to enforce
• lambdas - Mutable reference to Lagrange multipliers (updated in-place)

Returns