/// Compute all forces in a contiguous array.
/// Used in finite-difference Jacobian approximation.
public void CalculateQ(ChState stateA_x, //< state positions for objA
ChStateDelta stateA_w, //< state velocities for objA
ChState stateB_x, //< state positions for objB
ChStateDelta stateB_w, //< state velocities for objB
ChMaterialCompositeSMC mat, //< composite material for contact pair
ref ChVectorDynamic <double> Q //< output generalized forces
)
{
ChBody oA = (this.objA as ChBody);
ChBody oB = (this.objB as ChBody);
// Express contact points in local frames.
// We assume that these points remain fixed to their respective contactable objects.
ChVector p1_loc = oA.GetCsysForCollisionModel().TransformPointParentToLocal(this.p1);
ChVector p2_loc = oB.GetCsysForCollisionModel().TransformPointParentToLocal(this.p2);
// Express the local points in global frame
ChVector p1_abs = oA.GetContactPoint(p1_loc, stateA_x);
ChVector p2_abs = oB.GetContactPoint(p2_loc, stateB_x);
/*
* Note: while this can be somewhat justified for a ChBody, it will not work
* for a mesh vertex for instance...
*
* // Project the points onto the unperturbed normal line
* p1_abs = this.p1 + Vdot(p1_abs - this.p1, this.normal) * this.normal;
* p2_abs = this.p2 + Vdot(p2_abs - this.p2, this.normal) * this.normal;
*/
// Calculate normal direction (expressed in global frame)
ChVector normal_dir = (p1_abs - p2_abs).GetNormalized();
// Calculate penetration depth
double delta = (p1_abs - p2_abs).Length();
// If the normal direction flipped sign, change sign of delta
if (ChVector.Vdot(normal_dir, this.normal) < 0)
{
delta = -delta;
}
// Calculate velocity of contact points (expressed in global frame)
ChVector vel1 = oA.GetContactPointSpeed(p1_loc, stateA_x, stateA_w);
ChVector vel2 = oB.GetContactPointSpeed(p2_loc, stateB_x, stateB_w);
// Compute the contact force.
ChVector force = CalculateForce(delta, normal_dir, vel1, vel2, mat);
// Compute and load the generalized contact forces.
oA.ContactForceLoadQ(-force, p1_abs, stateA_x, ref Q, 0);
oB.ContactForceLoadQ(force, p2_abs, stateB_x, ref Q, oA.ContactableGet_ndof_w());
}
/// 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);
}
}