public void ShurBvectorCompute(ref ChSystemDescriptor sysd) { // ***TO DO*** move the following thirty lines in a short function ChSystemDescriptor::ShurBvectorCompute() ? // Compute the b_shur vector in the Shur complement equation N*l = b_shur // with // N_shur = D'* (M^-1) * D // b_shur = - c + D'*(M^-1)*k = b_i + D'*(M^-1)*k // but flipping the sign of lambdas, b_shur = - b_i - D'*(M^-1)*k // Do this in three steps: // Put (M^-1)*k in q sparse vector of each variable.. for (int iv = 0; iv < sysd.GetVariablesList().Count; iv++) { if (sysd.GetVariablesList()[iv].IsActive()) { sysd.GetVariablesList()[iv].Compute_invMb_v(sysd.GetVariablesList()[iv].Get_qb().matrix, sysd.GetVariablesList()[iv].Get_fb().matrix); // q = [M]'*fb } } // ...and now do b_shur = - D'*q = - D'*(M^-1)*k .. int s_i = 0; for (int ic = 0; ic < sysd.GetConstraintsList().Count; ic++) { if (sysd.GetConstraintsList()[ic].IsActive()) { r.matrix[s_i, 0] = sysd.GetConstraintsList()[ic].Compute_Cq_q(); ++s_i; } } // ..and finally do b_shur = b_shur - c sysd.BuildBiVector(tmp.matrix); // b_i = -c = phi/h r.matrix.MatrInc(tmp.matrix); }
/// Performs the solution of the problem. /// \return the maximum constraint violation after termination. public override double Solve(ref ChSystemDescriptor sysd //< system description with constraints and variables ) { List <ChConstraint> mconstraints = sysd.GetConstraintsList(); List <ChVariables> mvariables = sysd.GetVariablesList(); // 1) Update auxiliary data in all constraints before starting, // that is: g_i=[Cq_i]*[invM_i]*[Cq_i]' and [Eq_i]=[invM_i]*[Cq_i]' for (int ic = 0; ic < mconstraints.Count; ic++) { mconstraints[ic].Update_auxiliary(); } // 2) Compute, for all items with variables, the initial guess for // still unconstrained system: for (int iv = 0; iv < mvariables.Count; iv++) { if (mvariables[iv].IsActive()) { mvariables[iv].Compute_invMb_v(mvariables[iv].Get_qb().matrix, mvariables[iv].Get_fb().matrix); // q = [M]'*fb } } // 3) For all items with variables, add the effect of initial (guessed) // lagrangian reactions of constraints, if a warm start is desired. // Otherwise, if no warm start, simply resets initial lagrangians to zero. if (warm_start) { for (int ic = 0; ic < mconstraints.Count; ic++) { if (mconstraints[ic].IsActive()) { mconstraints[ic].Increment_q(mconstraints[ic].Get_l_i()); } } } else { for (int ic = 0; ic < mconstraints.Count; ic++) { mconstraints[ic].Set_l_i(0.0); } } // 4) Perform the iteration loops (if there are any constraints) double maxviolation = 0.0; double maxdeltalambda = 0.0; if (mconstraints.Count == 0) { return(maxviolation); } for (int iter = 0; iter < max_iterations; iter++) { maxviolation = 0; maxdeltalambda = 0; // The iteration on all constraints for (int ic = 0; ic < mconstraints.Count; ic++) { // skip computations if constraint not active. if (mconstraints[ic].IsActive()) { // compute residual c_i = [Cq_i]*q + b_i double mresidual = mconstraints[ic].Compute_Cq_q() + mconstraints[ic].Get_b_i(); // true constraint violation may be different from 'mresidual' (ex:clamped if unilateral) double candidate_violation = Math.Abs(mconstraints[ic].Violation(mresidual)); // compute: delta_lambda = -(omega/g_i) * ([Cq_i]*q + b_i ) double deltal = (omega / mconstraints[ic].Get_g_i()) * (-mresidual); // update: lambda += delta_lambda; double old_lambda = mconstraints[ic].Get_l_i(); mconstraints[ic].Set_l_i(old_lambda + deltal); // If new lagrangian multiplier does not satisfy inequalities, project // it into an admissible orthant (or, in general, onto an admissible set) mconstraints[ic].Project(); // After projection, the lambda may have changed a bit.. double new_lambda = mconstraints[ic].Get_l_i(); // Apply the smoothing: lambda= sharpness*lambda_new_projected + (1-sharpness)*lambda_old if (this.shlambda != 1.0) { new_lambda = shlambda * new_lambda + (1.0 - shlambda) * old_lambda; mconstraints[ic].Set_l_i(new_lambda); } double true_delta = new_lambda - old_lambda; // For all items with variables, add the effect of incremented // (and projected) lagrangian reactions: mconstraints[ic].Increment_q(true_delta); if (this.record_violation_history) { maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta)); } maxviolation = ChMaths.ChMax(maxviolation, Math.Abs(candidate_violation)); } // end IsActive() } // end loop on constraints // For recording into violation history, if debugging if (this.record_violation_history) { AtIterationEnd(maxviolation, maxdeltalambda, iter); } // Terminate the loop if violation in constraints has been successfully limited. if (maxviolation < tolerance) { break; } } // end iteration loop return(maxviolation); }
/// Performs the solution of the problem. public override double Solve(ref ChSystemDescriptor sysd) { bool verbose = false; List <ChConstraint> mconstraints = sysd.GetConstraintsList(); // List<ChVariables> mvariables;// = sysd.GetVariablesList(); // if (verbose) // Debug.Log("Number of constraints: " + mconstraints.Count + "nNumber of variables :" + mvariables.Count); // Update auxiliary data in all constraints before starting, // that is: g_i=[Cq_i]*[invM_i]*[Cq_i]' and [Eq_i]=[invM_i]*[Cq_i]' for (int ic = 0; ic < mconstraints.Count; ic++) { mconstraints[ic].Update_auxiliary(); } double L, t; double theta; double thetaNew; double Beta; double obj1, obj2; nc = sysd.CountActiveConstraints(); gamma_hat.Resize(nc, 1); gammaNew.Resize(nc, 1); g.Resize(nc, 1); y.Resize(nc, 1); gamma.Resize(nc, 1); yNew.Resize(nc, 1); r.Resize(nc, 1); tmp.Resize(nc, 1); residual = 10e30; Beta = 0.0; obj1 = 0.0; obj2 = 0.0; // Compute the b_shur vector in the Shur complement equation N*l = b_shur ShurBvectorCompute(ref sysd); // Optimization: backup the q sparse data computed above, // because (M^-1)*k will be needed at the end when computing primals. ChMatrixDynamic <double> Minvk = new ChMatrixDynamic <double>(); sysd.FromVariablesToVector(Minvk.matrix, true); // (1) gamma_0 = zeros(nc,1) if (warm_start) { for (int ic = 0; ic < mconstraints.Count; ic++) { if (mconstraints[ic].IsActive()) { mconstraints[ic].Increment_q(mconstraints[ic].Get_l_i()); } } } else { for (int ic = 0; ic < mconstraints.Count; ic++) { mconstraints[ic].Set_l_i(0.0); } } sysd.FromConstraintsToVector(gamma.matrix); // (2) gamma_hat_0 = ones(nc,1) gamma_hat.matrix.FillElem(1); // (3) y_0 = gamma_0 y.matrix.CopyFromMatrix(gamma.matrix); // (4) theta_0 = 1 theta = 1.0; // (5) L_k = norm(N * (gamma_0 - gamma_hat_0)) / norm(gamma_0 - gamma_hat_0) tmp.matrix.MatrSub(gamma.matrix, gamma_hat.matrix); L = tmp.matrix.NormTwo(); sysd.ShurComplementProduct(yNew.matrix, tmp.matrix); L = yNew.matrix.NormTwo() / L; yNew.matrix.FillElem(0); // reset yNew to be all zeros // (6) t_k = 1 / L_k t = 1.0 / L; // (7) for k := 0 to N_max for (tot_iterations = 0; tot_iterations < max_iterations; tot_iterations++) { // (8) g = N * y_k - r sysd.ShurComplementProduct(g.matrix, y.matrix); g.matrix.MatrInc(r.matrix); // (9) gamma_(k+1) = ProjectionOperator(y_k - t_k * g) gammaNew.matrix.CopyFromMatrix(g.matrix); gammaNew.matrix.MatrScale(-t); gammaNew.matrix.MatrInc(y.matrix); sysd.ConstraintsProject(gammaNew.matrix); // (10) while 0.5 * gamma_(k+1)' * N * gamma_(k+1) - gamma_(k+1)' * r >= 0.5 * y_k' * N * y_k - y_k' * r + g' * // (gamma_(k+1) - y_k) + 0.5 * L_k * norm(gamma_(k+1) - y_k)^2 sysd.ShurComplementProduct(tmp.matrix, gammaNew.matrix); // Here tmp is equal to N*gammaNew; tmp.matrix.MatrScale(0.5); tmp.matrix.MatrInc(r.matrix); obj1 = tmp.matrix.MatrDot(gammaNew.matrix, tmp.matrix); sysd.ShurComplementProduct(tmp.matrix, y.matrix); // Here tmp is equal to N*y; tmp.matrix.MatrScale(0.5); tmp.matrix.MatrInc(r.matrix); obj2 = tmp.matrix.MatrDot(y.matrix, tmp.matrix); tmp.matrix.MatrSub(gammaNew.matrix, y.matrix); // Here tmp is equal to gammaNew - y obj2 = obj2 + tmp.matrix.MatrDot(tmp.matrix, g.matrix) + 0.5 * L * tmp.matrix.MatrDot(tmp.matrix, tmp.matrix); while (obj1 >= obj2) { // (11) L_k = 2 * L_k L = 2.0 * L; // (12) t_k = 1 / L_k t = 1.0 / L; // (13) gamma_(k+1) = ProjectionOperator(y_k - t_k * g) gammaNew.matrix.CopyFromMatrix(g.matrix); gammaNew.matrix.MatrScale(-t); gammaNew.matrix.MatrInc(y.matrix); sysd.ConstraintsProject(gammaNew.matrix); // Update the components of the while condition sysd.ShurComplementProduct(tmp.matrix, gammaNew.matrix); // Here tmp is equal to N*gammaNew; tmp.matrix.MatrScale(0.5); tmp.matrix.MatrInc(r.matrix); obj1 = tmp.matrix.MatrDot(gammaNew.matrix, tmp.matrix); sysd.ShurComplementProduct(tmp.matrix, y.matrix); // Here tmp is equal to N*y; tmp.matrix.MatrScale(0.5); tmp.matrix.MatrInc(r.matrix); obj2 = tmp.matrix.MatrDot(y.matrix, tmp.matrix); tmp.matrix.MatrSub(gammaNew.matrix, y.matrix); // Here tmp is equal to gammaNew - y obj2 = obj2 + tmp.matrix.MatrDot(tmp.matrix, g.matrix) + 0.5 * L * tmp.matrix.MatrDot(tmp.matrix, tmp.matrix); // (14) endwhile } // (15) theta_(k+1) = (-theta_k^2 + theta_k * sqrt(theta_k^2 + 4)) / 2 thetaNew = (-Math.Pow(theta, 2.0) + theta * Math.Sqrt(Math.Pow(theta, 2.0) + 4.0)) / 2.0; // (16) Beta_(k+1) = theta_k * (1 - theta_k) / (theta_k^2 + theta_(k+1)) Beta = theta * (1.0 - theta) / (Math.Pow(theta, 2) + thetaNew); // (17) y_(k+1) = gamma_(k+1) + Beta_(k+1) * (gamma_(k+1) - gamma_k) tmp.matrix.MatrSub(gammaNew.matrix, gamma.matrix); // Here tmp is equal to gammaNew - gamma; tmp.matrix.MatrScale(Beta); yNew.matrix.MatrAdd(gammaNew.matrix, tmp.matrix); // (18) r = r(gamma_(k+1)) double res = Res4(ref sysd); // (19) if r < epsilon_min if (res < residual) { // (20) r_min = r residual = res; // (21) gamma_hat = gamma_(k+1) gamma_hat.matrix.CopyFromMatrix(gammaNew.matrix); // (22) endif } // (23) if r < Tau if (residual < this.tolerance) { // (24) break break; // (25) endif } // (26) if g' * (gamma_(k+1) - gamma_k) > 0 tmp.matrix.MatrSub(gammaNew.matrix, gamma.matrix); if (tmp.matrix.MatrDot(tmp.matrix, g.matrix) > 0) { // (27) y_(k+1) = gamma_(k+1) yNew.matrix.CopyFromMatrix(gammaNew.matrix); // (28) theta_(k+1) = 1 thetaNew = 1.0; // (29) endif } // (30) L_k = 0.9 * L_k L = 0.9 * L; // (31) t_k = 1 / L_k t = 1.0 / L; // perform some tasks at the end of the iteration /* if (this.record_violation_history) * { * tmp.MatrSub(gammaNew, gamma); * AtIterationEnd(residual, tmp.NormInf(), tot_iterations); * }*/ // Update iterates theta = thetaNew; gamma.matrix.CopyFromMatrix(gammaNew.matrix); y.matrix.CopyFromMatrix(yNew.matrix); // (32) endfor } // if (verbose) // Debug.Log("Residual: " + residual + ", Iter: " + tot_iterations); // (33) return Value at time step t_(l+1), gamma_(l+1) := gamma_hat sysd.FromVectorToConstraints(gamma_hat.matrix); // Resulting PRIMAL variables: // compute the primal variables as v = (M^-1)(k + D*l) // v = (M^-1)*k ... (by rewinding to the backup vector computed at the beginning) sysd.FromVectorToVariables(Minvk.matrix); // PROBLEM slow! // ... + (M^-1)*D*l (this increment and also stores 'qb' in the ChVariable items) for (int ic = 0; ic < mconstraints.Count; ic++) { if (mconstraints[ic].IsActive()) { mconstraints[ic].Increment_q(mconstraints[ic].Get_l_i()); } } return(residual); }
/// Performs the solution of the problem. /// \return the maximum constraint violation after termination. public override double Solve(ref ChSystemDescriptor sysd //< system description with constraints and variables ) { List <ChConstraint> mconstraints = sysd.GetConstraintsList(); List <ChVariables> mvariables = sysd.GetVariablesList(); double maxviolation = 0.0; double maxdeltalambda = 0.0; int i_friction_comp = 0; double[] old_lambda_friction = new double[3]; int nConstr = mconstraints.Count; int nVars = mvariables.Count; // 1) Update auxiliary data in all constraints before starting, // that is: g_i=[Cq_i]*[invM_i]*[Cq_i]' and [Eq_i]=[invM_i]*[Cq_i]' for (int ic = 0; ic < nConstr; ic++) { mconstraints[ic].Update_auxiliary(); } // Average all g_i for the triplet of contact constraints n,u,v. int j_friction_comp = 0; double[] gi_values = new double[3]; for (int ic = 0; ic < nConstr; ic++) { if (mconstraints[ic].GetMode() == eChConstraintMode.CONSTRAINT_FRIC) { gi_values[j_friction_comp] = mconstraints[ic].Get_g_i(); j_friction_comp++; if (j_friction_comp == 3) { double average_g_i = (gi_values[0] + gi_values[1] + gi_values[2]) / 3.0; mconstraints[ic - 2].Set_g_i(average_g_i); mconstraints[ic - 1].Set_g_i(average_g_i); mconstraints[ic - 0].Set_g_i(average_g_i); j_friction_comp = 0; } } } // 2) Compute, for all items with variables, the initial guess for // still unconstrained system: for (int iv = 0; iv < nVars; iv++) { if (mvariables[iv].IsActive()) { mvariables[iv].Compute_invMb_v(mvariables[iv].Get_qb().matrix, mvariables[iv].Get_fb().matrix); // q = [M]'*fb } } // 3) For all items with variables, add the effect of initial (guessed) // lagrangian reactions of constraints, if a warm start is desired. // Otherwise, if no warm start, simply resets initial lagrangians to zero. if (warm_start) { for (int ic = 0; ic < nConstr; ic++) { if (mconstraints[ic].IsActive()) { mconstraints[ic].Increment_q(mconstraints[ic].Get_l_i()); } } } else { for (int ic = 0; ic < nConstr; ic++) { mconstraints[ic].Set_l_i(0.0); } } // 4) Perform the iteration loops for (int iter = 0; iter < max_iterations;) { // // Forward sweep, for symmetric SOR // maxviolation = 0; maxdeltalambda = 0; i_friction_comp = 0; int dummy = mconstraints.Count; for (int ic = 0; ic < dummy; ic++) { // skip computations if constraint not active. if (mconstraints[ic].IsActive()) { // compute residual c_i = [Cq_i]*q + b_i + cfm_i*l_i double mresidual = mconstraints[ic].Compute_Cq_q() + mconstraints[ic].Get_b_i() + mconstraints[ic].Get_cfm_i() * mconstraints[ic].Get_l_i(); // true constraint violation may be different from 'mresidual' (ex:clamped if unilateral) double candidate_violation = Math.Abs(mconstraints[ic].Violation(mresidual)); // compute: delta_lambda = -(omega/g_i) * ([Cq_i]*q + b_i + cfm_i*l_i ) double deltal = (omega / mconstraints[ic].Get_g_i()) * (-mresidual); if (mconstraints[ic].GetMode() == eChConstraintMode.CONSTRAINT_FRIC) { candidate_violation = 0; // update: lambda += delta_lambda; old_lambda_friction[i_friction_comp] = mconstraints[ic].Get_l_i(); mconstraints[ic].Set_l_i(old_lambda_friction[i_friction_comp] + deltal); i_friction_comp++; if (i_friction_comp == 1) { candidate_violation = Math.Abs(ChMaths.ChMin(0.0, mresidual)); } if (i_friction_comp == 3) { mconstraints[ic - 2].Project(); // the N normal component will take care of N,U,V double new_lambda_0 = mconstraints[ic - 2].Get_l_i(); double new_lambda_1 = mconstraints[ic - 1].Get_l_i(); double new_lambda_2 = mconstraints[ic - 0].Get_l_i(); // Apply the smoothing: lambda= sharpness*lambda_new_projected + (1-sharpness)*lambda_old if (this.shlambda != 1.0) { new_lambda_0 = shlambda * new_lambda_0 + (1.0 - shlambda) * old_lambda_friction[0]; new_lambda_1 = shlambda * new_lambda_1 + (1.0 - shlambda) * old_lambda_friction[1]; new_lambda_2 = shlambda * new_lambda_2 + (1.0 - shlambda) * old_lambda_friction[2]; mconstraints[ic - 2].Set_l_i(new_lambda_0); mconstraints[ic - 1].Set_l_i(new_lambda_1); mconstraints[ic - 0].Set_l_i(new_lambda_2); } double true_delta_0 = new_lambda_0 - old_lambda_friction[0]; double true_delta_1 = new_lambda_1 - old_lambda_friction[1]; double true_delta_2 = new_lambda_2 - old_lambda_friction[2]; mconstraints[ic - 2].Increment_q(true_delta_0); mconstraints[ic - 1].Increment_q(true_delta_1); mconstraints[ic - 0].Increment_q(true_delta_2); if (this.record_violation_history) { maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta_0)); maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta_1)); maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta_2)); } i_friction_comp = 0; } } else { // update: lambda += delta_lambda; double old_lambda = mconstraints[ic].Get_l_i(); mconstraints[ic].Set_l_i(old_lambda + deltal); // If new lagrangian multiplier does not satisfy inequalities, project // it into an admissible orthant (or, in general, onto an admissible set) mconstraints[ic].Project(); // After projection, the lambda may have changed a bit.. double new_lambda = mconstraints[ic].Get_l_i(); // Apply the smoothing: lambda= sharpness*lambda_new_projected + (1-sharpness)*lambda_old if (this.shlambda != 1.0) { new_lambda = shlambda * new_lambda + (1.0 - shlambda) * old_lambda; mconstraints[ic].Set_l_i(new_lambda); } double true_delta = new_lambda - old_lambda; // For all items with variables, add the effect of incremented // (and projected) lagrangian reactions: mconstraints[ic].Increment_q(true_delta); if (this.record_violation_history) { maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta)); } } maxviolation = ChMaths.ChMax(maxviolation, Math.Abs(candidate_violation)); } // end IsActive() } // end constraint loop // Terminate the loop if violation in constraints has been successfully limited. // if (maxviolation < tolerance) // break; // For recording into violation history, if debugging if (this.record_violation_history) { AtIterationEnd(maxviolation, maxdeltalambda, iter); } // Increment iter count (each sweep, either forward or backward, is considered // as a complete iteration, to be fair when comparing to the non-symmetric SOR :) iter++; // // Backward sweep, for symmetric SOR // maxviolation = 0.0; maxdeltalambda = 0.0; i_friction_comp = 0; for (int ic = (nConstr - 1); ic >= 0; ic--) { // skip computations if constraint not active. if (mconstraints[ic].IsActive()) { // compute residual c_i = [Cq_i]*q + b_i + cfm_i*l_i double mresidual = mconstraints[ic].Compute_Cq_q() + mconstraints[ic].Get_b_i() + mconstraints[ic].Get_cfm_i() * mconstraints[ic].Get_l_i(); // true constraint violation may be different from 'mresidual' (ex:clamped if unilateral) double candidate_violation = Math.Abs(mconstraints[ic].Violation(mresidual)); // compute: delta_lambda = -(omega/g_i) * ([Cq_i]*q + b_i + cfm_i*l_i ) double deltal = (omega / mconstraints[ic].Get_g_i()) * (-mresidual); if (mconstraints[ic].GetMode() == eChConstraintMode.CONSTRAINT_FRIC) { candidate_violation = 0; // update: lambda += delta_lambda; old_lambda_friction[i_friction_comp] = mconstraints[ic].Get_l_i(); mconstraints[ic].Set_l_i(old_lambda_friction[i_friction_comp] + deltal); i_friction_comp++; if (i_friction_comp == 3) { mconstraints[ic].Project(); // the N normal component will take care of N,U,V double new_lambda_0 = mconstraints[ic + 2].Get_l_i(); double new_lambda_1 = mconstraints[ic + 1].Get_l_i(); double new_lambda_2 = mconstraints[ic + 0].Get_l_i(); // Apply the smoothing: lambda= sharpness*lambda_new_projected + (1-sharpness)*lambda_old if (this.shlambda != 1.0) { new_lambda_0 = shlambda * new_lambda_0 + (1.0 - shlambda) * old_lambda_friction[0]; new_lambda_1 = shlambda * new_lambda_1 + (1.0 - shlambda) * old_lambda_friction[1]; new_lambda_2 = shlambda * new_lambda_2 + (1.0 - shlambda) * old_lambda_friction[2]; mconstraints[ic + 2].Set_l_i(new_lambda_0); mconstraints[ic + 1].Set_l_i(new_lambda_1); mconstraints[ic + 0].Set_l_i(new_lambda_2); } double true_delta_0 = new_lambda_0 - old_lambda_friction[0]; double true_delta_1 = new_lambda_1 - old_lambda_friction[1]; double true_delta_2 = new_lambda_2 - old_lambda_friction[2]; mconstraints[ic + 2].Increment_q(true_delta_0); mconstraints[ic + 1].Increment_q(true_delta_1); mconstraints[ic + 0].Increment_q(true_delta_2); candidate_violation = Math.Abs(ChMaths.ChMin(0.0, mresidual)); if (this.record_violation_history) { maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta_0)); maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta_1)); maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta_2)); } i_friction_comp = 0; } } else { // update: lambda += delta_lambda; double old_lambda = mconstraints[ic].Get_l_i(); mconstraints[ic].Set_l_i(old_lambda + deltal); // If new lagrangian multiplier does not satisfy inequalities, project // it into an admissible orthant (or, in general, onto an admissible set) mconstraints[ic].Project(); // After projection, the lambda may have changed a bit.. double new_lambda = mconstraints[ic].Get_l_i(); // Apply the smoothing: lambda= sharpness*lambda_new_projected + (1-sharpness)*lambda_old if (this.shlambda != 1.0) { new_lambda = shlambda * new_lambda + (1.0 - shlambda) * old_lambda; mconstraints[ic].Set_l_i(new_lambda); } double true_delta = new_lambda - old_lambda; // For all items with variables, add the effect of incremented // (and projected) lagrangian reactions: mconstraints[ic].Increment_q(true_delta); if (this.record_violation_history) { maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta)); } } maxviolation = ChMaths.ChMax(maxviolation, Math.Abs(candidate_violation)); } // end IsActive() } // end loop on constraints // For recording into violation history, if debugging if (this.record_violation_history) { AtIterationEnd(maxviolation, maxdeltalambda, iter); } // Terminate the loop if violation in constraints has been successfully limited. if (maxviolation < tolerance) { break; } iter++; } return(maxviolation); }