/// 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());
}
/// Initialize this joint by specifying the two bodies to be connected, a point
/// and a direction on body1 defining the revolute joint, and a point on the
/// second body defining the spherical joint. If local = true, it is assumed
/// that these quantities are specified in the local body frames. Otherwise,
/// it is assumed that they are specified in the absolute frame. The imposed
/// distance between the two points can be either inferred from the provided
/// configuration (auto_distance = true) or specified explicitly.
public void Initialize(ChBodyFrame body1, //< first frame (revolute side)
ChBodyFrame body2, //< second frame (spherical side)
bool local, //< true if data given in body local frames
ChVector pos1, //< point on first frame (center of revolute)
ChVector dir1, //< direction of revolute on first frame
ChVector pos2, //< point on second frame (center of spherical)
bool auto_distance = true, //< true if imposed distance equal to |pos1 - po2|
double distance = 0 //< imposed distance (used only if auto_distance = false)
)
{
Body1 = body1;
Body2 = body2;
m_cnstr_dist.SetVariables(Body1.Variables(), Body2.Variables());
m_cnstr_dot.SetVariables(Body1.Variables(), Body2.Variables());
ChVector pos1_abs;
ChVector pos2_abs;
ChVector dir1_abs;
if (local)
{
m_pos1 = pos1;
m_pos2 = pos2;
m_dir1 = ChVector.Vnorm(dir1);
pos1_abs = Body1.TransformPointLocalToParent(m_pos1);
pos2_abs = Body2.TransformPointLocalToParent(m_pos2);
dir1_abs = Body1.TransformDirectionLocalToParent(m_dir1);
}
else
{
pos1_abs = pos1;
pos2_abs = pos2;
dir1_abs = ChVector.Vnorm(dir1);
m_pos1 = Body1.TransformPointParentToLocal(pos1_abs);
m_pos2 = Body2.TransformPointParentToLocal(pos2_abs);
m_dir1 = Body1.TransformDirectionParentToLocal(dir1_abs);
}
ChVector d12_abs = pos2_abs - pos1_abs;
m_cur_dist = d12_abs.Length();
m_dist = auto_distance ? m_cur_dist : distance;
m_cur_dot = ChVector.Vdot(d12_abs, dir1_abs);
}
/// Include the rotational spring custom torque.
public override void UpdateForces(double time)
{
// Allow the base class to update itself (possibly adding its own forces).
base.UpdateForces(time);
// Invoke the provided functor to evaluate torque.
// NOTE: we implicitly assume that the kinematics are CONSISTENT with this
// type of link!
double angle = relAngle;
double angle_dt = ChVector.Vdot(relWvel, relAxis);
if (m_torque_fun != null)
{
m_torque = (m_torque_fun)[time, relAngle, angle_dt, this];
}
// Add to existing torque.
C_torque += ChVector.Vmul(relAxis, m_torque);
}
/// Initialize this joint by specifying the two bodies to be connected and the
/// joint frames on each body. If local = true, it is assumed that these quantities
/// are specified in the local body frames. Otherwise, it is assumed that they are
/// specified in the absolute frame.
public void Initialize(ChBodyFrame body1, //< first body frame
ChBodyFrame body2, //< second body frame
bool local, //< true if data given in body local frames
ChFrame <double> frame1, //< joint frame on body 1
ChFrame <double> frame2 //< joint frame on body 2
)
{
Body1 = body1;
Body2 = body2;
m_cnstr_x.SetVariables(Body1.Variables(), Body2.Variables());
m_cnstr_y.SetVariables(Body1.Variables(), Body2.Variables());
m_cnstr_z.SetVariables(Body1.Variables(), Body2.Variables());
m_cnstr_dot.SetVariables(Body1.Variables(), Body2.Variables());
ChFrame <double> frame1_abs;
ChFrame <double> frame2_abs;
if (local)
{
m_frame1 = frame1;
m_frame2 = frame2;
frame1_abs = ChFrame <double> .BitShiftRight(frame1, Body1);
frame2_abs = ChFrame <double> .BitShiftRight(frame2, Body2);
}
else
{
((ChFrame <double>)Body1).TransformParentToLocal(frame1, m_frame1);
((ChFrame <double>)Body2).TransformParentToLocal(frame2, m_frame2);
frame1_abs = frame1;
frame2_abs = frame2;
}
m_u1_tilde.Set_X_matrix(m_frame1.GetA().Get_A_Xaxis());
m_v2_tilde.Set_X_matrix(m_frame2.GetA().Get_A_Yaxis());
m_C.matrix.SetElement(0, 0, frame2_abs.coord.pos.x - frame1_abs.coord.pos.x);
m_C.matrix.SetElement(1, 0, frame2_abs.coord.pos.y - frame1_abs.coord.pos.y);
m_C.matrix.SetElement(2, 0, frame2_abs.coord.pos.z - frame1_abs.coord.pos.z);
m_C.matrix.SetElement(3, 0, ChVector.Vdot(frame1_abs.GetA().Get_A_Xaxis(), frame2_abs.GetA().Get_A_Yaxis()));
}
/// 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;
}
/// 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);
}
}
public override void UpdateForces(double mytime)
{
// First, inherit to parent class
base.UpdateForces(mytime);
if (this.IsDisabled())
{
return;
}
// then, if not sticking,
if (this.brake_torque != 0)
{
if (brake_mode == eChBrmode.BRAKE_ROTATION)
{
if (((ChLinkMaskLF)mask).Constr_E3().IsActive() == false)
{
int mdir;
ChVector mv_torque = ChVector.Vmul(ChVector.VECT_Z, this.brake_torque);
mdir = 0; // clockwise torque
if (ChVector.Vdot(this.relWvel, mv_torque) > 0.0)
{
mv_torque = ChVector.Vmul(mv_torque, -1.0); // keep torque always opposed to ang speed.
mdir = 1; // counterclockwise torque
}
if (mdir != this.last_dir)
{
this.must_stick = true;
}
this.last_dir = mdir;
// +++ADD TO LINK TORQUE VECTOR
C_torque = ChVector.Vadd(C_torque, mv_torque);
}
}
if (brake_mode == eChBrmode.BRAKE_TRANSLATEX)
{
if (((ChLinkMaskLF)mask).Constr_X().IsActive() == false)
{
int mdir;
ChVector mv_force = ChVector.Vmul(ChVector.VECT_X, this.brake_torque);
mdir = 0; // F-. rear motion: frontfacing break force
if (this.relM_dt.pos.x > 0.0)
{
mv_force = ChVector.Vmul(mv_force, -1.0); // break force always opposed to speed
mdir = 1; // F<-- backfacing breakforce for front motion
}
if (mdir != this.last_dir)
{
this.must_stick = true;
}
this.last_dir = mdir;
// +++ADD TO LINK TORQUE VECTOR
C_force = ChVector.Vadd(C_force, mv_force);
}
}
}
// turn off sticking feature if stick ration not > 1.0
if (this.stick_ratio <= 1.0)
{
must_stick = false;
}
}
/// Get the master coordinate system for the assets (this will return the
/// absolute coordinate system of the 'master' marker2)
// public override ChFrame<double> GetAssetsFrame(int nclone = 0) { return marker2.GetAbsFrame(); }
//
// UPDATING FUNCTIONS
//
/// Updates auxiliary vars relM, relM_dt, relM_dtdt,
/// dist, dist_dt et simila.
public virtual void UpdateRelMarkerCoords()
{
// FOR ALL THE 6(or3) COORDINATES OF RELATIVE MOTION OF THE TWO MARKERS:
// COMPUTE THE relM, relM_dt relM_dtdt COORDINATES, AND AUXILIARY DATA (distance,etc.)
ChVector PQw = ChVector.Vsub(marker1.GetAbsCoord().pos, marker2.GetAbsCoord().pos);
ChVector PQw_dt = ChVector.Vsub(marker1.GetAbsCoord_dt().pos, marker2.GetAbsCoord_dt().pos);
ChVector PQw_dtdt = ChVector.Vsub(marker1.GetAbsCoord_dtdt().pos, marker2.GetAbsCoord_dtdt().pos);
dist = ChVector.Vlength(PQw); // distance between origins, modulus
dist_dt = ChVector.Vdot(ChVector.Vnorm(PQw), PQw_dt); // speed between origins, modulus.
ChVector vtemp1; // for intermediate calculus
ChVector vtemp2;
ChQuaternion qtemp1;
// ChMatrixNM<IntInterface.Three, IntInterface.Four> relGw = new ChMatrixNM<IntInterface.Three, IntInterface.Four>(0);
/* ChQuaternion q_AD;
* ChQuaternion q_BC;
* ChQuaternion q_8;
* ChVector q_4;*/
ChQuaternion temp1 = marker1.FrameMoving.GetCoord_dt().rot;
ChQuaternion temp2 = marker2.FrameMoving.GetCoord_dt().rot;
if (ChQuaternion.Qnotnull(temp1) || ChQuaternion.Qnotnull(temp2))
{
q_AD = // q'qqq + qqqq'
ChQuaternion.Qadd(ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord_dt().rot),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.GetBody().BodyFrame.GetCoord().rot),
ChQuaternion.Qcross((marker1.GetBody().BodyFrame.GetCoord().rot), (marker1.FrameMoving.GetCoord().rot)))),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord().rot),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.GetBody().BodyFrame.GetCoord().rot),
ChQuaternion.Qcross((marker1.GetBody().BodyFrame.GetCoord().rot), (marker1.FrameMoving.GetCoord_dt().rot)))));
}
else
{
//q_AD = ChQuaternion.QNULL;
q_BC = // qq'qq + qqq'q
ChQuaternion.Qadd(ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord().rot),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.GetBody().BodyFrame.GetCoord_dt().rot),
ChQuaternion.Qcross((marker1.GetBody().BodyFrame.GetCoord().rot), (marker1.FrameMoving.GetCoord().rot)))),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord().rot),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.GetBody().BodyFrame.GetCoord().rot),
ChQuaternion.Qcross((marker1.GetBody().BodyFrame.GetCoord_dt().rot), (marker1.FrameMoving.GetCoord().rot)))));
}
// q_8 = q''qqq + 2q'q'qq + 2q'qq'q + 2q'qqq'
// + 2qq'q'q + 2qq'qq' + 2qqq'q' + qqqq''
temp2 = marker2.FrameMoving.GetCoord_dtdt().rot;
if (ChQuaternion.Qnotnull(temp2))
{
q_8 = ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord_dtdt().rot),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(Body2.GetCoord().rot),
ChQuaternion.Qcross(Body1.GetCoord().rot,
marker1.FrameMoving.GetCoord().rot))); // q_dtdt'm2 * q'o2 * q,o1 * q,m1
}
else
{
//q_8 = ChQuaternion.QNULL;
temp1 = marker1.FrameMoving.GetCoord_dtdt().rot;
}
if (ChQuaternion.Qnotnull(temp1))
{
qtemp1 = ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord().rot),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(Body2.GetCoord().rot),
ChQuaternion.Qcross(Body1.GetCoord().rot,
marker1.FrameMoving.GetCoord_dtdt().rot))); // q'm2 * q'o2 * q,o1 * q_dtdt,m1
q_8 = ChQuaternion.Qadd(q_8, qtemp1);
}
temp2 = marker2.FrameMoving.GetCoord_dt().rot;
if (ChQuaternion.Qnotnull(temp2))
{
qtemp1 = ChQuaternion.Qcross(
ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord_dt().rot),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(Body2.GetCoord_dt().rot), ChQuaternion.Qcross(Body1.GetCoord().rot, marker1.FrameMoving.GetCoord().rot)));
qtemp1 = ChQuaternion.Qscale(qtemp1, 2); // 2( q_dt'm2 * q_dt'o2 * q,o1 * q,m1)
q_8 = ChQuaternion.Qadd(q_8, qtemp1);
}
temp2 = marker2.FrameMoving.GetCoord_dt().rot;
if (ChQuaternion.Qnotnull(temp2))
{
qtemp1 = ChQuaternion.Qcross(
ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord_dt().rot),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(Body2.GetCoord().rot), ChQuaternion.Qcross(Body1.GetCoord_dt().rot, marker1.FrameMoving.GetCoord().rot)));
qtemp1 = ChQuaternion.Qscale(qtemp1, 2); // 2( q_dt'm2 * q'o2 * q_dt,o1 * q,m1)
q_8 = ChQuaternion.Qadd(q_8, qtemp1);
}
temp1 = marker1.FrameMoving.GetCoord_dt().rot;
temp2 = marker2.FrameMoving.GetCoord_dt().rot;
if (ChQuaternion.Qnotnull(temp2) && ChQuaternion.Qnotnull(temp1))
{
qtemp1 = ChQuaternion.Qcross(
ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord_dt().rot),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(Body2.GetCoord().rot), ChQuaternion.Qcross(Body1.GetCoord().rot, marker1.FrameMoving.GetCoord_dt().rot)));
qtemp1 = ChQuaternion.Qscale(qtemp1, 2); // 2( q_dt'm2 * q'o2 * q,o1 * q_dt,m1)
q_8 = ChQuaternion.Qadd(q_8, qtemp1);
}
qtemp1 =
ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord().rot),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(Body2.GetCoord_dt().rot), ChQuaternion.Qcross(Body1.GetCoord_dt().rot, marker1.FrameMoving.GetCoord().rot)));
qtemp1 = ChQuaternion.Qscale(qtemp1, 2); // 2( q'm2 * q_dt'o2 * q_dt,o1 * q,m1)
q_8 = ChQuaternion.Qadd(q_8, qtemp1);
temp1 = marker1.FrameMoving.GetCoord_dt().rot;
if (ChQuaternion.Qnotnull(temp1))
{
qtemp1 = ChQuaternion.Qcross(
ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord().rot),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(Body2.GetCoord_dt().rot), ChQuaternion.Qcross(Body1.GetCoord().rot, marker1.FrameMoving.GetCoord_dt().rot)));
qtemp1 = ChQuaternion.Qscale(qtemp1, 2); // 2( q'm2 * q_dt'o2 * q,o1 * q_dt,m1)
q_8 = ChQuaternion.Qadd(q_8, qtemp1);
}
temp1 = marker1.FrameMoving.GetCoord_dt().rot;
if (ChQuaternion.Qnotnull(temp1))
{
qtemp1 = ChQuaternion.Qcross(
ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord().rot),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(Body2.GetCoord().rot), ChQuaternion.Qcross(Body1.GetCoord_dt().rot, marker1.FrameMoving.GetCoord_dt().rot)));
qtemp1 = ChQuaternion.Qscale(qtemp1, 2); // 2( q'm2 * q'o2 * q_dt,o1 * q_dt,m1)
q_8 = ChQuaternion.Qadd(q_8, qtemp1);
}
// q_4 = [Adtdt]'[A]'q + 2[Adt]'[Adt]'q
// + 2[Adt]'[A]'qdt + 2[A]'[Adt]'qdt
// ChMatrix33<double> m2_Rel_A_dt = new ChMatrix33<double>(0);
marker2.FrameMoving.Compute_Adt(ref m2_Rel_A_dt);
// ChMatrix33<double> m2_Rel_A_dtdt = new ChMatrix33<double>(0);
marker2.FrameMoving.Compute_Adtdt(ref m2_Rel_A_dtdt);
vtemp1 = Body2.GetA_dt().MatrT_x_Vect(PQw);
vtemp2 = m2_Rel_A_dt.MatrT_x_Vect(vtemp1);
q_4 = ChVector.Vmul(vtemp2, 2); // 2[Aq_dt]'[Ao2_dt]'*Qpq,w
vtemp1 = Body2.GetA().MatrT_x_Vect(PQw_dt);
vtemp2 = m2_Rel_A_dt.MatrT_x_Vect(vtemp1);
vtemp2 = ChVector.Vmul(vtemp2, 2); // 2[Aq_dt]'[Ao2]'*Qpq,w_dt
q_4 = ChVector.Vadd(q_4, vtemp2);
vtemp1 = Body2.GetA_dt().MatrT_x_Vect(PQw_dt);
vtemp2 = marker2.FrameMoving.GetA().MatrT_x_Vect(vtemp1);
vtemp2 = ChVector.Vmul(vtemp2, 2); // 2[Aq]'[Ao2_dt]'*Qpq,w_dt
q_4 = ChVector.Vadd(q_4, vtemp2);
vtemp1 = Body2.GetA().MatrT_x_Vect(PQw);
vtemp2 = m2_Rel_A_dtdt.MatrT_x_Vect(vtemp1);
q_4 = ChVector.Vadd(q_4, vtemp2); // [Aq_dtdt]'[Ao2]'*Qpq,w
// ----------- RELATIVE MARKER COORDINATES
// relM.pos
relM.pos = marker2.FrameMoving.GetA().MatrT_x_Vect(Body2.GetA().MatrT_x_Vect(PQw));
// relM.rot
relM.rot = ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord().rot),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.GetBody().BodyFrame.GetCoord().rot),
ChQuaternion.Qcross((marker1.GetBody().BodyFrame.GetCoord().rot), (marker1.FrameMoving.GetCoord().rot))));
// relM_dt.pos
relM_dt.pos = ChVector.Vadd(ChVector.Vadd(m2_Rel_A_dt.MatrT_x_Vect(Body2.GetA().MatrT_x_Vect(PQw)),
marker2.FrameMoving.GetA().MatrT_x_Vect(Body2.GetA_dt().MatrT_x_Vect(PQw))),
marker2.FrameMoving.GetA().MatrT_x_Vect(Body2.GetA().MatrT_x_Vect(PQw_dt)));
// relM_dt.rot
relM_dt.rot = ChQuaternion.Qadd(q_AD, q_BC);
// relM_dtdt.pos
relM_dtdt.pos = ChVector.Vadd(ChVector.Vadd(marker2.FrameMoving.GetA().MatrT_x_Vect(Body2.GetA_dtdt().MatrT_x_Vect(PQw)),
marker2.FrameMoving.GetA().MatrT_x_Vect(Body2.GetA().MatrT_x_Vect(PQw_dtdt))),
q_4);
// relM_dtdt.rot
qtemp1 = ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord().rot),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(Body2.GetCoord_dtdt().rot),
ChQuaternion.Qcross(Body1.GetCoord().rot,
marker1.FrameMoving.GetCoord().rot))); // ( q'm2 * q_dtdt'o2 * q,o1 * q,m1)
relM_dtdt.rot = ChQuaternion.Qadd(q_8, qtemp1);
qtemp1 = ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord().rot),
ChQuaternion.Qcross(ChQuaternion.Qconjugate(Body2.GetCoord().rot),
ChQuaternion.Qcross(Body1.GetCoord_dtdt().rot,
marker1.FrameMoving.GetCoord().rot))); // ( q'm2 * q'o2 * q_dtdt,o1 * q,m1)
relM_dtdt.rot = ChQuaternion.Qadd(relM_dtdt.rot, qtemp1); // = q_8 + qq''qq + qqq''q
// ... and also "user-friendly" relative coordinates:
// relAngle and relAxis
ChQuaternion.Q_to_AngAxis(relM.rot, ref relAngle, ref relAxis);
// flip rel rotation axis if jerky sign
if (relAxis.z < 0)
{
relAxis = ChVector.Vmul(relAxis, -1);
relAngle = -relAngle;
}
// rotation axis
relRotaxis = ChVector.Vmul(relAxis, relAngle);
// relWvel
ChFrame <double> .SetMatrix_Gw(ref relGw, relM.rot); // relGw.Set_Gw_matrix(relM.rot);
relWvel = relGw.matrix.Matr34_x_Quat(relM_dt.rot);
// relWacc
relWacc = relGw.matrix.Matr34_x_Quat(relM_dtdt.rot);
}
//
// UPDATING FUNCTIONS
//
/// Perform the update of this joint at the specified time: compute jacobians
/// and constraint violations, cache in internal structures
public override void update(double time, bool update_assets = true)
{
// Inherit time changes of parent class
base.UpdateTime(time);
// Express the joint frames in absolute frame
ChFrame <double> frame1_abs = m_frame1.BitShiftRight(Body1);
ChFrame <double> frame2_abs = m_frame2.BitShiftRight(Body2);
// Calculate violations of the spherical constraints
m_C.matrix.SetElement(0, 0, frame2_abs.coord.pos.x - frame1_abs.coord.pos.x);
m_C.matrix.SetElement(1, 0, frame2_abs.coord.pos.y - frame1_abs.coord.pos.y);
m_C.matrix.SetElement(2, 0, frame2_abs.coord.pos.z - frame1_abs.coord.pos.z);
// Compute Jacobian of the spherical constraints
// pos2_abs - pos1_abs = 0
{
ChMatrix33 <double> tilde1 = new ChMatrix33 <double>(0);
ChMatrix33 <double> tilde2 = new ChMatrix33 <double>(0);
tilde1.Set_X_matrix(m_frame1.coord.pos);
tilde2.Set_X_matrix(m_frame2.coord.pos);
ChMatrix33 <double> Phi_pi1 = Body1.GetA() * tilde1;
ChMatrix33 <double> Phi_pi2 = Body2.GetA() * tilde2;
m_cnstr_x.Get_Cq_a().ElementN(0) = -1;
m_cnstr_x.Get_Cq_b().ElementN(0) = +1;
m_cnstr_x.Get_Cq_a().ElementN(1) = 0;
m_cnstr_x.Get_Cq_b().ElementN(1) = 0;
m_cnstr_x.Get_Cq_a().ElementN(2) = 0;
m_cnstr_x.Get_Cq_b().ElementN(2) = 0;
m_cnstr_x.Get_Cq_a().ElementN(3) = Phi_pi1.nm.matrix[0, 0];
m_cnstr_x.Get_Cq_b().ElementN(3) = -Phi_pi2.nm.matrix[0, 0];
m_cnstr_x.Get_Cq_a().ElementN(4) = Phi_pi1.nm.matrix[0, 1];
m_cnstr_x.Get_Cq_b().ElementN(4) = -Phi_pi2.nm.matrix[0, 1];
m_cnstr_x.Get_Cq_a().ElementN(5) = Phi_pi1.nm.matrix[0, 2];
m_cnstr_x.Get_Cq_b().ElementN(5) = -Phi_pi2.nm.matrix[0, 2];
m_cnstr_y.Get_Cq_a().ElementN(0) = 0;
m_cnstr_y.Get_Cq_b().ElementN(0) = 0;
m_cnstr_y.Get_Cq_a().ElementN(1) = -1;
m_cnstr_y.Get_Cq_b().ElementN(1) = +1;
m_cnstr_y.Get_Cq_a().ElementN(2) = 0;
m_cnstr_y.Get_Cq_b().ElementN(2) = 0;
m_cnstr_y.Get_Cq_a().ElementN(3) = Phi_pi1.nm.matrix[1, 0];
m_cnstr_y.Get_Cq_b().ElementN(3) = -Phi_pi2.nm.matrix[1, 0];
m_cnstr_y.Get_Cq_a().ElementN(4) = Phi_pi1.nm.matrix[1, 1];
m_cnstr_y.Get_Cq_b().ElementN(4) = -Phi_pi2.nm.matrix[1, 1];
m_cnstr_y.Get_Cq_a().ElementN(5) = Phi_pi1.nm.matrix[1, 2];
m_cnstr_y.Get_Cq_b().ElementN(5) = -Phi_pi2.nm.matrix[1, 2];
m_cnstr_z.Get_Cq_a().ElementN(0) = 0;
m_cnstr_z.Get_Cq_b().ElementN(0) = 0;
m_cnstr_z.Get_Cq_a().ElementN(1) = 0;
m_cnstr_z.Get_Cq_b().ElementN(1) = 0;
m_cnstr_z.Get_Cq_a().ElementN(2) = -1;
m_cnstr_z.Get_Cq_b().ElementN(2) = +1;
m_cnstr_z.Get_Cq_a().ElementN(3) = Phi_pi1.nm.matrix[2, 0];
m_cnstr_z.Get_Cq_b().ElementN(3) = -Phi_pi2.nm.matrix[2, 0];
m_cnstr_z.Get_Cq_a().ElementN(4) = Phi_pi1.nm.matrix[2, 1];
m_cnstr_z.Get_Cq_b().ElementN(4) = -Phi_pi2.nm.matrix[2, 1];
m_cnstr_z.Get_Cq_a().ElementN(5) = Phi_pi1.nm.matrix[2, 2];
m_cnstr_z.Get_Cq_b().ElementN(5) = -Phi_pi2.nm.matrix[2, 2];
}
// Calculate violation of the dot constraint
ChVector u1 = frame1_abs.GetA().Get_A_Xaxis();
ChVector v2 = frame2_abs.GetA().Get_A_Yaxis();
m_C.matrix.SetElement(3, 0, ChVector.Vdot(u1, v2));
// Compute Jacobian of the dot constraint
// dot(u1_abs, v2_abs) = 0
{
ChMatrix33 <double> mat1 = Body1.GetA() * m_u1_tilde;
ChMatrix33 <double> mat2 = Body2.GetA() * m_v2_tilde;
ChVector Phi_pi1 = mat1.MatrT_x_Vect(v2);
ChVector Phi_pi2 = mat2.MatrT_x_Vect(u1);
m_cnstr_dot.Get_Cq_a().ElementN(0) = 0;
m_cnstr_dot.Get_Cq_a().ElementN(1) = 0;
m_cnstr_dot.Get_Cq_a().ElementN(2) = 0;
m_cnstr_dot.Get_Cq_a().ElementN(3) = -Phi_pi1.x;
m_cnstr_dot.Get_Cq_a().ElementN(4) = -Phi_pi1.y;
m_cnstr_dot.Get_Cq_a().ElementN(5) = -Phi_pi1.z;
m_cnstr_dot.Get_Cq_b().ElementN(0) = 0;
m_cnstr_dot.Get_Cq_b().ElementN(1) = 0;
m_cnstr_dot.Get_Cq_b().ElementN(2) = 0;
m_cnstr_dot.Get_Cq_b().ElementN(3) = -Phi_pi2.x;
m_cnstr_dot.Get_Cq_b().ElementN(4) = -Phi_pi2.y;
m_cnstr_dot.Get_Cq_b().ElementN(5) = -Phi_pi2.z;
}
}
//
// UPDATING FUNCTIONS
//
/// Perform the update of this joint at the specified time: compute jacobians,
/// raint violations, etc. and cache in internal structures
public override void update(double time, bool update_assets = true)
{
// Inherit time changes of parent class (ChLink)
base.update(time, update_assets);
// Express the body locations and direction in absolute frame
ChVector pos1_abs = Body1.TransformPointLocalToParent(m_pos1);
ChVector pos2_abs = Body2.TransformPointLocalToParent(m_pos2);
ChVector dir1_abs = Body1.TransformDirectionLocalToParent(m_dir1);
ChVector d12_abs = pos2_abs - pos1_abs;
// Update current distance and dot product
m_cur_dist = d12_abs.Length();
m_cur_dot = ChVector.Vdot(d12_abs, dir1_abs);
// Calculate a unit vector in the direction d12, expressed in absolute frame
// Then express it in the two body frames
ChVector u12_abs = d12_abs / m_cur_dist;
ChVector u12_loc1 = Body1.TransformDirectionParentToLocal(u12_abs);
ChVector u12_loc2 = Body2.TransformDirectionParentToLocal(u12_abs);
// Express the direction vector in the frame of body 2
ChVector dir1_loc2 = Body2.TransformDirectionParentToLocal(dir1_abs);
// Cache violation of the distance constraint
m_C.matrix.SetElement(0, 0, m_cur_dist - m_dist);
// Compute Jacobian of the distance constraint
// ||pos2_abs - pos1_abs|| - dist = 0
{
ChVector Phi_r1 = -u12_abs;
ChVector Phi_pi1 = ChVector.Vcross(u12_loc1, m_pos1);
m_cnstr_dist.Get_Cq_a().ElementN(0) = Phi_r1.x;
m_cnstr_dist.Get_Cq_a().ElementN(1) = Phi_r1.y;
m_cnstr_dist.Get_Cq_a().ElementN(2) = Phi_r1.z;
m_cnstr_dist.Get_Cq_a().ElementN(3) = Phi_pi1.x;
m_cnstr_dist.Get_Cq_a().ElementN(4) = Phi_pi1.y;
m_cnstr_dist.Get_Cq_a().ElementN(5) = Phi_pi1.z;
ChVector Phi_r2 = u12_abs;
ChVector Phi_pi2 = -ChVector.Vcross(u12_loc2, m_pos2);
m_cnstr_dist.Get_Cq_b().ElementN(0) = Phi_r2.x;
m_cnstr_dist.Get_Cq_b().ElementN(1) = Phi_r2.y;
m_cnstr_dist.Get_Cq_b().ElementN(2) = Phi_r2.z;
m_cnstr_dist.Get_Cq_b().ElementN(3) = Phi_pi2.x;
m_cnstr_dist.Get_Cq_b().ElementN(4) = Phi_pi2.y;
m_cnstr_dist.Get_Cq_b().ElementN(5) = Phi_pi2.z;
}
// Cache violation of the dot constraint
m_C.matrix.SetElement(1, 0, m_cur_dot);
// Compute Jacobian of the dot constraint
// dot(dir1_abs, pos2_abs - pos1_abs) = 0
{
ChVector Phi_r1 = -dir1_abs;
ChVector Phi_pi1 = ChVector.Vcross(m_dir1, m_pos1) - ChVector.Vcross(u12_loc1, m_pos1);
m_cnstr_dot.Get_Cq_a().ElementN(0) = Phi_r1.x;
m_cnstr_dot.Get_Cq_a().ElementN(1) = Phi_r1.y;
m_cnstr_dot.Get_Cq_a().ElementN(2) = Phi_r1.z;
m_cnstr_dot.Get_Cq_a().ElementN(3) = Phi_pi1.x;
m_cnstr_dot.Get_Cq_a().ElementN(4) = Phi_pi1.y;
m_cnstr_dot.Get_Cq_a().ElementN(5) = Phi_pi1.z;
ChVector Phi_r2 = dir1_abs;
ChVector Phi_pi2 = -ChVector.Vcross(dir1_loc2, m_pos2);
m_cnstr_dot.Get_Cq_b().ElementN(0) = Phi_r2.x;
m_cnstr_dot.Get_Cq_b().ElementN(1) = Phi_r2.y;
m_cnstr_dot.Get_Cq_b().ElementN(2) = Phi_r2.z;
m_cnstr_dot.Get_Cq_b().ElementN(3) = Phi_pi2.x;
m_cnstr_dot.Get_Cq_b().ElementN(4) = Phi_pi2.y;
m_cnstr_dot.Get_Cq_b().ElementN(5) = Phi_pi2.z;
}
}
/// Get the current relative angular speed about the common rotation axis.
public double GetRotSpringSpeed()
{
return(ChVector.Vdot(relWvel, relAxis));
}
// Updates motion laws, marker positions, etc.
public override void UpdateTime(double mytime)
{
// First, inherit to parent class
base.UpdateTime(mytime);
// Move markers 1 and 2 to align them as gear teeth
ChMatrix33 <double> ma1 = new ChMatrix33 <double>(0);
ChMatrix33 <double> ma2 = new ChMatrix33 <double>(0);
ChMatrix33 <double> mrotma = new ChMatrix33 <double>(0);
ChMatrix33 <double> marot_beta = new ChMatrix33 <double>(0);
ChVector mx;
ChVector my;
ChVector mz;
ChVector mr;
ChVector mmark1;
ChVector mmark2;
ChVector lastX;
ChVector vrota;
ChCoordsys newmarkpos = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0));
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 vbdist = ChVector.Vsub(Get_shaft_pos1(), Get_shaft_pos2());
// ChVector Trad1 = ChVector.Vnorm(ChVector.Vcross(Get_shaft_dir1(), ChVector.Vnorm(ChVector.Vcross(Get_shaft_dir1(), vbdist))));
// ChVector Trad2 = ChVector.Vnorm(ChVector.Vcross(ChVector.Vnorm(ChVector.Vcross(Get_shaft_dir2(), vbdist)), Get_shaft_dir2()));
double dist = ChVector.Vlength(vbdist);
// compute actual rotation of the two wheels (relative to truss).
ChVector md1 = abs_shaft1.GetA().MatrT_x_Vect(-vbdist);
md1.z = 0;
md1 = ChVector.Vnorm(md1);
ChVector md2 = abs_shaft2.GetA().MatrT_x_Vect(-vbdist);
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;
}
// compute new markers coordsystem alignment
my = ChVector.Vnorm(vbdist);
mz = Get_shaft_dir1();
mx = ChVector.Vnorm(ChVector.Vcross(my, mz));
mr = ChVector.Vnorm(ChVector.Vcross(mz, mx));
mz = ChVector.Vnorm(ChVector.Vcross(mx, my));
ChVector mz2, mx2, mr2, my2;
my2 = my;
mz2 = Get_shaft_dir2();
mx2 = ChVector.Vnorm(ChVector.Vcross(my2, mz2));
mr2 = ChVector.Vnorm(ChVector.Vcross(mz2, mx2));
ma1.Set_A_axis(mx, my, mz);
// rotate csys because of beta
vrota.x = 0.0;
vrota.y = beta;
vrota.z = 0.0;
mrotma.Set_A_Rxyz(vrota);
marot_beta.nm.matrix.MatrMultiply(ma1.nm.matrix, mrotma.nm.matrix);
// rotate csys because of alpha
vrota.x = 0.0;
vrota.y = 0.0;
vrota.z = alpha;
if (react_force.x < 0)
{
vrota.z = alpha;
}
else
{
vrota.z = -alpha;
}
mrotma.Set_A_Rxyz(vrota);
ma1.nm.matrix.MatrMultiply(marot_beta.nm.matrix, mrotma.nm.matrix);
ma2.nm.matrix.CopyFromMatrix(ma1.nm.matrix);
// is a bevel gear?
double be = Math.Acos(ChVector.Vdot(Get_shaft_dir1(), Get_shaft_dir2()));
bool is_bevel = true;
if (Math.Abs(ChVector.Vdot(Get_shaft_dir1(), Get_shaft_dir2())) > 0.96)
{
is_bevel = false;
}
// compute wheel radii so that:
// w2 = - tau * w1
if (!is_bevel)
{
double pardist = ChVector.Vdot(mr, vbdist);
double inv_tau = 1.0 / tau;
if (!epicyclic)
{
r2 = pardist - pardist / (inv_tau + 1.0);
}
else
{
r2 = pardist - (tau * pardist) / (tau - 1.0);
}
r1 = r2 * tau;
}
else
{
double gamma2;
if (!epicyclic)
{
gamma2 = be / (tau + 1.0);
}
else
{
gamma2 = be / (-tau + 1.0);
}
double al = ChMaths.CH_C_PI - Math.Acos(ChVector.Vdot(Get_shaft_dir2(), my));
double te = ChMaths.CH_C_PI - al - be;
double fd = Math.Sin(te) * (dist / Math.Sin(be));
r2 = fd * Math.Tan(gamma2);
r1 = r2 * tau;
}
// compute markers positions, supposing they
// stay on the ideal wheel contact point
mmark1 = ChVector.Vadd(Get_shaft_pos2(), ChVector.Vmul(mr2, r2));
mmark2 = mmark1;
contact_pt = mmark1;
// correct marker 1 position if phasing is not correct
if (checkphase)
{
double realtau = tau;
if (epicyclic)
{
realtau = -tau;
}
double m_delta;
m_delta = -(a2 / realtau) - a1 - phase;
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);
}
vrota.x = vrota.y = 0.0;
vrota.z = -m_delta;
mrotma.Set_A_Rxyz(vrota); // rotate about Z of shaft to correct
mmark1 = abs_shaft1.GetA().MatrT_x_Vect(ChVector.Vsub(mmark1, Get_shaft_pos1()));
mmark1 = mrotma.Matr_x_Vect(mmark1);
mmark1 = ChVector.Vadd(abs_shaft1.GetA().Matr_x_Vect(mmark1), Get_shaft_pos1());
}
// Move Shaft 1 along its direction if not aligned to wheel
double offset = ChVector.Vdot(Get_shaft_dir1(), (contact_pt - Get_shaft_pos1()));
ChVector moff = Get_shaft_dir1() * offset;
if (Math.Abs(offset) > 0.0001)
{
local_shaft1.SetPos(local_shaft1.GetPos() + Body1.TransformDirectionParentToLocal(moff));
}
// ! 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);
// move marker1 in proper positions
newmarkpos.pos = mmark1;
newmarkpos.rot = ma1.Get_A_quaternion();
marker1.Impose_Abs_Coord(newmarkpos); // move marker1 into teeth position
// move marker2 in proper positions
newmarkpos.pos = mmark2;
newmarkpos.rot = ma2.Get_A_quaternion();
marker2.Impose_Abs_Coord(newmarkpos); // move marker2 into teeth position
// imposed relative positions/speeds
deltaC.pos = ChVector.VNULL;
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;
}
