/// Someone (ex. an ChExternalObject() ) may send this message to
/// the marker to tell that time has changed (even if simulation is
/// not running! - so it is different from the usual UpdateTime() -)
public void UpdatedExternalTime(double prevtime, double mtime)
{
double mstep = mtime - prevtime;
ChCoordsys m_rel_pos_dt = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0));
ChCoordsys m_rel_pos_dtdt = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0));
// do not try to switch on the M_MOTION_KEYFRAMED mode if
// we are already in the M_MOTION_EXTERNAL mode, maybe because
// a link point-surface is already moving the marker and
// it will handle the accelerations by itself
if (this.motion_type == eChMarkerMotion.M_MOTION_EXTERNAL)
{
return;
}
// otherwise see if a BDF is needed, cause an external 3rd party is moving the marker
this.motion_type = eChMarkerMotion.M_MOTION_FUNCTIONS;
// if POSITION or ROTATION ("rel_pos") has been changed in acceptable time step...
if ((!(ChVector.Vequal(FrameMoving.coord.pos, last_rel_coord.pos)) || !(ChQuaternion.Qequal(FrameMoving.coord.rot, last_rel_coord.rot))) && (Math.Abs(mstep) < 0.1) &&
(mstep != 0))
{
// ... and if motion wasn't caused by motion laws, then it was a keyframed movement!
if ((motion_X.Get_y(mtime) == 0) && (motion_Y.Get_y(mtime) == 0) && (motion_Z.Get_y(mtime) == 0) &&
(motion_ang.Get_y(mtime) == 0) && (motion_X.Get_Type() == ChFunction.FunctionType.FUNCT_CONST) &&
(motion_Y.Get_Type() == ChFunction.FunctionType.FUNCT_CONST) && (motion_Z.Get_Type() == ChFunction.FunctionType.FUNCT_CONST) &&
(motion_ang.Get_Type() == ChFunction.FunctionType.FUNCT_CONST))
{
// compute the relative speed by BDF !
m_rel_pos_dt.pos = ChVector.Vmul(ChVector.Vsub(FrameMoving.coord.pos, last_rel_coord.pos), 1 / mstep);
m_rel_pos_dt.rot = ChQuaternion.Qscale(ChQuaternion.Qsub(FrameMoving.coord.rot, last_rel_coord.rot), 1 / mstep);
// compute the relative acceleration by BDF !
m_rel_pos_dtdt.pos = ChVector.Vmul(ChVector.Vsub(m_rel_pos_dt.pos, last_rel_coord_dt.pos), 1 / mstep);
m_rel_pos_dtdt.rot = ChQuaternion.Qscale(ChQuaternion.Qsub(m_rel_pos_dt.rot, last_rel_coord_dt.rot), 1 / mstep);
// Set the position, speed and acceleration in relative space,
// automatically getting also the absolute values,
FrameMoving.SetCoord_dt(m_rel_pos_dt);
FrameMoving.SetCoord_dtdt(m_rel_pos_dtdt);
// update the remaining state variables
this.UpdateState();
// remember that the movement of this guy won't need further update
// of speed and acc. via motion laws!
this.motion_type = eChMarkerMotion.M_MOTION_KEYFRAMED;
}
}
// restore state buffers and that's all.
last_time = ChTime;
last_rel_coord = (ChCoordsys)FrameMoving.coord;
last_rel_coord_dt = (ChCoordsys)FrameMoving.coord_dt;
}
public void Set_eng_mode(eCh_eng_mode mset)
{
if (Get_learn())
{
Set_learn(false); // reset learn state when changing mode
}
if (eng_mode != mset)
{
eng_mode = mset;
switch (eng_mode)
{
case eCh_eng_mode.ENG_MODE_ROTATION:
case eCh_eng_mode.ENG_MODE_SPEED:
case eCh_eng_mode.ENG_MODE_KEY_ROTATION:
((ChLinkMaskLF)mask).Constr_E3().SetMode(eChConstraintMode.CONSTRAINT_LOCK);
break;
case eCh_eng_mode.ENG_MODE_KEY_POLAR:
((ChLinkMaskLF)mask).Constr_E1().SetMode(eChConstraintMode.CONSTRAINT_LOCK);
((ChLinkMaskLF)mask).Constr_E2().SetMode(eChConstraintMode.CONSTRAINT_LOCK);
((ChLinkMaskLF)mask).Constr_E3().SetMode(eChConstraintMode.CONSTRAINT_LOCK);
break;
case eCh_eng_mode.ENG_MODE_TORQUE:
((ChLinkMaskLF)mask).Constr_E3().SetMode(eChConstraintMode.CONSTRAINT_FREE);
break;
case eCh_eng_mode.ENG_MODE_TO_POWERTRAIN_SHAFT:
((ChLinkMaskLF)mask).Constr_E3().SetMode(eChConstraintMode.CONSTRAINT_FREE);
innerconstraint1 = gameObject.AddComponent <ChShaftsBody>();
innerconstraint2 = gameObject.AddComponent <ChShaftsBody>();
innershaft1 = gameObject.AddComponent <ChShaft>();
innershaft2 = gameObject.AddComponent <ChShaft>();
SetUpMarkers(marker1, marker2); // to initialize innerconstraint1 innerconstraint2
break;
}
ChangedLinkMask(); // update all from new mask
}
if (eng_mode == eCh_eng_mode.ENG_MODE_KEY_ROTATION && rot_funct.Get_Type() != ChFunction.FunctionType.FUNCT_CONST)
{
rot_funct = new ChFunction_Const();
}
if (eng_mode == eCh_eng_mode.ENG_MODE_KEY_POLAR)
{
if (rot_funct.Get_Type() != ChFunction.FunctionType.FUNCT_CONST)
{
rot_funct = new ChFunction_Const();
}
if (rot_funct_x.Get_Type() != ChFunction.FunctionType.FUNCT_CONST)
{
rot_funct_x = new ChFunction_Const();
}
if (rot_funct_y.Get_Type() != ChFunction.FunctionType.FUNCT_CONST)
{
rot_funct_y = new ChFunction_Const();
}
}
}
//
// UPDATING
//
/// Updates the time.dependant variables (ex: ChFunction objects
/// which impose the body-relative motion, etc.)
public void UpdateTime(double mytime)
{
ChCoordsys csys = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0));
ChCoordsys csys_dt = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0));
ChCoordsys csys_dtdt = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0));
ChQuaternion qtemp;// = new ChQuaternion(1, 0, 0, 0);
double ang, ang_dt, ang_dtdt;
ChTime = mytime;
// if a imposed motion (keyframed movement) affects the marker position (example,from R3D animation system),
// compute the speed and acceleration values by BDF (example,see the UpdatedExternalTime() function, later)
// so the updating via motion laws can be skipped!
if (motion_type == eChMarkerMotion.M_MOTION_KEYFRAMED)
{
return;
}
// skip relative-position-functions evaluation also if
// someone is already handling this from outside..
if (motion_type == eChMarkerMotion.M_MOTION_EXTERNAL)
{
return;
}
// positions:
// update positions: rel_pos
csys.pos.x = motion_X.Get_y(mytime);
csys.pos.y = motion_Y.Get_y(mytime);
csys.pos.z = motion_Z.Get_y(mytime);
if (motion_X.Get_Type() != ChFunction.FunctionType.FUNCT_MOCAP)
{
csys.pos += rest_coord.pos;
}
// update speeds: rel_pos_dt
csys_dt.pos.x = motion_X.Get_y_dx(mytime);
csys_dt.pos.y = motion_Y.Get_y_dx(mytime);
csys_dt.pos.z = motion_Z.Get_y_dx(mytime);
// update accelerations
csys_dtdt.pos.x = motion_X.Get_y_dxdx(mytime);
csys_dtdt.pos.y = motion_Y.Get_y_dxdx(mytime);
csys_dtdt.pos.z = motion_Z.Get_y_dxdx(mytime);
// rotations:
ang = motion_ang.Get_y(mytime);
ang_dt = motion_ang.Get_y_dx(mytime);
ang_dtdt = motion_ang.Get_y_dxdx(mytime);
if ((ang != 0) || (ang_dt != 0) || (ang_dtdt != 0))
{
// update q
ChVector motion_axis_versor = ChVector.Vnorm(motion_axis);
qtemp = ChQuaternion.Q_from_AngAxis2(ang, motion_axis_versor);
csys.rot = ChQuaternion.Qcross(qtemp, rest_coord.rot);
// update q_dt
csys_dt.rot = ChQuaternion.Qdt_from_AngAxis(csys.rot, ang_dt, motion_axis_versor);
// update q_dtdt
csys_dtdt.rot = ChQuaternion.Qdtdt_from_AngAxis(ang_dtdt, motion_axis_versor, csys.rot, csys_dt.rot);
}
else
{
csys.rot = FrameMoving.coord.rot; // rel_pos.rot;
csys_dt.rot = ChQuaternion.QNULL;
csys_dtdt.rot = ChQuaternion.QNULL;
}
// Set the position, speed and acceleration in relative space,
// automatically getting also the absolute values,
if (!(csys == this.FrameMoving.coord))
{
FrameMoving.SetCoord(csys);
}
if (!(csys_dt == this.FrameMoving.coord_dt) || !(csys_dt.rot == new ChQuaternion(0, 0, 0, 0)))
{
FrameMoving.SetCoord_dt(csys_dt);
}
if (!(csys_dtdt == this.FrameMoving.coord_dtdt) || !(csys_dtdt.rot == new ChQuaternion(0, 0, 0, 0)))
{
FrameMoving.SetCoord_dtdt(csys_dtdt);
}
}
/// Updates motion laws, etc. for the impose rotation / impose speed modes
public override void UpdateTime(double mytime)
{
// First, inherit to parent class
base.UpdateTime(mytime);
if (!IsActive())
{
return;
}
// DEFAULTS compute rotation vars...
// by default for torque control..
motion_axis = ChVector.VECT_Z; // motion axis is always the marker2 Z axis (in m2 relative coords)
mot_rot = relAngle;
mot_rot_dt = ChVector.Vdot(relWvel, motion_axis);
mot_rot_dtdt = ChVector.Vdot(relWacc, motion_axis);
mot_rerot = mot_rot / mot_tau;
mot_rerot_dt = mot_rot_dt / mot_tau;
mot_rerot_dtdt = mot_rot_dtdt / mot_tau;
// nothing more to do here for torque control
if (eng_mode == eCh_eng_mode.ENG_MODE_TORQUE)
{
return;
}
// If LEARN MODE, just record motion
if (learn)
{
deltaC.pos = ChVector.VNULL;
deltaC_dt.pos = ChVector.VNULL;
deltaC_dtdt.pos = ChVector.VNULL;
if (!(limit_Rx.Get_active() || limit_Ry.Get_active() || limit_Rz.Get_active()))
{
deltaC.rot = ChQuaternion.QUNIT;
deltaC_dt.rot = ChQuaternion.QNULL;
deltaC_dtdt.rot = ChQuaternion.QNULL;
}
if (eng_mode == eCh_eng_mode.ENG_MODE_ROTATION)
{
if (rot_funct.Get_Type() != ChFunction.FunctionType.FUNCT_RECORDER)
{
rot_funct = new ChFunction_Recorder();
}
// record point
double rec_rot = relAngle; // ***TO DO*** compute also rotations with cardano mode?
if (impose_reducer)
{
rec_rot = rec_rot / mot_tau;
}
ChFunction_Recorder rec = (ChFunction_Recorder)rot_funct;
rec.AddPoint(mytime, rec_rot, 1); // x=t
}
if (eng_mode == eCh_eng_mode.ENG_MODE_SPEED)
{
if (spe_funct.Get_Type() != ChFunction.FunctionType.FUNCT_RECORDER)
{
spe_funct = new ChFunction_Recorder();
}
// record point
double rec_spe = ChVector.Vlength(relWvel); // ***TO DO*** compute also with cardano mode?
if (impose_reducer)
{
rec_spe = rec_spe / mot_tau;
}
ChFunction_Recorder rec = (ChFunction_Recorder)spe_funct;
rec.AddPoint(mytime, rec_spe, 1); // x=t
}
}
if (learn)
{
return; // no need to go on further...--.>>>
}
// Impose relative positions/speeds
deltaC.pos = ChVector.VNULL;
deltaC_dt.pos = ChVector.VNULL;
deltaC_dtdt.pos = ChVector.VNULL;
if (eng_mode == eCh_eng_mode.ENG_MODE_ROTATION)
{
if (impose_reducer)
{
mot_rerot = rot_funct.Get_y(ChTime);
mot_rerot_dt = rot_funct.Get_y_dx(ChTime);
mot_rerot_dtdt = rot_funct.Get_y_dxdx(ChTime);
mot_rot = mot_rerot * mot_tau;
mot_rot_dt = mot_rerot_dt * mot_tau;
mot_rot_dtdt = mot_rerot_dtdt * mot_tau;
}
else
{
mot_rot = rot_funct.Get_y(ChTime);
mot_rot_dt = rot_funct.Get_y_dx(ChTime);
mot_rot_dtdt = rot_funct.Get_y_dxdx(ChTime);
mot_rerot = mot_rot / mot_tau;
mot_rerot_dt = mot_rot_dt / mot_tau;
mot_rerot_dtdt = mot_rot_dtdt / mot_tau;
}
deltaC.rot = ChQuaternion.Q_from_AngAxis2(mot_rot, motion_axis);
deltaC_dt.rot = ChQuaternion.Qdt_from_AngAxis(deltaC.rot, mot_rot_dt, motion_axis);
deltaC_dtdt.rot = ChQuaternion.Qdtdt_from_AngAxis(mot_rot_dtdt, motion_axis, deltaC.rot, deltaC_dt.rot);
}
if (eng_mode == eCh_eng_mode.ENG_MODE_SPEED)
{
if (impose_reducer)
{
mot_rerot_dt = spe_funct.Get_y(ChTime);
mot_rerot_dtdt = spe_funct.Get_y_dx(ChTime);
mot_rot_dt = mot_rerot_dt * mot_tau;
mot_rot_dtdt = mot_rerot_dtdt * mot_tau;
}
else
{
mot_rot_dt = spe_funct.Get_y(ChTime);
mot_rot_dtdt = spe_funct.Get_y_dx(ChTime);
mot_rerot_dt = mot_rot_dt / mot_tau;
mot_rerot_dtdt = mot_rot_dtdt / mot_tau;
}
deltaC.rot = ChQuaternion.Qnorm(GetRelM().rot); // just keep current position, -assume always good after integration-.
ChMatrix33 <double> relA = new ChMatrix33 <double>(0);
relA.Set_A_quaternion(GetRelM().rot); // ..but adjust to keep Z axis aligned to shaft, anyway!
ChVector displaced_z_axis = relA.Get_A_Zaxis();
ChVector adjustment = ChVector.Vcross(displaced_z_axis, ChVector.VECT_Z);
deltaC.rot = ChQuaternion.Q_from_AngAxis2(ChVector.Vlength(adjustment), ChVector.Vnorm(adjustment)) % deltaC.rot;
deltaC_dt.rot = ChQuaternion.Qdt_from_AngAxis(deltaC.rot, mot_rot_dt, motion_axis);
deltaC_dtdt.rot = ChQuaternion.Qdtdt_from_AngAxis(mot_rot_dtdt, motion_axis, deltaC.rot, deltaC_dt.rot);
}
}