Function adjust_complementarity_diagonal 

pub fn adjust_complementarity_diagonal(
    jac: &mut [Vec<f64>],
    h: &[f64],
    n: usize,
)

Adjust the complementarity diagonal block with the rhs h.

Call after compute_kkt_jacobian to set diag(Gx - h) correctly.