/// 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;
}
// Updates motion laws, marker positions, etc.
public override void UpdateTime(double mytime)
{
// First, inherit to parent class
base.UpdateTime(mytime);
// If LEARN MODE, just record motion
if (learn)
{
/* do not change deltas, in free mode maybe that 'limit on X' changed them
* deltaC.pos = VNULL;
* deltaC_dt.pos = VNULL;
* deltaC_dtdt.pos = VNULL;
* deltaC.rot = QUNIT;
* deltaC_dt.rot = QNULL;
* deltaC_dtdt.rot = QNULL;
*/
// if (dist_funct.Get_Type() != ChFunction.FunctionType.FUNCT_RECORDER)
// dist_funct = new ChFunction_Recorder();
// record point
double rec_dist = ChVector.Vlength(ChVector.Vsub(marker1.GetAbsCoord().pos, marker2.GetAbsCoord().pos));
rec_dist -= offset;
// (ChFunction_Recorder)(dist_funct).AddPoint(mytime, rec_dist, 1); // (x,y,w) x=t
}
// Move (well, rotate...) marker 2 to align it in actuator direction
// ! Require that the BDF routine of marker won't handle speed and acc.calculus of the moved marker 2!
marker2.SetMotionType(ChMarker.eChMarkerMotion.M_MOTION_EXTERNAL);
// ChMatrix33<double> ma = new ChMatrix33<double>(0);
ma.Set_A_quaternion(marker2.GetAbsCoord().rot);
ChVector absdist = ChVector.Vsub(marker1.GetAbsCoord().pos, marker2.GetAbsCoord().pos);
ChVector mx = ChVector.Vnorm(absdist);
ChVector my = ma.Get_A_Yaxis();
if (ChVector.Vequal(mx, my))
{
if (mx.x == 1.0)
{
my = ChVector.VECT_Y;
}
else
{
my = ChVector.VECT_X;
}
}
ChVector mz = ChVector.Vnorm(ChVector.Vcross(mx, my));
my = ChVector.Vnorm(ChVector.Vcross(mz, mx));
ma.Set_A_axis(mx, my, mz);
ChCoordsys newmarkpos;
ChVector oldpos = marker2.FrameMoving.GetPos(); // backup to avoid numerical err.accumulation
newmarkpos.pos = marker2.GetAbsCoord().pos;
newmarkpos.rot = ma.Get_A_quaternion();
marker2.Impose_Abs_Coord(newmarkpos); // rotate "main" marker2 into tangent position (may add err.accumulation)
marker2.FrameMoving.SetPos(oldpos); // backup to avoid numerical err.accumulation
if (learn)
{
return; // no need to go on further...--.>>>
}
// imposed relative positions/speeds
deltaC.pos = ChVector.VNULL;
deltaC.pos.x = dist_funct.Get_y(ChTime) + offset; // distance is always on M2 'X' axis
deltaC_dt.pos = ChVector.VNULL;
deltaC_dt.pos.x = dist_funct.Get_y_dx(ChTime); // distance speed
deltaC_dtdt.pos = ChVector.VNULL;
deltaC_dtdt.pos.x = dist_funct.Get_y_dxdx(ChTime); // distance acceleration
// add also the centripetal acceleration if distance vector's rotating,
// as centripetal acc. of point sliding on a sphere surface.
ChVector tang_speed = GetRelM_dt().pos;
tang_speed.x = 0; // only z-y coords in relative tang speed vector
double len_absdist = ChVector.Vlength(absdist); // don't divide by zero
if (len_absdist > 1E-6)
{
deltaC_dtdt.pos.x -= Math.Pow(ChVector.Vlength(tang_speed), 2) / ChVector.Vlength(absdist); // An = Adelta -(Vt^2 / r)
}
deltaC.rot = ChQuaternion.QUNIT; // no relative rotations imposed!
deltaC_dt.rot = ChQuaternion.QNULL;
deltaC_dtdt.rot = ChQuaternion.QNULL;
// Compute motor variables
// double m_rotation;
// double m_torque;
mot_rerot = (deltaC.pos.x - offset) / mot_tau;
mot_rerot_dt = deltaC_dt.pos.x / mot_tau;
mot_rerot_dtdt = deltaC_dtdt.pos.x / mot_tau;
mot_retorque = mot_rerot_dtdt * mot_inertia + (react_force.x * mot_tau) / mot_eta;
// m_rotation = (deltaC.pos.x() - offset) / mot_tau;
// m_torque = (deltaC_dtdt.pos.x() / mot_tau) * mot_inertia + (react_force.x() * mot_tau) / mot_eta;
if (learn_torque_rotation)
{
// if (mot_torque.Get_Type() != ChFunction.FunctionType.FUNCT_RECORDER)
// mot_torque = new ChFunction_Recorder();
// if (mot_rot.Get_Type() != ChFunction.FunctionType.FUNCT_RECORDER)
// mot_rot = new ChFunction_Recorder();
// std::static_pointer_cast<ChFunction_Recorder>(mot_torque).AddPoint(mytime, mot_retorque, 1); // (x,y,w) x=t
// std::static_pointer_cast<ChFunction_Recorder>(mot_rot).AddPoint(mytime, mot_rerot, 1); // (x,y,w) x=t
}
}
