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. /// \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); }