public override void Start()
{
// shaft1Direction = new Vector3(0, 0, 1);
// shaft2Direction = new Vector3(0, 0, 1);
// line1 = new GameObject().AddComponent<LineRenderer>();
// line2 = new GameObject().AddComponent<LineRenderer>();
ChCoordsys csys = new ChCoordsys(Utils.ToChrono(transform.position), Utils.ToChrono(transform.rotation));
Initialize(body1, body2, csys);
//// TODO: Check that this is correct.
var rot1 = UnityEngine.Quaternion.Euler(shaft1Direction);
//var rot1 = Quaternion.LookRotation(shaft1Direction.normalized);
Set_local_shaft1(new ChFrame <double>(Utils.ToChrono(shaft1Origin), Utils.ToChrono(rot1)));
var rot2 = UnityEngine.Quaternion.Euler(shaft2Direction);
//var rot2 = Quaternion.LookRotation(shaft2Direction.normalized);
Set_local_shaft2(new ChFrame <double>(Utils.ToChrono(shaft2Origin), Utils.ToChrono(rot2)));
var height = 2 * transform.localScale.y;
var pulley2height = Body2.GetType();
Set_r1(4);
Set_r2(2);
//ChSystem msystem = FindObjectOfType<ChSystem>();
// msystem.AddLink(this);
ChSystem.system.AddLink(this);
}
public override void Start()
{
// shaft1Direction = new Vector3(0, 0, 1);
// shaft2Direction = new Vector3(0, 0, 1);
ChCoordsys csys = new ChCoordsys(Utils.ToChrono(transform.position), Utils.ToChrono(transform.rotation));
Initialize(body1, body2, csys);
//// TODO: Check that this is correct.
var rot1 = UnityEngine.Quaternion.Euler(shaft1Direction);
//var rot1 = Quaternion.LookRotation(shaft1Direction.normalized);
Set_local_shaft1(new ChFrame <double>(Utils.ToChrono(shaft1Origin), Utils.ToChrono(rot1)));
var rot2 = UnityEngine.Quaternion.Euler(shaft2Direction);
//var rot2 = Quaternion.LookRotation(shaft2Direction.normalized);
Set_local_shaft2(new ChFrame <double>(Utils.ToChrono(shaft2Origin), Utils.ToChrono(rot2)));
Set_tau(transmissionRatio);
Set_epicyclic(epicyclic);
Set_checkphase(checkphase);
//ChSystem msystem = FindObjectOfType<ChSystem>();
// msystem.AddLink(this);
ChSystem.system.AddLink(this);
}
/// 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);
}
public ChLinkMarkers(ChLinkMarkers other) : base(other)
{
marker1 = null;
marker2 = null;
markID1 = other.markID1;
markID2 = other.markID2;
relM = other.relM; // copy vars
relM_dt = other.relM_dt;
relM_dtdt = other.relM_dtdt;
relAngle = other.relAngle;
relRotaxis = other.relRotaxis;
relAxis = other.relAxis;
relWvel = other.relWvel;
relWacc = other.relWacc;
dist = other.dist;
dist_dt = other.dist_dt;
C_force = other.C_force;
C_torque = other.C_torque;
Scr_force = other.Scr_force;
Scr_torque = other.Scr_torque;
}
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 Start()
{
ChCoordsys csys = new ChCoordsys(Utils.ToChrono(transform.position), Utils.ToChrono(transform.rotation));
Initialize(body1.BodyFrame, body2.BodyFrame, csys, m_dist);
ChSystem.system.AddLink(this);
}
/// Get the contact coordinate system, expressed in absolute frame.
/// This represents the 'main' reference of the link: reaction forces
/// are expressed in this coordinate system. Its origin is point P2.
/// (It is the coordinate system of the contact plane and normal)
public ChCoordsys GetContactCoords()
{
ChCoordsys mcsys = ChCoordsys.CSYSNULL;
ChQuaternion mrot = this.contact_plane.Get_A_quaternion();
mcsys.rot.Set(mrot.e0, mrot.e1, mrot.e2, mrot.e3);
mcsys.pos = this.p2;
return(mcsys);
}
/// 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;
}
/// Set body-relative coord. and update auxiliary variables
/// Also, current position becomes the 'resting position' coordinates
/// for the current time.
public void Impose_Rel_Coord(ChCoordsys m_coord)
{
ChQuaternion qtemp;// = new ChQuaternion(1, 0, 0, 0);
// set the actual coordinates
FrameMoving.SetCoord(m_coord);
// set the resting position coordinates
rest_coord.pos.x = m_coord.pos.x - motion_X.Get_y(ChTime);
rest_coord.pos.y = m_coord.pos.y - motion_Y.Get_y(ChTime);
rest_coord.pos.z = m_coord.pos.z - motion_Z.Get_y(ChTime);
qtemp = ChQuaternion.Q_from_AngAxis2(-(motion_ang.Get_y(ChTime)), motion_axis);
rest_coord.rot = ChQuaternion.Qcross(m_coord.rot, qtemp); // ***%%% check
// set also the absolute positions, and other.
UpdateState();
}
public override void Start()
{
ChCoordsys pos = ChCoordsys.CSYSNULL;
pos.pos.x = transform.position.x;
pos.pos.y = transform.position.y;
pos.pos.z = transform.position.z;
pos.rot.e0 = transform.rotation.w;
pos.rot.e1 = transform.rotation.x;
pos.rot.e2 = transform.rotation.y;
pos.rot.e3 = transform.rotation.z;
//ChFrame<double> frame = new ChFrame<double>(ToChrono(transform.position), ToChrono(transform.rotation));
Initialize(body1, body2, pos);
Set_shaft_mode(shaft_mode);
Set_eng_mode(EngineMode);
// Get a handle to the associated function component and set the motor's function
var fun_component = this.GetComponent <ChFunction>();
switch (eng_mode)
{
case eCh_eng_mode.ENG_MODE_ROTATION:
if (fun_component != null)
{
Set_rot_funct(fun_component);
}
break;
case eCh_eng_mode.ENG_MODE_SPEED:
if (fun_component != null)
{
Set_spe_funct(fun_component);
}
break;
case eCh_eng_mode.ENG_MODE_TORQUE:
if (fun_component != null)
{
Set_tor_funct(fun_component);
}
break;
}
ChSystem msystem = FindObjectOfType <ChSystem>();
msystem.AddLink(this);
//ChSystem.system.AddLink(this);
}
/// Set absolute coordinates and update auxiliary variables
/// Also, current position becomes the 'resting position' coordinates
/// for the current time.
public void Impose_Abs_Coord(ChCoordsys m_coord)
{
ChBody my_body;
my_body = GetBody();
ChCoordsys csys;// = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0));//ChCoordsys.CSYSNULL;
// coordsys: transform the representation from the parent reference frame
// to the local reference frame.
csys.pos = ChTransform <double> .TransformParentToLocal(m_coord.pos, my_body.BodyFrame.GetCoord().pos, my_body.BodyFrame.GetA());
csys.rot = ChQuaternion.Qcross(ChQuaternion.Qconjugate(my_body.BodyFrame.GetCoord().rot), m_coord.rot);
// apply the imposition on local coordinate and resting coordinate:
Impose_Rel_Coord(csys);
}
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 ChLinkMarkers()
{
marker1 = null;
marker2 = null;
markID1 = 0;
markID2 = 0;
relM = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0)); //Coordsys.CSYSNORM;
relM_dt = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(0, 0, 0, 0)); //ChCoordsys.CSYSNULL;
relM_dtdt = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(0, 0, 0, 0)); // ChCoordsys.CSYSNULL;
relAngle = 0;
relRotaxis = new ChVector(0, 0, 0);
relWvel = new ChVector(0, 0, 0);
relWacc = new ChVector(0, 0, 0);
C_force = new ChVector(0, 0, 0);
C_torque = new ChVector(0, 0, 0);
Scr_force = new ChVector(0, 0, 0);
Scr_torque = new ChVector(0, 0, 0);
}
public void Start()
{
ChCoordsys csys = new ChCoordsys(Utils.ToChrono(transform.position), Utils.ToChrono(transform.rotation));
Initialize(body1, body2, csys);
// Get a handle to the associated function component and set the motor's function
var fun_component = this.GetComponent <TorqueFunctor>();
if (fun_component != null)
{
RegisterTorqueFunctor(fun_component);
}
ChSystem msystem = FindObjectOfType <ChSystem>();
msystem.AddLink(this);
}
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;
}
/// Initialize this joint by specifying the two bodies to be connected, a
/// coordinate system specified in the absolute frame, and the distance of
/// the massless connector. The composite joint is ructed such that the
/// direction of the revolute joint is aligned with the z axis of the specified
/// coordinate system and the spherical joint is at the specified distance
/// along the x axis.
public void Initialize(ChBodyFrame body1, //< first frame (revolute side)
ChBodyFrame body2, //< second frame (spherical side)
ChCoordsys csys, //< joint coordinate system (in absolute frame)
double distance //< imposed distance
)
{
Body1 = body1;
Body2 = body2;
m_cnstr_dist.SetVariables(Body1.Variables(), Body2.Variables());
m_cnstr_dot.SetVariables(Body1.Variables(), Body2.Variables());
ChVector x_Axis = csys.rot.GetXaxis();
ChVector z_axis = csys.rot.GetZaxis();
m_pos1 = Body1.TransformPointParentToLocal(csys.pos);
m_dir1 = Body1.TransformDirectionParentToLocal(z_axis);
m_pos2 = Body2.TransformPointParentToLocal(csys.pos + distance * x_Axis);
m_dist = distance;
m_cur_dist = distance;
m_cur_dot = 0;
}
// -----------------------------------------------------------------------------
// Draw a spring in 3D space, with given color.
// -----------------------------------------------------------------------------
public static void drawSpring(double radius,
ChVector start,
ChVector end,
int mresolution,
double turns)
{
ChMatrix33 <double> rel_matrix = new ChMatrix33 <double>(0);
ChVector dist = end - start;
ChVector Vx = new ChVector(0, 0, 0);
ChVector Vy = new ChVector(0, 0, 0);
ChVector Vz = new ChVector(0, 0, 0);
double length = dist.Length();
ChVector dir = ChVector.Vnorm(dist);
ChVector.XdirToDxDyDz(dir, ChVector.VECT_Y, ref Vx, ref Vy, ref Vz);
rel_matrix.Set_A_axis(Vx, Vy, Vz);
ChQuaternion Q12 = rel_matrix.Get_A_quaternion();
ChCoordsys mpos = new ChCoordsys(start, Q12);
double phaseA = 0;
double phaseB = 0;
double heightA = 0;
double heightB = 0;
for (int iu = 1; iu <= mresolution; iu++)
{
phaseB = turns * ChMaths.CH_C_2PI * (double)iu / (double)mresolution;
heightB = length * ((double)iu / mresolution);
ChVector V1 = new ChVector(heightA, radius * Math.Cos(phaseA), radius * Math.Sin(phaseA));
ChVector V2 = new ChVector(heightB, radius * Math.Cos(phaseB), radius * Math.Sin(phaseB));
Gizmos.color = new Color(255, 255, 0);
Gizmos.DrawLine(new Vector3((float)mpos.TransformLocalToParent(V1).x, (float)mpos.TransformLocalToParent(V1).y, (float)mpos.TransformLocalToParent(V1).z),
new Vector3((float)mpos.TransformLocalToParent(V2).x, (float)mpos.TransformLocalToParent(V2).y, (float)mpos.TransformLocalToParent(V2).z));
phaseA = phaseB;
heightA = heightB;
}
}
/// 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);
}
}
//
// 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);
}
}
/// Set the speed of translation and rotation (as a ChCoordsys) of the marker
/// respect to the absolute coordinates.
/// NOTE! inner use only, for the moment.
public void SetAbsCoord_dtdt(ChCoordsys newpos_dtdt)
{
abs_frame.SetCoord(newpos_dtdt);
}
/// Set the translation and rotation (as a ChCoordsys) of the marker
/// respect to the absolute coordinates.
/// NOTE! inner use only, for the moment. Use Impose_Abs_Coord() if needed.
public void SetAbsCoord(ChCoordsys newpos)
{
abs_frame.SetCoord(newpos);
}
/// Copy constructor, build from another frame
public ChFrame(ChFrame <Real> other)
{
coord = other.coord;
Amatrix = other.Amatrix;
}
/// Get the link coordinate system in absolute reference.
/// This represents the 'main' reference of the link: reaction forces
/// and reaction torques are expressed in this coordinate system.
/// Child classes should implement this.
public override ChCoordsys GetLinkAbsoluteCoords()
{
return(ChCoordsys.BitShiftRight(GetLinkRelativeCoords(), Body2.GetCoord()));
}
public bool Equals(ChCoordsys other, double tol)
{
return(rot.Equals(other.rot, tol) && pos.Equals(other.pos, tol));
}
/// Returns true if coordsys is identical to other coordsys
public bool Equals(ChCoordsys other)
{
return(rot.Equals(other.rot) && pos.Equals(other.pos));
}
public void PasteCoordsys(ChCoordsys cs, int insrow, int inscol)
{
PasteVector(cs.pos, insrow, inscol);
PasteQuaternion(cs.rot, insrow + 3, inscol);
}
/// Updates motion laws, marker positions, etc.
public override void UpdateTime(double mytime)
{
// First, inherit to parent class
base.UpdateTime(mytime);
ChFrame <double> abs_shaft1 = ChFrame <double> .FNULL; //new ChFrame<double>();
ChFrame <double> abs_shaft2 = ChFrame <double> .FNULL; //new ChFrame<double>();
((ChFrame <double>)Body1).TransformLocalToParent(local_shaft1, abs_shaft1);
((ChFrame <double>)Body2).TransformLocalToParent(local_shaft2, abs_shaft2);
ChVector dcc_w = ChVector.Vsub(Get_shaft_pos2(), Get_shaft_pos1());
// compute actual rotation of the two wheels (relative to truss).
ChVector md1 = abs_shaft1.GetA().MatrT_x_Vect(dcc_w);
md1.z = 0;
md1 = ChVector.Vnorm(md1);
ChVector md2 = abs_shaft2.GetA().MatrT_x_Vect(dcc_w);
md2.z = 0;
md2 = ChVector.Vnorm(md2);
double periodic_a1 = ChMaths.ChAtan2(md1.x, md1.y);
double periodic_a2 = ChMaths.ChAtan2(md2.x, md2.y);
double old_a1 = a1;
double old_a2 = a2;
double turns_a1 = Math.Floor(old_a1 / ChMaths.CH_C_2PI);
double turns_a2 = Math.Floor(old_a2 / ChMaths.CH_C_2PI);
double a1U = turns_a1 * ChMaths.CH_C_2PI + periodic_a1 + ChMaths.CH_C_2PI;
double a1M = turns_a1 * ChMaths.CH_C_2PI + periodic_a1;
double a1L = turns_a1 * ChMaths.CH_C_2PI + periodic_a1 - ChMaths.CH_C_2PI;
a1 = a1M;
if (Math.Abs(a1U - old_a1) < Math.Abs(a1M - old_a1))
{
a1 = a1U;
}
if (Math.Abs(a1L - a1) < Math.Abs(a1M - a1))
{
a1 = a1L;
}
double a2U = turns_a2 * ChMaths.CH_C_2PI + periodic_a2 + ChMaths.CH_C_2PI;
double a2M = turns_a2 * ChMaths.CH_C_2PI + periodic_a2;
double a2L = turns_a2 * ChMaths.CH_C_2PI + periodic_a2 - ChMaths.CH_C_2PI;
a2 = a2M;
if (Math.Abs(a2U - old_a2) < Math.Abs(a2M - old_a2))
{
a2 = a2U;
}
if (Math.Abs(a2L - a2) < Math.Abs(a2M - a2))
{
a2 = a2L;
}
// correct marker positions if phasing is not correct
double m_delta = 0;
if (checkphase)
{
double realtau = tau;
m_delta = a1 - phase - (a2 / realtau);
if (m_delta > ChMaths.CH_C_PI)
{
m_delta -= (ChMaths.CH_C_2PI); // range -180..+180 is better than 0...360
}
if (m_delta > (ChMaths.CH_C_PI / 4.0))
{
m_delta = (ChMaths.CH_C_PI / 4.0); // phase correction only in +/- 45°
}
if (m_delta < -(ChMaths.CH_C_PI / 4.0))
{
m_delta = -(ChMaths.CH_C_PI / 4.0);
}
//***TODO***
}
// Move markers 1 and 2 to align them as pulley ends
ChVector d21_w = dcc_w - Get_shaft_dir1() * ChVector.Vdot(Get_shaft_dir1(), dcc_w);
ChVector D21_w = ChVector.Vnorm(d21_w);
shaft_dist = d21_w.Length();
ChVector U1_w = ChVector.Vcross(Get_shaft_dir1(), D21_w);
double gamma1 = Math.Acos((r1 - r2) / shaft_dist);
ChVector Ru_w = D21_w * Math.Cos(gamma1) + U1_w * Math.Sin(gamma1);
ChVector Rl_w = D21_w * Math.Cos(gamma1) - U1_w * Math.Sin(gamma1);
belt_up1 = Get_shaft_pos1() + Ru_w * r1;
belt_low1 = Get_shaft_pos1() + Rl_w * r1;
belt_up2 = Get_shaft_pos1() + d21_w + Ru_w * r2;
belt_low2 = Get_shaft_pos1() + d21_w + Rl_w * r2;
// marker alignment
ChMatrix33 <double> maU = new ChMatrix33 <double>(0);
ChMatrix33 <double> maL = new ChMatrix33 <double>(0);
ChVector Dxu = ChVector.Vnorm(belt_up2 - belt_up1);
ChVector Dyu = Ru_w;
ChVector Dzu = ChVector.Vnorm(ChVector.Vcross(Dxu, Dyu));
Dyu = ChVector.Vnorm(ChVector.Vcross(Dzu, Dxu));
maU.Set_A_axis(Dxu, Dyu, Dzu);
// ! 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);
marker1.SetMotionType(ChMarker.eChMarkerMotion.M_MOTION_EXTERNAL);
ChCoordsys newmarkpos = new ChCoordsys();
// move marker1 in proper positions
newmarkpos.pos = this.belt_up1;
newmarkpos.rot = maU.Get_A_quaternion();
marker1.Impose_Abs_Coord(newmarkpos); // move marker1 into teeth position
// move marker2 in proper positions
newmarkpos.pos = this.belt_up2;
newmarkpos.rot = maU.Get_A_quaternion();
marker2.Impose_Abs_Coord(newmarkpos); // move marker2 into teeth position
double phase_correction_up = m_delta * r1;
double phase_correction_low = -phase_correction_up;
double hU = ChVector.Vlength(belt_up2 - belt_up1) + phase_correction_up;
double hL = ChVector.Vlength(belt_low2 - belt_low1) + phase_correction_low;
// imposed relative positions/speeds
deltaC.pos = new ChVector(-hU, 0, 0);
deltaC_dt.pos = ChVector.VNULL;
deltaC_dtdt.pos = ChVector.VNULL;
deltaC.rot = ChQuaternion.QUNIT; // no relative rotations imposed!
deltaC_dt.rot = ChQuaternion.QNULL;
deltaC_dtdt.rot = ChQuaternion.QNULL;
}
/// Returns true if coordsys is equal to other coordsys, within a tolerance 'tol'
/* public bool Equals(ChCoordsys other, double tol)
* {
* return rot.Equals(other.rot, tol) && pos.Equals(other.pos, tol);
* }*/
public static ChCoordsys BitShiftRight(ChCoordsys a, ChCoordsys Fb)
{
return(Fb.TransformLocalToParent(a));
}
/// This function transforms a coordsys given in 'this' coordinate system to
/// the parent coordinate system
public ChCoordsys TransformLocalToParent(ChCoordsys local)
{
return(new ChCoordsys(TransformLocalToParent(local.pos), rot % local.rot));
}
/// Copy constructor
public ChCoordsys(ChCoordsys other) : this()
{
pos = other.pos;
rot = other.rot;
}