public double Res4(ref ChSystemDescriptor sysd) { // Project the gradient (for rollback strategy) // g_proj = (l-project_orthogonal(l - gdiff*g, fric))/gdiff; double gdiff = 1.0 / Math.Pow(nc, 2.0); sysd.ShurComplementProduct(tmp.matrix, gammaNew.matrix); tmp.matrix.MatrInc(r.matrix); tmp.matrix.MatrScale(-gdiff); tmp.matrix.MatrInc(gammaNew.matrix); sysd.ConstraintsProject(tmp.matrix); tmp.matrix.MatrSub(gammaNew.matrix, tmp.matrix); tmp.matrix.MatrScale(1.0 / gdiff); return(tmp.matrix.NormTwo()); }
/// 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); }