public override void ContIntToDescriptor(int off_L, ChVectorDynamic <double> L, ChVectorDynamic <double> Qc ) { // only for solver warm start Nx.Set_l_i(L.matrix[off_L]); Tu.Set_l_i(L.matrix[off_L + 1]); Tv.Set_l_i(L.matrix[off_L + 2]); // solver known terms Nx.Set_b_i(Qc.matrix[off_L]); Tu.Set_b_i(Qc.matrix[off_L + 1]); Tv.Set_b_i(Qc.matrix[off_L + 2]); }
/// For iterative solvers: project the value of a possible /// 'l_i' value of constraint reaction onto admissible set. /// This projection will also modify the l_i values of the two /// tangential friction constraints (projection onto the friction cone, /// as by Anitescu-Tasora theory). public override void Project() { if (constraint_U == null) { return; } if (constraint_V == null) { return; } if (constraint_N == null) { return; } // METHOD // Anitescu-Tasora projection on rolling-friction cone generator and polar cone // (contractive, but performs correction on three components: normal,u,v) double f_n = constraint_N.Get_l_i(); double t_n = this.Get_l_i(); double t_u = constraint_U.Get_l_i(); double t_v = constraint_V.Get_l_i(); double t_tang = Math.Sqrt(t_v * t_v + t_u * t_u); double t_sptang = Math.Abs(t_n); // = sqrt(t_n*t_n); // A. Project the spinning friction (approximate - should do cone // projection stuff as in B, but spinning friction is usually very low...) if (spinningfriction != 0) { if (t_sptang < spinningfriction * f_n) { // inside upper cone? keep untouched! } else { // inside lower cone? reset normal,u,v to zero! if ((t_sptang < -(1.0 / spinningfriction) * f_n) || (Math.Abs(f_n) < 10e-15)) { constraint_N.Set_l_i(0); this.Set_l_i(0); } else { // remaining case: project orthogonally to generator segment of upper cone (CAN BE simplified) double f_n_proj = (t_sptang * spinningfriction + f_n) / (spinningfriction * spinningfriction + 1); double t_tang_proj = f_n_proj * spinningfriction; double tproj_div_t = t_tang_proj / t_sptang; double t_n_proj = tproj_div_t * t_n; constraint_N.Set_l_i(f_n_proj); this.Set_l_i(t_n_proj); } } } // B. Project the rolling friction // shortcut if (rollingfriction == 0) { constraint_U.Set_l_i(0); constraint_V.Set_l_i(0); if (f_n < 0) { constraint_N.Set_l_i(0); } return; } // inside upper cone? keep untouched! if (t_tang < rollingfriction * f_n) { return; } // inside lower cone? reset normal,u,v to zero! if ((t_tang < -(1.0 / rollingfriction) * f_n) || (Math.Abs(f_n) < 10e-15)) { double f_n_proj2 = 0; double t_u_proj2 = 0; double t_v_proj2 = 0; constraint_N.Set_l_i(f_n_proj2); constraint_U.Set_l_i(t_u_proj2); constraint_V.Set_l_i(t_v_proj2); return; } // remaining case: project orthogonally to generator segment of upper cone double f_n_proj3 = (t_tang * rollingfriction + f_n) / (rollingfriction * rollingfriction + 1); double t_tang_proj3 = f_n_proj3 * rollingfriction; double tproj_div_t3 = t_tang_proj3 / t_tang; double t_u_proj = tproj_div_t3 * t_u; double t_v_proj = tproj_div_t3 * t_v; constraint_N.Set_l_i(f_n_proj3); constraint_U.Set_l_i(t_u_proj); constraint_V.Set_l_i(t_v_proj); }