/// Initialize again this constraint.
public override void Reset(Ta mobjA, //< collidable object A
Tb mobjB, //< collidable object B
collision.ChCollisionInfo cinfo //< data for the contact pair
) //where T1: IntInterface.IBase
{
// Inherit base class.
base.Reset(mobjA, mobjB, cinfo);
// Note: cinfo.distance is the same as this.norm_dist.
// Debug.Assert(cinfo.distance < 0);
ChBody oA = (this.objA as ChBody);
ChBody oB = (this.objB as ChBody);
// Calculate composite material properties
ChMaterialCompositeSMC mat = new ChMaterialCompositeSMC(
this.container.GetSystem().composition_strategy,
(ChMaterialSurfaceSMC)(oA.GetMaterialSurfaceBase()),
(ChMaterialSurfaceSMC)(oB.GetMaterialSurfaceBase()));
// Check for a user-provided callback to modify the material
if (this.container.GetAddContactCallback() != null)
{
this.container.GetAddContactCallback().OnAddContact(cinfo, mat);
}
// Calculate contact force.
m_force = CalculateForce(-this.norm_dist, // overlap (here, always positive)
this.normal, // normal contact direction
oA.GetContactPointSpeed(this.p1), // velocity of contact point on objA
oB.GetContactPointSpeed(this.p2), // velocity of contact point on objB
mat // composite material for contact pair
);
ChSystemSMC smc = (ChSystemSMC)this.container.GetSystem();
// Set up and compute Jacobian matrices.
if (smc.GetStiffContact() == true)
{
CreateJacobians();
CalculateJacobians(mat);
}
}
/// Calculate contact force, expressed in absolute coordinates.
public ChVector CalculateForce(
double delta, //< overlap in normal direction
ChVector normal_dir, //< normal contact direction (expressed in global frame)
ChVector vel1, //< velocity of contact point on objA (expressed in global frame)
ChVector vel2, //< velocity of contact point on objB (expressed in global frame)
ChMaterialCompositeSMC mat //< composite material for contact pair
)
{
// Set contact force to zero if no penetration.
if (delta <= 0)
{
return(new ChVector(0, 0, 0));
}
// Extract parameters from containing system
ChSystemSMC sys = (ChSystemSMC)(this.container.GetSystem());
double dT = sys.GetStep();
bool use_mat_props = sys.UsingMaterialProperties();
ChSystemSMC.ContactForceModel contact_model = sys.GetContactForceModel();
ChSystemSMC.AdhesionForceModel adhesion_model = sys.GetAdhesionForceModel();
ChSystemSMC.TangentialDisplacementModel tdispl_model = sys.GetTangentialDisplacementModel();
// Relative velocity at contact
ChVector relvel = vel2 - vel1;
double relvel_n_mag = relvel.Dot(normal_dir);
ChVector relvel_n = relvel_n_mag * normal_dir;
ChVector relvel_t = relvel - relvel_n;
double relvel_t_mag = relvel_t.Length();
ChBody oA = (this.objA as ChBody);
ChBody oB = (this.objB as ChBody);
// Calculate effective mass
double eff_mass = oA.GetContactableMass() * oB.GetContactableMass() /
(oA.GetContactableMass() + oB.GetContactableMass());
// Calculate stiffness and viscous damping coefficients.
// All models use the following formulas for normal and tangential forces:
// Fn = kn * delta_n - gn * v_n
// Ft = kt * delta_t - gt * v_t
double kn = 0;
double kt = 0;
double gn = 0;
double gt = 0;
double eps = double.Epsilon;
switch (contact_model)
{
case ChSystemSMC.ContactForceModel.Hooke:
if (use_mat_props)
{
double tmp_k = (16.0 / 15) * Math.Sqrt(this.eff_radius) * mat.E_eff;
double v2 = sys.GetCharacteristicImpactVelocity() * sys.GetCharacteristicImpactVelocity();
double loge = (mat.cr_eff < eps) ? Math.Log(eps) : Math.Log(mat.cr_eff);
loge = (mat.cr_eff > 1 - eps) ? Math.Log(1 - eps) : loge;
double tmp_g = 1 + Math.Pow(ChMaths.CH_C_PI / loge, 2);
kn = tmp_k * Math.Pow(eff_mass * v2 / tmp_k, 1.0 / 5);
kt = kn;
gn = Math.Sqrt(4 * eff_mass * kn / tmp_g);
gt = gn;
}
else
{
kn = mat.kn;
kt = mat.kt;
gn = eff_mass * mat.gn;
gt = eff_mass * mat.gt;
}
break;
case ChSystemSMC.ContactForceModel.Hertz:
if (use_mat_props)
{
double sqrt_Rd = Math.Sqrt(this.eff_radius * delta);
double Sn = 2 * mat.E_eff * sqrt_Rd;
double St = 8 * mat.G_eff * sqrt_Rd;
double loge = (mat.cr_eff < eps) ? Math.Log(eps) : Math.Log(mat.cr_eff);
double beta = loge / Math.Sqrt(loge * loge + ChMaths.CH_C_PI * ChMaths.CH_C_PI);
kn = (2.0 / 3) * Sn;
kt = St;
gn = -2 * Math.Sqrt(5.0 / 6) * beta * Math.Sqrt(Sn * eff_mass);
gt = -2 * Math.Sqrt(5.0 / 6) * beta * Math.Sqrt(St * eff_mass);
}
else
{
double tmp = this.eff_radius * Math.Sqrt(delta);
kn = tmp * mat.kn;
kt = tmp * mat.kt;
gn = tmp * eff_mass * mat.gn;
gt = tmp * eff_mass * mat.gt;
}
break;
case ChSystemSMC.ContactForceModel.PlainCoulomb:
if (use_mat_props)
{
double sqrt_Rd = Math.Sqrt(delta);
double Sn = 2 * mat.E_eff * sqrt_Rd;
double St = 8 * mat.G_eff * sqrt_Rd;
double loge = (mat.cr_eff < eps) ? Math.Log(eps) : Math.Log(mat.cr_eff);
double beta = loge / Math.Sqrt(loge * loge + ChMaths.CH_C_PI * ChMaths.CH_C_PI);
kn = (2.0 / 3) * Sn;
gn = -2 * Math.Sqrt(5.0 / 6) * beta * Math.Sqrt(Sn * eff_mass);
}
else
{
double tmp = Math.Sqrt(delta);
kn = tmp * mat.kn;
gn = tmp * mat.gn;
}
kt = 0;
gt = 0;
{
double forceNb = kn * delta - gn * relvel_n_mag;
if (forceNb < 0)
{
forceNb = 0;
}
double forceTb = mat.mu_eff * Math.Tanh(5.0 * relvel_t_mag) * forceNb;
switch (adhesion_model)
{
case ChSystemSMC.AdhesionForceModel.Constant:
forceNb -= mat.adhesion_eff;
break;
case ChSystemSMC.AdhesionForceModel.DMT:
forceNb -= mat.adhesionMultDMT_eff * Math.Sqrt(this.eff_radius);
break;
}
ChVector forceb = forceNb * normal_dir;
if (relvel_t_mag >= sys.GetSlipVelocityThreshold())
{
forceb -= (forceTb / relvel_t_mag) * relvel_t;
}
return(forceb);
}
}
// Tangential displacement (magnitude)
double delta_t = 0;
switch (tdispl_model)
{
case ChSystemSMC.TangentialDisplacementModel.OneStep:
delta_t = relvel_t_mag * dT;
break;
case ChSystemSMC.TangentialDisplacementModel.MultiStep:
//// TODO: implement proper MultiStep mode
delta_t = relvel_t_mag * dT;
break;
default:
break;
}
// Calculate the magnitudes of the normal and tangential contact forces
double forceN = kn * delta - gn * relvel_n_mag;
double forceT = kt * delta_t + gt * relvel_t_mag;
// If the resulting normal contact force is negative, the two shapes are moving
// away from each other so fast that no contact force is generated.
if (forceN < 0)
{
forceN = 0;
forceT = 0;
}
// Include adhesion force
switch (adhesion_model)
{
case ChSystemSMC.AdhesionForceModel.Constant:
forceN -= mat.adhesion_eff;
break;
case ChSystemSMC.AdhesionForceModel.DMT:
forceN -= mat.adhesionMultDMT_eff * Math.Sqrt(this.eff_radius);
break;
}
// Coulomb law
forceT = Math.Min(forceT, mat.mu_eff * Math.Abs(forceN));
// Accumulate normal and tangential forces
ChVector force = forceN * normal_dir;
if (relvel_t_mag >= sys.GetSlipVelocityThreshold())
{
force -= (forceT / relvel_t_mag) * relvel_t;
}
return(force);
}
