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();
}
public void SetPolar_Max(ChFunction m_funct)
{
if (polar_Max != null)
{
polar_Max = null;
}
polar_Max = m_funct;
}
public void SetModul_Rmin(ChFunction m_funct)
{
if (modul_Rmin != null)
{
modul_Rmin = null;
}
modul_Rmin = m_funct;
}
public void SetModul_Rmax(ChFunction m_funct)
{
if (modul_Rmax != null)
{
modul_Rmax = null;
}
modul_Rmax = m_funct;
}
public void Set_modul_R(ChFunction m_funct)
{
if (modul_R != null)
{
modul_R = null;
}
modul_R = m_funct;
}
public void Set_modul_K(ChFunction m_funct)
{
if (modul_K != null)
{
modul_K = null;
}
modul_K = m_funct;
}
public void Set_modul_iforce(ChFunction m_funct)
{
if (modul_iforce != null)
{
modul_iforce = null;
}
modul_iforce = m_funct;
}
private ChFunction modul_R; //< modulation of R along the dof coord
public ChLinkForce()
{
active = false;
iforce = 0;
K = 0;
R = 0;
modul_iforce = new ChFunction_Const(1); // default: const.modulation of iforce
modul_K = new ChFunction_Const(1); // default: const.modulation of K
modul_R = new ChFunction_Const(1); // default: const.modulation of R
}
public ChShaftsMotorSpeed(ChShaftsMotorSpeed other)
{
this.variable = other.variable;
this.f_speed = other.f_speed;
this.rot_offset = other.rot_offset;
this.aux_dt = other.aux_dt;
this.aux_dtdt = other.aux_dtdt;
this.avoid_angle_drift = other.avoid_angle_drift;
}
public ChLinkSpring(ChLinkSpring other) : base(other)
{
spr_restlength = other.spr_restlength;
spr_f = other.spr_f;
spr_k = other.spr_k;
spr_r = other.spr_r;
spr_react = other.spr_react;
mod_f_time = new ChFunction(other.mod_f_time.Clone());
mod_k_d = new ChFunction(other.mod_k_d.Clone());
mod_k_speed = new ChFunction(other.mod_k_speed.Clone());
mod_r_d = new ChFunction(other.mod_r_d.Clone());
mod_r_speed = new ChFunction(other.mod_r_speed.Clone());
}
public ChLinkLinActuator(ChLinkLinActuator other)
{
learn = other.learn;
learn_torque_rotation = other.learn_torque_rotation;
offset = other.offset;
dist_funct = new ChFunction(other.dist_funct.Clone());
mot_torque = new ChFunction(other.mot_torque.Clone());
mot_rot = new ChFunction(other.mot_rot.Clone());
mot_tau = other.mot_tau;
mot_eta = other.mot_eta;
mot_inertia = other.mot_inertia;
}
private ChConstraintTwoGeneric constraint; //< used as an interface to the solver
public ChShaftsMotorSpeed()
{
motor_torque = 0;
this.variable.GetMass()[0, 0] = 1.0;
this.variable.GetInvMass()[0, 0] = 1.0;
this.f_speed = new ChFunction_Const(1.0);
this.rot_offset = 0;
this.aux_dt = 0; // used for integrating speed, = pos
this.aux_dtdt = 0;
this.avoid_angle_drift = true;
}
public ChLinkSpring()
{
spr_restlength = 0;
spr_k = 100;
spr_r = 5;
spr_f = 0;
mod_f_time = new ChFunction_Const(1);
mod_k_d = new ChFunction_Const(1);
mod_k_speed = new ChFunction_Const(1);
mod_r_d = new ChFunction_Const(1);
mod_r_speed = new ChFunction_Const(1);
spr_react = 0.0;
}
public ChLinkForce(ChLinkForce other)
{
active = other.active;
iforce = other.iforce;
K = other.K;
R = other.R;
// replace functions:
modul_iforce = null;
modul_K = null;
modul_R = null;
modul_iforce = other.modul_iforce.Clone();
modul_K = other.modul_K.Clone();
modul_R = other.modul_R.Clone();
}
// Note, *always!" override the Awake function inheriting from LinkLock! Otherwise the LinkLock Awake is executed.
public override void Start()
{
dist_funct = gameObject.AddComponent <ChFunction_Const>();
ChVector link_abs = new ChVector(transform.position.x, transform.position.y, transform.position.z);
ChVector yaxis = new ChVector(0.0, 0.0, 1.0);
double _offset = 10;
ChVector pt1 = link_abs;
ChVector pt2 = link_abs - _offset * yaxis;
Initialize(body1, body2, pos_are_relative, new ChCoordsys(pt1, ChQuaternion.QUNIT), new ChCoordsys(pt2, ChQuaternion.QUNIT));
Set_lin_offset(_offset);
ChSystem.system.AddLink(this);
}
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();
}
public void Set_learn(bool mset)
{
learn = mset;
if ((eng_mode == eCh_eng_mode.ENG_MODE_ROTATION) || (eng_mode == eCh_eng_mode.ENG_MODE_SPEED) || (eng_mode == eCh_eng_mode.ENG_MODE_KEY_ROTATION))
{
if (mset)
{
((ChLinkMaskLF)mask).Constr_E3().SetMode(eChConstraintMode.CONSTRAINT_FREE);
}
else
{
((ChLinkMaskLF)mask).Constr_E3().SetMode(eChConstraintMode.CONSTRAINT_LOCK);
}
ChangedLinkMask();
}
if (eng_mode == eCh_eng_mode.ENG_MODE_KEY_POLAR)
{
if (mset)
{
((ChLinkMaskLF)mask).Constr_E1().SetMode(eChConstraintMode.CONSTRAINT_FREE);
((ChLinkMaskLF)mask).Constr_E2().SetMode(eChConstraintMode.CONSTRAINT_FREE);
((ChLinkMaskLF)mask).Constr_E3().SetMode(eChConstraintMode.CONSTRAINT_FREE);
}
else
{
((ChLinkMaskLF)mask).Constr_E1().SetMode(eChConstraintMode.CONSTRAINT_LOCK);
((ChLinkMaskLF)mask).Constr_E2().SetMode(eChConstraintMode.CONSTRAINT_LOCK);
((ChLinkMaskLF)mask).Constr_E3().SetMode(eChConstraintMode.CONSTRAINT_LOCK);
}
ChangedLinkMask();
}
if (eng_mode == eCh_eng_mode.ENG_MODE_ROTATION && rot_funct.Get_Type() != ChFunction.FunctionType.FUNCT_RECORDER)
{
rot_funct = new ChFunction_Recorder();
}
if (eng_mode == eCh_eng_mode.ENG_MODE_SPEED && spe_funct.Get_Type() != ChFunction.FunctionType.FUNCT_RECORDER)
{
spe_funct = new ChFunction_Recorder();
}
}
public ChLinkEngine()
{
mot_rot = 0;
mot_rot_dt = 0;
mot_rot_dtdt = 0;
mot_rerot = 0;
mot_rerot_dt = 0;
mot_rerot_dtdt = 0;
mot_torque = 0;
mot_retorque = 0;
last_r3mot_rot = 0;
last_r3mot_rot_dt = 0;
last_r3relm_rot = ChQuaternion.QUNIT;
last_r3relm_rot_dt = ChQuaternion.QNULL;
last_r3time = 0;
keyed_polar_rotation = ChQuaternion.QNULL;
impose_reducer = false;
mot_tau = 1;
mot_eta = 1;
mot_inertia = 0;
torque_react2 = 0;
eng_mode = eCh_eng_mode.ENG_MODE_ROTATION;
learn = false;
rot_funct = new ChFunction_Const(0);
spe_funct = new ChFunction_Const(0);
tor_funct = new ChFunction_Const(0);
torque_w = new ChFunction_Const(1);
rot_funct_x = new ChFunction_Const(0);
rot_funct_y = new ChFunction_Const(0);
// Mask: initialize our LinkMaskLF (lock formulation mask)
// to E3 only.
ChLinkMaskLF masklf = new ChLinkMaskLF();
mask = masklf;
((ChLinkMaskLF)mask).SetLockMask(true, false, false, false, false, false, true);
ChangedLinkMask();
// Mask: initialize remaining LinkMaskLF (lock formulation mask) for the engine.
// All shaft modes at least are setting the lock on E3 (z-rotation) coordinate.
Set_shaft_mode(eCh_shaft_mode.ENG_SHAFT_LOCK);
}
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;
}
public ChLinkLimit(ChLinkLimit other)
{
active = other.active;
penalty_only = other.penalty_only;
polar = other.polar;
rotation = other.rotation;
max = other.max;
min = other.min;
maxCushion = other.maxCushion;
minCushion = other.minCushion;
Kmax = other.Kmax;
Kmin = other.Kmin;
Rmax = other.Rmax;
Rmin = other.Rmin;
minElastic = other.minElastic;
maxElastic = other.maxElastic;
// replace functions:
modul_Kmax = other.modul_Kmax.Clone();
modul_Kmin = other.modul_Kmin.Clone();
modul_Rmax = other.modul_Rmax.Clone();
modul_Rmin = other.modul_Rmin.Clone();
polar_Max = other.polar_Max.Clone();
}
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);
}
public ChLinkLimit()
{
active = false;
penalty_only = false;
polar = false;
rotation = false;
max = 1;
min = -1;
maxCushion = 0;
minCushion = 0;
Kmax = 1000;
Kmin = 1000;
Rmax = 100;
Rmin = 100;
minElastic = 0;
maxElastic = 0;
modul_Kmax = new ChFunction_Const(1); // default: const.modulation of K
modul_Kmin = new ChFunction_Const(1); // default: const.modulation of K
modul_Rmax = new ChFunction_Const(1); // default: const.modulation of K
modul_Rmin = new ChFunction_Const(1); // default: const.modulation of K
polar_Max = new ChFunction_Const(1); // default function for polar limits
constr_upper.SetMode(eChConstraintMode.CONSTRAINT_UNILATERAL);
constr_lower.SetMode(eChConstraintMode.CONSTRAINT_UNILATERAL);
}
/// Set the angular speed function of time w(t).
/// To prevent acceleration pikes, this function should be C0 continuous.
public void SetSpeedFunction(ChFunction function)
{
SetMotorFunction(function);
}
/// Set the imposed motion law, for rotation about an axis
public void SetMotion_ang(ChFunction m_funct)
{
motion_ang = m_funct;
}
/// Set the imposed motion law, for translation on Z body axis
public void SetMotion_Z(ChFunction m_funct)
{
motion_Z = m_funct;
}
/// Set the imposed motion law, for translation on Y body axis
public void SetMotion_Y(ChFunction m_funct)
{
motion_Y = m_funct;
}
//
// Imposed motion
//
/// Set the imposed motion law, for translation on X body axis
public void SetMotion_X(ChFunction m_funct)
{
motion_X = m_funct;
}
/// Set the torque function of time T(t).
public void SetTorqueFunction(ChFunction function)
{
SetMotorFunction(function);
}
/// Set the rotation angle function of time a(t).
/// This function should be C0 continuous and, to prevent acceleration spikes,
/// it should ideally be C1 continuous.
public void SetAngleFunction(ChFunction function)
{
SetMotorFunction(function);
}
public ChFunction(ChFunction other)
{
}