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);
}
