public override void ContIntLoadConstraint_C(int off_L,
ref ChVectorDynamic <double> Qc,
double c,
bool do_clamp,
double recovery_clamp
)
{
bool bounced = false;
// Elastic Restitution model (use simple Newton model with coefficient e=v(+)/v(-))
// Note that this works only if the two connected items are two ChBody.
if (this.objA != null && this.objB != null)
{
var oA = (ChBody)(object)this.objA;
var oB = (ChBody)(object)this.objB;
if (this.restitution != 0)
{
// compute normal rebounce speed
ChVector V1_w = oA.GetContactPointSpeed(this.p1);
ChVector V2_w = oB.GetContactPointSpeed(this.p2);
ChVector Vrel_w = V2_w - V1_w;
ChVector Vrel_cplane = this.contact_plane.MatrT_x_Vect(Vrel_w);
double h = this.container.GetSystem().GetStep(); // = 1.0 / c; // not all steppers have c = 1/h
double neg_rebounce_speed = Vrel_cplane.x * this.restitution;
if (neg_rebounce_speed < -this.container.GetSystem().GetMinBounceSpeed())
{
if (this.norm_dist + neg_rebounce_speed * h < 0)
{
// CASE: BOUNCE
bounced = true;
Qc.matrix[off_L] += neg_rebounce_speed;
}
}
}
}
if (!bounced)
{
// CASE: SETTLE (most often, and also default if two colliding items are not two ChBody)
if (this.compliance != 0)
{
double h = 1.0 / c; // was: this->container->GetSystem()->GetStep(); note not all steppers have c = 1/h
double alpha = this.dampingf; // [R]=alpha*[K]
double inv_hpa = 1.0 / (h + alpha); // 1/(h+a)
double inv_hhpa = 1.0 / (h * (h + alpha)); // 1/(h*(h+a))
//***TODO*** move to KRMmatricesLoad() the following, and only for !bounced case
Nx.Set_cfm_i((inv_hhpa) * this.compliance);
Tu.Set_cfm_i((inv_hhpa) * this.complianceT);
Tv.Set_cfm_i((inv_hhpa) * this.complianceT);
double qc = inv_hpa * this.norm_dist; //***TODO*** see how to move this in KRMmatricesLoad()
// Note: clamping of Qc in case of compliance is questionable: it does not limit only the outbound
// speed, but also the reaction, so it might allow longer 'sinking' not related to the real compliance.
// I.e. If clamping kicks in (when using large timesteps and low compliance), it acts as a numerical damping.
if (do_clamp)
{
qc = ChMaths.ChMax(qc, -recovery_clamp);
}
Qc.matrix[off_L] += qc;
}
else
{
if (do_clamp)
{
if (this.Nx.Constraint2TuplesNall.GetCohesion() != 0)
{
Qc.matrix[off_L] += ChMaths.ChMin(0.0, ChMaths.ChMax(c * this.norm_dist, -recovery_clamp));
}
else
{
Qc.matrix[off_L] += ChMaths.ChMax(c * this.norm_dist, -recovery_clamp);
}
}
else
{
Qc.matrix[off_L] += c * this.norm_dist;
}
}
}
}
