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