/// Initialization based on passing two vectors (point + dir) on the
/// two bodies, they will represent the X axes of the two frames (Y and Z will
/// be built from the X vector via Gram Schmidt orthonormalization).
/// Use the other ChLinkMateGeneric::Initialize() if you want to set the two frames directly.
public virtual void Initialize(ChBodyFrame mbody1, //< first body to link
ChBodyFrame mbody2, //< second body to link
bool pos_are_relative, //< true: following pos. are relative to bodies.
ChVector mpt1, //< origin of slave frame 1, for 1st body (rel. or abs., see flag above)
ChVector mpt2, //< origin of master frame 2, for 2nd body (rel. or abs., see flag above)
ChVector mnorm1, //< X axis of slave plane, for 1st body (rel. or abs., see flag above)
ChVector mnorm2 //< X axis of master plane, for 2nd body (rel. or abs., see flag above)
)
{
Debug.Assert(mbody1 != mbody2);
this.Body1 = mbody1;
this.Body2 = mbody2;
// this.SetSystem(mbody1.GetSystem());
this.mask.SetTwoBodiesVariables(Body1.Variables(), Body2.Variables());
ChVector mx = new ChVector(0, 0, 0);
ChVector my = new ChVector(0, 0, 0);
ChVector mz = new ChVector(0, 0, 0);
ChVector mN = new ChVector(0, 0, 0);
ChMatrix33 <double> mrot = new ChMatrix33 <double>();
ChFrame <double> mfr1 = new ChFrame <double>();
ChFrame <double> mfr2 = new ChFrame <double>();
if (pos_are_relative)
{
mN = mnorm1;
mN.DirToDxDyDz(ref mx, ref my, ref mz, new ChVector(0, 1, 0));
mrot.Set_A_axis(mx, my, mz);
mfr1.SetRot(mrot);
mfr1.SetPos(mpt1);
mN = mnorm2;
mN.DirToDxDyDz(ref mx, ref my, ref mz, new ChVector(0, 1, 0));
mrot.Set_A_axis(mx, my, mz);
mfr2.SetRot(mrot);
mfr2.SetPos(mpt2);
}
else
{
ChVector temp = ChVector.VECT_Z;
// from abs to body-rel
mN = this.Body1.TransformDirectionParentToLocal(mnorm1);
mN.DirToDxDyDz(ref mx, ref my, ref mz, temp);
mrot.Set_A_axis(mx, my, mz);
mfr1.SetRot(mrot);
mfr1.SetPos(this.Body1.TransformPointParentToLocal(mpt1));
mN = this.Body2.TransformDirectionParentToLocal(mnorm2);
mN.DirToDxDyDz(ref mx, ref my, ref mz, temp);
mrot.Set_A_axis(mx, my, mz);
mfr2.SetRot(mrot);
mfr2.SetPos(this.Body2.TransformPointParentToLocal(mpt2));
}
this.frame1 = mfr1;
this.frame2 = mfr2;
}
//
// FUNCTIONS
//
/// Initialize again this constraint.
public virtual void Reset(Ta mobjA, //< ChContactable object A
Tb mobjB, //< ChContactable object B
collision.ChCollisionInfo cinfo //< data for the contact pair
)
{
//Debug.Assert(mobjA);
//Debug.Assert(mobjB);
this.objA = mobjA;
this.objB = mobjB;
this.p1 = cinfo.vpA;
this.p2 = cinfo.vpB;
this.normal = cinfo.vN;
this.norm_dist = cinfo.distance;
this.eff_radius = cinfo.eff_radius;
// Contact plane
ChVector Vx = new ChVector(0, 0, 0);
ChVector Vy = new ChVector(0, 0, 0);
ChVector Vz = new ChVector(0, 0, 0);
ChVector.XdirToDxDyDz(normal, ChVector.VECT_Y, ref Vx, ref Vy, ref Vz);
contact_plane.Set_A_axis(Vx, Vy, Vz);
}
/// Get the link coordinate system, expressed relative to Body2 (spherical side).
/// This represents the 'main' reference of the link: reaction forces
/// and reaction torques are reported in this coordinate system.
public override ChCoordsys GetLinkRelativeCoords()
{
ChVector pos1 = Body2.TransformPointParentToLocal(Body1.TransformPointLocalToParent(m_pos1));
ChMatrix33 <double> A = new ChMatrix33 <double>(0);
ChVector u = (m_pos2 - pos1).GetNormalized();
ChVector w = Body2.TransformDirectionParentToLocal(Body1.TransformDirectionLocalToParent(m_dir1));
ChVector v = ChVector.Vcross(w, u);
A.Set_A_axis(u, v, w);
return(new ChCoordsys(pos1, A.Get_A_quaternion()));
}
/// Get the link coordinate system, expressed relative to Body2 (the 'master'
/// body). This represents the 'main' reference of the link: reaction forces
/// are expressed in this coordinate system.
/// (It is the coordinate system of the contact plane relative to Body2)
public override ChCoordsys GetLinkRelativeCoords()
{
//ChVector D2local;
ChVector D2temp = (ChVector.Vnorm(Body1.TransformPointLocalToParent(pos1) - Body2.TransformPointLocalToParent(pos2)));
ChVector D2rel = Body2.TransformDirectionParentToLocal(D2temp);
ChVector Vx = new ChVector(0, 0, 0), Vy = new ChVector(0, 0, 0), Vz = new ChVector(0, 0, 0);
//ChMatrix33<double> rel_matrix = new ChMatrix33<double>();
ChVector.XdirToDxDyDz(D2rel, ChVector.VECT_Y, ref Vx, ref Vy, ref Vz);
rel_matrix.Set_A_axis(Vx, Vy, Vz);
ChQuaternion Ql2 = rel_matrix.Get_A_quaternion();
return(new ChCoordsys(pos2, Ql2));
}
// -----------------------------------------------------------------------------
// 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;
}
}
/// 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, 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
}
}
// 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;
}
