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; } // METHOD // Anitescu-Tasora projection on cone generator and polar cone // (contractive, but performs correction on three components: normal,u,v) double f_n = this.l_i + this.Constraint2TuplesNall.GetCohesion(); double f_u = constraint_U.Get_l_i(); double f_v = constraint_V.Get_l_i(); double f_tang = Math.Sqrt(f_v * f_v + f_u * f_u); // shortcut if (Constraint2TuplesNall.GetFrictionCoefficient() == 0) { constraint_U.Set_l_i(0); constraint_V.Set_l_i(0); if (f_n < 0) { this.Set_l_i(0); } return; } // inside upper cone? keep untouched! double fcoif = Constraint2TuplesNall.GetFrictionCoefficient(); if (f_tang < Constraint2TuplesNall.GetFrictionCoefficient() * f_n) { return; } // inside lower cone? reset normal,u,v to zero! if ((f_tang < -(1.0 / Constraint2TuplesNall.GetFrictionCoefficient()) * f_n) || (Math.Abs(f_n) < 10e-15)) { double f_n_proj2 = 0; double f_u_proj2 = 0; double f_v_proj2 = 0; this.Set_l_i(f_n_proj2); constraint_U.Set_l_i(f_u_proj2); constraint_V.Set_l_i(f_v_proj2); return; } // remaining case: project orthogonally to generator segment of upper cone double f_n_proj = (f_tang * Constraint2TuplesNall.GetFrictionCoefficient() + f_n) / (Constraint2TuplesNall.GetFrictionCoefficient() * Constraint2TuplesNall.GetFrictionCoefficient() + 1); double f_tang_proj = f_n_proj * Constraint2TuplesNall.GetFrictionCoefficient(); double tproj_div_t = f_tang_proj / f_tang; double f_u_proj = tproj_div_t * f_u; double f_v_proj = tproj_div_t * f_v; this.Set_l_i(f_n_proj - this.Constraint2TuplesNall.GetCohesion()); constraint_U.Set_l_i(f_u_proj); constraint_V.Set_l_i(f_v_proj); }