//
// FUNCTIONS
//
/// Initialize again this constraint.
public virtual void Reset(Ta mobjA, //< ChContactable object A
Tb mobjB, //< ChContactable object B
collision.ChCollisionInfo cinfo //< data for the contact pair
)
{
//Debug.Assert(mobjA);
//Debug.Assert(mobjB);
this.objA = mobjA;
this.objB = mobjB;
this.p1 = cinfo.vpA;
this.p2 = cinfo.vpB;
this.normal = cinfo.vN;
this.norm_dist = cinfo.distance;
this.eff_radius = cinfo.eff_radius;
// Contact plane
ChVector Vx = new ChVector(0, 0, 0);
ChVector Vy = new ChVector(0, 0, 0);
ChVector Vz = new ChVector(0, 0, 0);
ChVector.XdirToDxDyDz(normal, ChVector.VECT_Y, ref Vx, ref Vy, ref Vz);
contact_plane.Set_A_axis(Vx, Vy, Vz);
}
public ChContactSMC(ChContactContainer mcontainer, //< contact container
Ta mobjA, //< collidable object A
Tb mobjB, //< collidable object B
collision.ChCollisionInfo cinfo //< data for the contact pair
)
: base(mcontainer, mobjA, mobjB, cinfo)
{
// m_Jac = null;
Reset(mobjA, mobjB, cinfo);
}
public ChContactNSC(ChContactContainer mcontainer, //< contact container
Ta mobjA, //< collidable object A
Tb mobjB, //< collidable object B
collision.ChCollisionInfo cinfo //< data for the contact pair
)
: base(mcontainer, mobjA, mobjB, cinfo)
{
Nx.SetTangentialConstraintU(Tu);
Nx.SetTangentialConstraintV(Tv);
Reset(mobjA, mobjB, cinfo);
}
public ChContactNSCrolling(ChContactContainer mcontainer, //< contact container
Ta mobjA, //< collidable object A
Tb mobjB, //< collidable object B
collision.ChCollisionInfo cinfo //< data for the contact pair
)
: base(mcontainer, mobjA, mobjB, cinfo)
{
Rx.SetRollingConstraintU(this.Ru);
Rx.SetRollingConstraintV(this.Rv);
Rx.SetNormalConstraint(this.Nx);
Reset(mobjA, mobjB, cinfo);
}
/// Add a contact between two frames.
public override void AddContact(collision.ChCollisionInfo mcontact)
{
// Debug.Assert(mcontact.modelA.GetContactable() != null);
// Debug.Assert(mcontact.modelB.GetContactable() != null);
var contactableA = mcontact.modelA.GetContactable();
var contactableB = mcontact.modelB.GetContactable();
// Check that the two collision models are compatible with penalty contact.
// If either one has a contact material for complementarity, skip processing this contact.
var mmatA = (ChMaterialSurfaceSMC)(contactableA.GetMaterialSurfaceBase());
var mmatB = (ChMaterialSurfaceSMC)(contactableB.GetMaterialSurfaceBase());
if (!mmatA || !mmatB)
{
return;
}
// Bail out if any of the two contactable objects is
// not contact-active:
bool inactiveA = !contactableA.IsContactActive();
bool inactiveB = !contactableB.IsContactActive();
if ((inactiveA && inactiveB))
{
return;
}
// CREATE THE CONTACTS
//
// Switch among the various cases of contacts: i.e. between a 6-dof variable and another 6-dof variable,
// or 6 vs 3, etc.
// These cases are made distinct to exploit the optimization coming from templates and static data sizes
// in contact types.
ChBody mmboA = (ChBody)contactableA;
ChBody mmboB = (ChBody)contactableB;
/* if ((mmatA.rolling_friction != 0 && mmatB.rolling_friction != 0) ||
* (mmatA.spinning_friction != 0 && mmatB.spinning_friction != 0))
* {
* _OptimalContactInsert(ref contactlist_6_6_rolling, ref lastcontact_6_6_rolling, ref n_added_6_6_rolling, this,
* mmboA, mmboB, mcontact);
* }
* else
* {*/
_OptimalContactInsert(ref contactlist_6_6, ref lastcontact_6_6, ref n_added_6_6, this, mmboA, mmboB, mcontact);
// }
}
public ChContactTuple(ChContactContainer mcontainer, //< contact container
Ta mobjA, //< ChContactable object A
Tb mobjB, //< ChContactable object B
collision.ChCollisionInfo cinfo //< data for the contact pair
)
{
// Debug.Assert(mcontainer != null);
// Debug.Assert(mobjA);
// Debug.Assert(mobjB);
container = mcontainer;
Reset(mobjA, mobjB, cinfo);
}
/// 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);
}
}
public void _OptimalContactInsert <Ta, Tb, Tcont, Titer>(ref List <Tcont> contactlist, //< contact list
ref IEnumerator <Titer> lastcontact, //< last contact acquired
// ref Titer lastcontact, //< last contact acquired
// ref IEnumerator<ChContactNSC<ChContactable_1vars<IntInterface.Six>, ChContactable_1vars<IntInterface.Six>>> lastcontact,
ref int n_added, //< number of contact inserted
ChContactContainer mcontainer, //< contact container
Ta objA, //< collidable object A
Tb objB, //< collidable object B
collision.ChCollisionInfo cinfo //< collision informations
)
// where Titer: object
//
where Tcont : ChContactSMC <ChContactable_1vars <IntInterface.Six>, ChContactable_1vars <IntInterface.Six> >
where Titer : ChContactSMC <ChContactable_1vars <IntInterface.Six>, ChContactable_1vars <IntInterface.Six> >
where Ta : ChContactable_1vars <IntInterface.Six>
where Tb : ChContactable_1vars <IntInterface.Six>
{
if (/*lastcontact != null && */ lastcontact.Current != contactlist.LastOrDefault())
{
// reuse old contacts
lastcontact.MoveNext();
((IEnumerator <ChContactSMC <ChContactable_1vars <IntInterface.Six>, ChContactable_1vars <IntInterface.Six> > >)lastcontact).Current.Reset(objA, objB, cinfo);
}
else
{
ChContactSMC <ChContactable_1vars <IntInterface.Six>, ChContactable_1vars <IntInterface.Six> > mc = new ChContactSMC <ChContactable_1vars <IntInterface.Six>, ChContactable_1vars <IntInterface.Six> >(mcontainer, objA, objB, cinfo);
Tcont coont = (Tcont)mc;
contactlist.Add(coont);
// move to last element in list
IEnumerator <Tcont> iter = contactlist.GetEnumerator();
for (int i = 0; i < contactlist.Count; i++)
{
iter.MoveNext();
}
lastcontact = (IEnumerator <Titer>)iter;
// lastcontact = (dynamic)contactlist.GetEnumerator();
}
n_added++;
}
/// Initialize again this constraint.
public override void Reset(Ta mobjA,
Tb mobjB,
collision.ChCollisionInfo cinfo)
{
// Base method call:
base.Reset(mobjA, mobjB, cinfo);
ChBody oA = (this.objA as ChBody);
ChBody oB = (this.objB as ChBody);
Rx.Get_tuple_a().SetVariables(ref this.objA);
Rx.Get_tuple_b().SetVariables(ref this.objB);
Ru.Get_tuple_a().SetVariables(ref this.objA);
Ru.Get_tuple_b().SetVariables(ref this.objB);
Rv.Get_tuple_a().SetVariables(ref this.objA);
Rv.Get_tuple_b().SetVariables(ref this.objB);
// Calculate composite material properties
ChMaterialCompositeNSC mat = new ChMaterialCompositeNSC(
this.container.GetSystem().composition_strategy,
(ChMaterialSurfaceNSC)oA.GetMaterialSurfaceBase(),
(ChMaterialSurfaceNSC)oB.GetMaterialSurfaceBase());
Rx.SetRollingFrictionCoefficient(mat.rolling_friction);
Rx.SetSpinningFrictionCoefficient(mat.spinning_friction);
this.complianceRoll = mat.complianceRoll;
this.complianceSpin = mat.complianceSpin;
// COMPUTE JACOBIANS
// delegate objA to compute its half of jacobian
oA.ComputeJacobianForRollingContactPart(this.p1, ref this.contact_plane, ref Rx.Get_tuple_a(),
ref Ru.Get_tuple_a(), ref Rv.Get_tuple_a(), false);
// delegate objB to compute its half of jacobian
oB.ComputeJacobianForRollingContactPart(this.p2, ref this.contact_plane, ref Rx.Get_tuple_b(),
ref Ru.Get_tuple_b(), ref Rv.Get_tuple_b(), true);
this.react_torque = ChVector.VNULL;
}
/// 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);
ChBody oA = (this.objA as ChBody);
ChBody oB = (this.objB as ChBody);
Nx.Get_tuple_a().SetVariables(ref this.objA);
Nx.Get_tuple_b().SetVariables(ref this.objB);
Tu.Get_tuple_a().SetVariables(ref this.objA);
Tu.Get_tuple_b().SetVariables(ref this.objB);
Tv.Get_tuple_a().SetVariables(ref this.objA);
Tv.Get_tuple_b().SetVariables(ref this.objB);
// Calculate composite material properties
ChMaterialCompositeNSC mat = new ChMaterialCompositeNSC(
this.container.GetSystem().composition_strategy,
(ChMaterialSurfaceNSC)(oA.GetMaterialSurfaceBase()),
(ChMaterialSurfaceNSC)(oB.GetMaterialSurfaceBase()));
// Check for a user-provided callback to modify the material
if (this.container.GetAddContactCallback() != null)
{
this.container.GetAddContactCallback().OnAddContact(cinfo, mat);
}
Nx.Constraint2TuplesNall.SetFrictionCoefficient(mat.static_friction);
Nx.Constraint2TuplesNall.SetCohesion(mat.cohesion);
this.restitution = mat.restitution;
this.dampingf = mat.dampingf;
this.compliance = mat.compliance;
this.complianceT = mat.complianceT;
this.reactions_cache = cinfo.reaction_cache;
// COMPUTE JACOBIANS
// delegate objA to compute its half of jacobian
oA.ComputeJacobianForContactPart(this.p1, ref this.contact_plane, ref Nx.Get_tuple_a(), ref Tu.Get_tuple_a(),
ref Tv.Get_tuple_a(), false);
// delegate objB to compute its half of jacobian
oB.ComputeJacobianForContactPart(this.p2, ref this.contact_plane, ref Nx.Get_tuple_b(), ref Tu.Get_tuple_b(),
ref Tv.Get_tuple_b(), true);
if (reactions_cache != null)
{
react_force.x = reactions_cache[0];
react_force.y = reactions_cache[1];
react_force.z = reactions_cache[2];
//GetLog() << "Reset Fn=" << (double)reactions_cache[0] << " at cache address:" << (int)this->reactions_cache << "\n";
}
else
{
react_force = new ChVector(0, 0, 0);
}
}
/// Callback used to process contact points being added to the container.
/// A derived user-provided callback class must implement this. The provided
/// composite material should be downcast to the appropriate type.
public abstract void OnAddContact(
collision.ChCollisionInfo contactinfo, //< information about the collision pair
ChMaterialComposite material //< composite material can be modified
);
/// Add a contact between two models, storing it into this container. /// To be implemented by child classes. /// Some specialized child classes (ex. one that uses GPU buffers) /// could implement also other more efficient functions to add many contacts /// in a batch (so that, for example, a special GPU collision system can exploit it); /// yet most collision system might still fall back to this function if no other /// specialized add-functions are found. public abstract void AddContact(collision.ChCollisionInfo mcontact);
