public ChShaftsBody(ChShaftsBody other)
{
torque_react = other.torque_react;
shaft_dir = other.shaft_dir;
shaft = null;
body = null;
}
public ChMarker(string name,
ChBody body,
ChCoordsys rel_pos,
ChCoordsys rel_pos_dt,
ChCoordsys rel_pos_dtdt)
{
// SetNameString(name);
Body = body;
motion_X = new ChFunction_Const(0); // default: no motion
motion_Y = new ChFunction_Const(0);
motion_Z = new ChFunction_Const(0);
motion_ang = new ChFunction_Const(0);
motion_axis = ChVector.VECT_Z;
rest_coord = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0));//ChCoordsys.CSYSNORM;
motion_type = eChMarkerMotion.M_MOTION_FUNCTIONS;
FrameMoving.SetCoord(rel_pos);
FrameMoving.SetCoord_dt(rel_pos_dt);
FrameMoving.SetCoord_dtdt(rel_pos_dtdt);
last_rel_coord = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0)); //ChCoordsys.CSYSNORM;
last_rel_coord_dt = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(0, 0, 0, 0)); // ChCoordsys.CSYSNULL;
last_time = 0;
UpdateState();
}
/// Specialized initialization for springs, given the two bodies to be connected,
/// the positions of the two anchor endpoints of the spring (each expressed
/// in body or abs. coordinates) and the imposed rest length of the spring.
/// NOTE! As in ChLinkMarkers::Initialize(), the two markers are automatically
/// created and placed inside the two connected bodies.
public void Initialize(
ChBody mbody1, //< first body to link
ChBody mbody2, //< second body to link
bool pos_are_relative, //< true: following pos. are relative to bodies
ChVector mpos1, //< position of spring endpoint, for 1st body (rel. or abs., see flag above)
ChVector mpos2, //< position of spring endpoint, for 2nd body (rel. or abs., see flag above)
bool auto_rest_length = true, //< if true, initializes the rest-length as the distance between mpos1 and mpos2
double mrest_length = 0 //< imposed rest_length (no need to define, if auto_rest_length=true.)
)
{
// First, initialize as all constraint with markers.
// In this case, create the two markers also!.
base.Initialize(mbody1, mbody2, new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0)));
if (pos_are_relative)
{
marker1.Impose_Rel_Coord(new ChCoordsys(mpos1, new ChQuaternion(1, 0, 0, 0)));
marker2.Impose_Rel_Coord(new ChCoordsys(mpos2, new ChQuaternion(1, 0, 0, 0)));
}
else
{
marker1.Impose_Abs_Coord(new ChCoordsys(mpos1, new ChQuaternion(1, 0, 0, 0)));
marker2.Impose_Abs_Coord(new ChCoordsys(mpos2, new ChQuaternion(1, 0, 0, 0)));
}
ChVector AbsDist = marker1.GetAbsCoord().pos - marker2.GetAbsCoord().pos;
dist = AbsDist.Length();
spr_restlength = auto_rest_length ? dist : mrest_length;
}
/// Use this function after link creation, to initialize the link from
/// two joined rigid bodies.
/// Both rigid bodies must belong to the same ChSystem.
/// Two markers will be created and added to the rigid bodies (later,
/// you can use GetMarker1() and GetMarker2() to access them.
/// To specify the (absolute) position of link and markers, use 'mpos'.
public virtual void Initialize(ChBody mbody1, //< first body to join
ChBody mbody2, //< second body to join
ChCoordsys mpos //< the current absolute pos.& alignment.
)
{
Initialize(mbody1, mbody2, false, mpos, mpos);
}
// Do not add the same item multiple times; also, do not remove items which haven't ever been added!
// This will most often cause an assert() failure in debug mode.
// Note. adding/removing items to the assembly doesn't call Update() automatically.
/// Attach a body to this assembly.
public virtual void AddBody(ChBody body)
{
// Debug.Assert(std::find(std::begin(bodylist), std::end(bodylist), body) == bodylist.end());
// Debug.Assert(bodylist[i].GetSystem() == null); // should remove from other system before adding here
// set system and also add collision models to system
body.SetSystem(system);
bodylist.Add(body);
}
/// 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());
}
/// 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);
// }
}
/// Apply contact forces to the two objects.
/// (new version, for interfacing to ChTimestepper and ChIntegrable)
public override void ContIntLoadResidual_F(ref ChVectorDynamic <double> R, double c)
{
ChVector abs_force_scaled = new ChVector(m_force * c);
ChBody oA = (this.objA as ChBody);
ChBody oB = (this.objB as ChBody);
if (oA.IsContactActive())
{
oA.ContactForceLoadResidual_F(-abs_force_scaled, this.p1, ref R);
}
if (oB.IsContactActive())
{
oB.ContactForceLoadResidual_F(abs_force_scaled, this.p2, ref R);
}
}
private ChFrameMoving <double> abs_frame = new ChFrameMoving <double>(); //< absolute frame position
/* public void Start()
* {
* FrameMoving = new ChFrameMoving<double>();
* Body = null;
* rest_coord = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0));//ChCoordsys.CSYSNORM;
* motion_type = eChMarkerMotion.M_MOTION_FUNCTIONS;
* motion_axis = ChVector.VECT_Z;
* last_rel_coord = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0));//ChCoordsys.CSYSNORM;
* last_rel_coord_dt = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(0, 0, 0, 0));// ChCoordsys.CSYSNULL;
* last_time = 0;
* motion_X = new ChFunction_Const(0); // default: no motion
* motion_Y = new ChFunction_Const(0);
* motion_Z = new ChFunction_Const(0);
* motion_ang = new ChFunction_Const(0);
*
* UpdateState();
* }*/
public ChMarker()
{
FrameMoving = new ChFrameMoving <double>();
Body = null;
rest_coord = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0));//ChCoordsys.CSYSNORM;
motion_type = eChMarkerMotion.M_MOTION_FUNCTIONS;
motion_axis = ChVector.VECT_Z;
last_rel_coord = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0)); //ChCoordsys.CSYSNORM;
last_rel_coord_dt = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(0, 0, 0, 0)); // ChCoordsys.CSYSNULL;
last_time = 0;
motion_X = new ChFunction_Const(0); // default: no motion
motion_Y = new ChFunction_Const(0);
motion_Z = new ChFunction_Const(0);
motion_ang = new ChFunction_Const(0);
UpdateState();
}
/// 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);
}
}
/// Attach an arbitrary ChPhysicsItem (e.g. ChBody, ChParticles, ChLink, etc.) to the assembly.
/// It will take care of adding it to the proper list of bodies, links, meshes, or generic
/// physic item. (i.e. it calls AddBody(), AddLink(), AddMesh(), or AddOtherPhysicsItem()).
/// Note, you cannot call Add() during an Update (i.e. items like particle generators that
/// are already inserted in the assembly cannot call this) because not thread safe; instead,
/// use AddBatch().
public void Add(ChPhysicsItem item)
{
//ChBody body = new ChBody();
System.Type body = typeof(ChBody);
if (body.Equals(item))
{
ChBody b = (ChBody)item;
AddBody(b);
return;
}
System.Type link = typeof(ChLink);
if (link.Equals(item))
{
ChLink l = (ChLink)item;
AddLink(l);
return;
}
AddOtherPhysicsItem(item);
}
/// 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;
}
/// Create the Jacobian matrices.
/// These matrices are created/resized as needed.
public void CreateJacobians()
{
// m_Jac = null;
m_Jac = new ChContactJacobian();
ChBody oA = (this.objA as ChBody);
ChBody oB = (this.objB as ChBody);
// Set variables and resize Jacobian matrices.
// NOTE: currently, only contactable objects derived from ChContactable_1vars<6>,
// ChContactable_1vars<3>, and ChContactable_3vars<3,3,3> are supported.
int ndof_w = 0;
List <ChVariables> vars = new List <ChVariables>();
// ChContactable_3vars<IntInterface.Three, IntInterface.Three, IntInterface.Three> contactableA;
// ChContactTuple<IntInterface.Three, IntInterface.Three> objA_333 = (this.objA as (ChBody)(ChContactable_3vars<IntInterface.Three, IntInterface.Three, IntInterface.Three>));
vars.Add(oA.GetVariables1());
//ChContactable_3vars<IntInterface.Three, IntInterface.Three, IntInterface.Three>> objA_333;// = new ChBody();
/*if (objA_333 == this.objA as ChContactable_3vars<IntInterface.Three, IntInterface.Three, IntInterface.Three>){ }
* vars.Add(objA_333.GetVariables2());
* vars.Add(objA_333.GetVariables3());
* }*/
ndof_w += oA.ContactableGet_ndof_w();
vars.Add(oB.GetVariables1());
/* if (auto objB_333 = dynamic_cast < ChContactable_3vars < 3, 3, 3 > *> (this.objB)) {
* vars.push_back(objB_333.GetVariables2());
* vars.push_back(objB_333.GetVariables3());
* }*/
ndof_w += oB.ContactableGet_ndof_w();
m_Jac.m_KRM.SetVariables(vars);
m_Jac.m_K.matrix.Reset(ndof_w, ndof_w);
m_Jac.m_R.matrix.Reset(ndof_w, ndof_w);
Debug.Assert(m_Jac.m_KRM.Get_K().GetColumns() == ndof_w);
}
public ChMarker(ChMarker other) : base(other)
{
FrameMoving = new ChFrameMoving <double>(other.FrameMoving);
Body = null;
motion_X = new ChFunction(other.motion_X.Clone());
motion_Y = new ChFunction(other.motion_Y.Clone());
motion_Z = new ChFunction(other.motion_Z.Clone());
motion_ang = new ChFunction(other.motion_ang.Clone());
motion_axis = other.motion_axis;
rest_coord = other.rest_coord;
motion_type = other.motion_type;
abs_frame = other.abs_frame;
last_rel_coord = other.last_rel_coord;
last_rel_coord_dt = other.last_rel_coord_dt;
last_time = other.last_time;
}
private ChMatrixDynamic <double> Qf = new ChMatrixDynamic <double>(); //< Lagrangian force
public ChForce()
{
Body = null;
vpoint = new ChVector(0, 0, 0);
vrelpoint = new ChVector(0, 0, 0);
force = new ChVector(0, 0, 0);
relforce = new ChVector(0, 0, 0);
vdir = ChVector.VECT_X;
vreldir = ChVector.VECT_X;
restpos = new ChVector(0, 0, 0);
mforce = 0;
align = AlignmentFrame.BODY_DIR;
frame = ReferenceFrame.BODY;
mode = ForceType.FORCE;
Qf = new ChMatrixDynamic <double>(7, 1);
modula = new ChFunction_Const(1);
move_x = new ChFunction_Const(0);
move_y = new ChFunction_Const(0);
move_z = new ChFunction_Const(0);
f_x = new ChFunction_Const(0);
f_y = new ChFunction_Const(0);
f_z = new ChFunction_Const(0);
}
/// Use this function after link creation, to initialize the link from
/// two joined rigid bodies.
/// Both rigid bodies must belong to the same ChSystem.
/// Two markers will be created and added to the rigid bodies (later,
/// you can use GetMarker1() and GetMarker2() to access them.
/// To specify the (absolute) position of link and markers, use 'mpos'.
public virtual void Initialize(
ChBody mbody1, //< first body to join
ChBody mbody2, //< second body to join
bool pos_are_relative, //< if =true, following two positions are relative to bodies. If false, are absolute.
ChCoordsys mpos1, //< the position & alignment of 1st marker (relative to body1 cords, or absolute)
ChCoordsys mpos2 //< the position & alignment of 2nd marker (relative to body2 cords, or absolute)
)
{
// Debug.Assert(mbody1 != mbody2);
// Debug.Assert(mbody1.GetSystem() == mbody2.GetSystem());
// create markers to add to the two bodies
//ChMarker mmark1 = new ChMarker(new ChMarker());
//ChMarker mmark2 = new ChMarker(new ChMarker());
ChMarker mmark1 = gameObject.AddComponent <ChMarker>();
ChMarker mmark2 = gameObject.AddComponent <ChMarker>();
mbody1.AddMarker(mmark1);
mbody2.AddMarker(mmark2);
ChMarker mm1 = mmark1;
ChMarker mm2 = mmark2;
ReferenceMarkers(mm1, mm2);
if (pos_are_relative)
{
mmark1.Impose_Rel_Coord(mpos1);
mmark2.Impose_Rel_Coord(mpos2);
}
else
{
mmark1.Impose_Abs_Coord(mpos1);
mmark2.Impose_Abs_Coord(mpos2);
}
}
public ChModelBullet(ChBody body)
{
}
/// 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);
}
/// Sets the parent rigid body.
public void SetBody(ChBody newRB)
{
Body = newRB;
}
/// Initialize this tire subsystem.
/// Cache the associated wheel body and vehicle side flag, and add tire mass and
/// inertia to the wheel body. A derived class must first call this base implementation.
public virtual void Initialize(ChBody wheel, ChSubsysDefs.VehicleSide side)
{
}
/// Calculate Jacobian of generalized contact forces.
public void CalculateJacobians(ChMaterialCompositeSMC mat)
{
// Compute a finite-difference approximations to the Jacobians of the contact forces and
// load dQ/dx into m_Jac.m_K and dQ/dw into m_Jac.m_R.
// Note that we only calculate these Jacobians whenever the contact force itself is calculated,
// that is only once per step. The Jacobian of generalized contact forces will therefore be
// constant over the time step.
ChBody oA = (this.objA as ChBody);
ChBody oB = (this.objB as ChBody);
// Get states for objA
int ndofA_x = oA.ContactableGet_ndof_x();
int ndofA_w = oA.ContactableGet_ndof_w();
ChState stateA_x = new ChState(ndofA_x, null);
ChStateDelta stateA_w = new ChStateDelta(ndofA_w, null);
oA.ContactableGetStateBlock_x(ref stateA_x);
oA.ContactableGetStateBlock_w(ref stateA_w);
// Get states for objB
int ndofB_x = oB.ContactableGet_ndof_x();
int ndofB_w = oB.ContactableGet_ndof_w();
ChState stateB_x = new ChState(ndofB_x, null);
ChStateDelta stateB_w = new ChStateDelta(ndofB_w, null);
oB.ContactableGetStateBlock_x(ref stateB_x);
oB.ContactableGetStateBlock_w(ref stateB_w);
// Compute Q at current state
ChVectorDynamic <double> Q0 = new ChVectorDynamic <double>(ndofA_w + ndofB_w);
CalculateQ(stateA_x, stateA_w, stateB_x, stateB_w, mat, ref Q0);
// Finite-difference approximation perturbation.
// Note that ChState and ChStateDelta are set to 0 on construction.
// To accommodate objects with quaternion states, use the method ContactableIncrementState while
// calculating Jacobian columns corresponding to position states.
double perturbation = 1e-5;
ChState stateA_x1 = new ChState(ndofA_x, null);
ChState stateB_x1 = new ChState(ndofB_x, null);
ChStateDelta prtrbA = new ChStateDelta(ndofA_w, null);
ChStateDelta prtrbB = new ChStateDelta(ndofB_w, null);
ChVectorDynamic <double> Q1 = new ChVectorDynamic <double>(ndofA_w + ndofB_w);
ChVectorDynamic <double> Jcolumn = new ChVectorDynamic <double>(ndofA_w + ndofB_w);
// Jacobian w.r.t. variables of objA
for (int i = 0; i < ndofA_w; i++)
{
prtrbA.matrix[i] += perturbation;
oA.ContactableIncrementState(stateA_x, prtrbA, ref stateA_x1);
CalculateQ(stateA_x1, stateA_w, stateB_x, stateB_w, mat, ref Q1);
prtrbA.matrix[i] -= perturbation;
Jcolumn = (Q1 - Q0.matrix) * (-1 / perturbation); // note sign change
m_Jac.m_K.matrix.PasteMatrix(Jcolumn.matrix, 0, i);
stateA_w.matrix[i] += perturbation;
CalculateQ(stateA_x, stateA_w, stateB_x, stateB_w, mat, ref Q1);
stateA_w.matrix[i] -= perturbation;
Jcolumn = (Q1 - Q0.matrix) * (-1 / perturbation); // note sign change
m_Jac.m_R.matrix.PasteMatrix(Jcolumn.matrix, 0, i);
}
// Jacobian w.r.t. variables of objB
for (int i = 0; i < ndofB_w; i++)
{
prtrbB.matrix[i] += perturbation;
oB.ContactableIncrementState(stateB_x, prtrbB, ref stateB_x1);
CalculateQ(stateA_x, stateA_w, stateB_x1, stateB_w, mat, ref Q1);
prtrbB.matrix[i] -= perturbation;
Jcolumn = (Q1 - Q0.matrix) * (-1 / perturbation); // note sign change
m_Jac.m_K.matrix.PasteMatrix(Jcolumn.matrix, 0, ndofA_w + i);
stateB_w.matrix[i] += perturbation;
CalculateQ(stateA_x, stateA_w, stateB_x, stateB_w, mat, ref Q1);
stateB_w.matrix[i] -= perturbation;
Jcolumn = (Q1 - Q0.matrix) * (-1 / perturbation); // note sign change
m_Jac.m_R.matrix.PasteMatrix(Jcolumn.matrix, 0, ndofA_w + i);
}
}
/// 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);
}
}
public override void OnInspectorGUI()
{
ChBody body = (ChBody)target;
EditorGUILayout.HelpBox("Class for rigid bodies. A rigid body is an entity which can move in 3D space, and can be constrained to other rigid bodies using ChLink objects.", MessageType.Info);
GUIContent type = new GUIContent("Collision Type", "Change what type of collision shape.");
body.type = (ChBody.CollisionType)EditorGUILayout.EnumPopup(type, body.type);
GUIContent bodyfixed = new GUIContent("Fixed", "Toggle whether the object is a static/fixed rigid body or dynamic.");
body.bodyfixed = EditorGUILayout.Toggle(bodyfixed, body.bodyfixed);
GUIContent collide = new GUIContent("Collide", "Toggle whether the rigid body can collide and interact with other collidable rigid body.");
body.collide = EditorGUILayout.Toggle(collide, body.collide);
GUIContent autoInertia = new GUIContent("Automatic Mass/Inertia", "Toggle whether the rigid body's mass and inertia is calculated automatically based on dimensions and custom density, or if you would like to set the mass and inertia manually.");
body.automaticMass = EditorGUILayout.Toggle(autoInertia, body.automaticMass);
if (body.automaticMass)
{
GUIContent density = new GUIContent("MDensity", "Set the rigid body density. Used when doing the 'recompute mass' operation.");
body.density = EditorGUILayout.FloatField(density, body.density);
}
else
{
GUIContent mass = new GUIContent("Mass", "Mass of the rigid body. Must be positive. Try not to mix bodies with too high/too low values of mass, for numerical stability.");
body.mass = EditorGUILayout.DoubleField(mass, body.mass);
GUIContent inertXX = new GUIContent("Moments of Inertia", "Advanced, Set the diagonal part of the inertia tensor, The provided 3x1 vector should contain the moments of inertia, expressed in the local coordinate frame");
body.inertiaMoments = EditorGUILayout.Vector3Field(inertXX, body.inertiaMoments);
GUIContent inertXY = new GUIContent("Products of Inertia", "Advanced, Set the off-diagonal part of the inertia tensor (Ixy, Ixz, Iyz values). Warning about sign: in some books they write the inertia tensor as I=[Ixx, -Ixy, -Ixz; etc.] but here is I=[Ixx, Ixy, Ixz; ...]. The provided 3x1 vector should contain the products of inertia, expressed in the local coordinate frame:");
body.inertiaProducts = EditorGUILayout.Vector3Field(inertXY, body.inertiaProducts);
}
EditorGUILayout.LabelField("", GUI.skin.horizontalSlider);
GUIContent linVel = new GUIContent("Linear Velocity", "Set the linear speed. This is executed at startup once.");
body.linearVelocity = EditorGUILayout.Vector3Field(linVel, body.linearVelocity);
GUIContent angVel = new GUIContent("Angular Velocity", "Set the rotation speed from given angular speed. This is executed at startup once.");
body.angularVelocity = EditorGUILayout.Vector3Field(angVel, body.angularVelocity);
EditorGUILayout.LabelField("", GUI.skin.horizontalSlider);
GUIContent matType = new GUIContent("Material Type", "Change the material physics type. NSC surface for use with non-smooth (complementarity) contact method. SMC Material data for a surface for use with smooth (penalty) contact method. This data is used to define surface properties owned by ChBody rigid bodies and similar objects; it carries information that is used to make contacts.");
body.materialType = (ChBody.MaterialType)EditorGUILayout.EnumPopup(matType, body.materialType);
EditorGUI.indentLevel++;
switch (body.materialType)
{
case ChBody.MaterialType.NSC:
EditorGUILayout.PropertyField(friction, new GUIContent("Friction"), GUILayout.Height(20));
GUIContent rolling = new GUIContent("Rolling Friction", "The rolling friction (rolling parameter, it has the dimension of a length). Rolling resistant torque is Tr <= (normal force) * (this parameter) Usually a very low value. Measuring unit: m Default =0. Note! a non-zero value will make the simulation 2x slower! Also, the GPU solver currently does not support rolling friction. Default: 0.");
EditorGUILayout.PropertyField(rolling_friction, new GUIContent(rolling), GUILayout.Height(20));
GUIContent spinning = new GUIContent("Spinning Friction", "The spinning friction (it has the dimension of a length). Spinning resistant torque is Ts <= (normal force) * (this parameter) Usually a very low value. Measuring unit: m Default =0. Note! a non-zero value will make the simulation 2x slower! Also, the GPU solver currently does not support spinning friction. Default: 0.");
EditorGUILayout.PropertyField(spinning_friction, new GUIContent(spinning), GUILayout.Height(20));
GUIContent rest = new GUIContent("Restitution", "The normal restitution coefficient, for collisions. Should be in 0..1 range. Default =0.");
EditorGUILayout.PropertyField(restitution, new GUIContent(rest), GUILayout.Height(20));
GUIContent coh = new GUIContent("Cohesion", "The cohesion max. force for normal pulling traction in contacts. Measuring unit: N Default =0.");
EditorGUILayout.PropertyField(cohesion, new GUIContent(coh), GUILayout.Height(20));
GUIContent dampf = new GUIContent("Dampingf", "The damping in contact, as a factor 'f': damping is a multiple of stiffness [K], that is: [R]=f*[K] Measuring unit: time, s. Default =0.");
EditorGUILayout.PropertyField(dampingf, new GUIContent(dampf), GUILayout.Height(20));
GUIContent comp = new GUIContent("Compliance", "Compliance of the contact, in normal direction. It is the inverse of the stiffness [K] , so for zero value one has a perfectly rigid contact. Measuring unit: m/N Default =0.");
EditorGUILayout.PropertyField(compliance, new GUIContent(comp), GUILayout.Height(20));
GUIContent compt = new GUIContent("ComplianceT", "Compliance of the contact, in normal direction. It is the inverse of the stiffness [K] , so for zero value one has a perfectly rigid contact. Measuring unit: m/N Default =0.");
EditorGUILayout.PropertyField(complianceT, new GUIContent(compt), GUILayout.Height(20));
GUIContent comproll = new GUIContent("Compliance Rolling", "Rolling compliance of the contact, if using a nonzero rolling friction. (If there is no rolling friction, this has no effect.) Measuring unit: rad/Nm Default =0.");
EditorGUILayout.PropertyField(complianceRoll, new GUIContent(comproll), GUILayout.Height(20));
GUIContent compspin = new GUIContent("Compliance Spinning", " Spinning compliance of the contact, if using a nonzero rolling friction. (If there is no spinning friction, this has no effect.) Measuring unit: rad/Nm Default =0.");
EditorGUILayout.PropertyField(complianceSpin, new GUIContent(compspin), GUILayout.Height(20));
serializedObject.ApplyModifiedProperties();
//body.matsurface = body.gameObject.AddComponent<ChMaterialSurfaceNSC>() as ChMaterialSurfaceNSC;
break;
case ChBody.MaterialType.SMC:
EditorGUILayout.PropertyField(friction, new GUIContent("Friction"), GUILayout.Height(20));
GUIContent restsmc = new GUIContent("Restitution", "The normal restitution coefficient, for collisions. Should be in 0..1 range. Default =0.");
EditorGUILayout.PropertyField(restitution, new GUIContent(restsmc), GUILayout.Height(20));
GUIContent young = new GUIContent("Young Modulus", "Young's modulus, modulus of elasticity");
EditorGUILayout.PropertyField(young_modulus, new GUIContent(young), GUILayout.Height(20));
GUIContent poison = new GUIContent("Poisson Ratio", "Poisson's ratio is a measure of the Poisson effect, the phenomenon in which a material tends to expand in directions perpendicular to the direction of compression. Conversely, if the material is stretched rather than compressed, it usually tends to contract in the directions transverse to the direction of stretching.");
EditorGUILayout.PropertyField(poisson_ratio, new GUIContent(poison), GUILayout.Height(20));
// EditorGUILayout.PropertyField(static_friction, new GUIContent("Static Friction"), GUILayout.Height(20));
EditorGUILayout.PropertyField(sliding_friction, new GUIContent("Sliding Friction"), GUILayout.Height(20));
EditorGUILayout.PropertyField(constant_adhesion, new GUIContent("Constant Adhesion"), GUILayout.Height(20));
EditorGUILayout.PropertyField(adhesionMultDMT, new GUIContent("AdhesionMultDMT"), GUILayout.Height(20));
EditorGUILayout.PropertyField(kn, new GUIContent("Kn"), GUILayout.Height(20));
EditorGUILayout.PropertyField(kt, new GUIContent("Kt"), GUILayout.Height(20));
EditorGUILayout.PropertyField(gn, new GUIContent("Gn"), GUILayout.Height(20));
EditorGUILayout.PropertyField(gt, new GUIContent("Gt"), GUILayout.Height(20));
serializedObject.ApplyModifiedProperties();
//matsurface = gameObject.AddComponent<ChMaterialSurfaceSMC>() as ChMaterialSurfaceSMC;
break;
}
if (GUI.changed)
{
EditorUtility.SetDirty(body);
}
}
