/// 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;
}
/// Multiplies this ChMatrix33 matrix and another ChMatrix33 matrix.
/// Performance warning: a new object is created.
public static ChMatrix33 <double> operator *(ChMatrix33 <Real> matbis, ChMatrix33 <Real> matbis2)
{
ChMatrix33 <double> result = new ChMatrix33 <double>(0);
result.nm.matrix.MatrMultiply(matbis.nm.matrix, matbis2.nm.matrix);
return(result);
}
/// Computes and returns an Adt matrix (-note: prefer using
/// Compute_Adtdt() directly for better performance)
public ChMatrix33 <Real> GetA_dtdt()
{
ChMatrix33 <Real> res = new ChMatrix33 <Real>(0);
Compute_Adtdt(ref res);
return(res);
}
/// Callback used to report contact points already added to the container.
/// If it returns false, the contact scanning will be stopped.
public abstract bool OnReportContact(
ChVector pA, //< contact pA
ChVector pB, //< contact pB
ChMatrix33 <double> plane_coord, //< contact plane coordsystem (A column 'X' is contact normal)
double distance, //< contact distance
double eff_radius, //< effective radius of curvature at contact
ChVector react_forces, //< react.forces (if already computed). In coordsystem 'plane_coord'
ChVector react_torques, //< react.torques, if rolling friction (if already computed).
ChContactable contactobjA, //< model A (note: some containers may not support it and could be nullptr)
ChContactable contactobjB //< model B (note: some containers may not support it and could be nullptr)
);
/// Computes the time derivative of rotation matrix, mAdt.
public void Compute_Adt(ref ChMatrix33 <Real> mA_dt)
{
// [A_dt]=2[dFp/dt][Fm]'=2[Fp(q_dt)][Fm(q)]'
//ChMatrixNM<IntInterface.Three, IntInterface.Four> Fpdt = new ChMatrixNM<IntInterface.Three, IntInterface.Four>(0); //ChMatrixNM<IntInterface.Four, IntInterface.Four>.NMNULL3_4;
// ChMatrixNM<IntInterface.Three, IntInterface.Four> Fm = new ChMatrixNM<IntInterface.Three, IntInterface.Four>(0);
ChFrame <Real> .SetMatrix_Fp(ref Fpdt, coord_dt.rot);
ChFrame <Real> .SetMatrix_Fm(ref Fm, this.coord.rot);
mA_dt.nm.matrix.MatrMultiplyT(Fpdt.matrix, Fm.matrix);
mA_dt.nm.matrix.MatrScale(2);
}
/// 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()));
}
/// This function transforms a point from the local reference
/// frame to the parent reference frame. The relative attitude of
/// the local reference frame with respect to the parent reference frame
/// is given by the 'origin' translation and the 'alignment' rotation matrix.
/// Since the function is static, you do not need a ChTransform object. For example,
/// use it as: mresult=ChTransform<>::TransformLocalToParent(mloc, morig, malign).
/// This function is optimized for fast execution.
/// \return The point in the parent reference frame, as parent = origin + [A]*(local)
public static ChVector TransformLocalToParent(
ChVector local, //< point to transform, given in local coordinates
ChVector origin, //< origin of frame respect to parent, in parent coords,
ChMatrix33 <Real> alignment //< rotation of frame respect to parent, in parent coords.
)
{
return(new ChVector(((alignment.nm.matrix.Get33Element(0, 0)) * local.x) + ((alignment.nm.matrix.Get33Element(0, 1)) * local.y) +
((alignment.nm.matrix.Get33Element(0, 2)) * local.z) + origin.x,
((alignment.nm.matrix.Get33Element(1, 0)) * local.x) + ((alignment.nm.matrix.Get33Element(1, 1)) * local.y) +
((alignment.nm.matrix.Get33Element(1, 2)) * local.z) + origin.y,
((alignment.nm.matrix.Get33Element(2, 0)) * local.x) + ((alignment.nm.matrix.Get33Element(2, 1)) * local.y) +
((alignment.nm.matrix.Get33Element(2, 2)) * local.z) + origin.z));
}
//
// STATIC FUNCTIONS
//
// TRANSFORMATIONS, USING POSITION AND ROTATION MATRIX [A]
/// This function transforms a point from the parent coordinate
/// system to a local coordinate system, whose relative position
/// is given by the 'origin' translation and 'alignment' rotation matrix.
/// Since the function is static, you do not need a ChTransform object, for example
/// use it as: mresult=ChTransform<>::TransformParentToLocal(mpar, morig, malign)
/// This function is optimized for fast execution.
/// \return The point in local coordinate, as local=[A]'*(parent-origin)
public static ChVector TransformParentToLocal(
ChVector parent, //< point to transform, given in parent coordinates;
ChVector origin, //< location of local frame with respect to parent, expressed in parent ref frame;
ChMatrix33 <Real> alignment //< rotation of local frame with respect to parent, expressed in parent coords.
)
{
double mx = parent.x - origin.x;
double my = parent.y - origin.y;
double mz = parent.z - origin.z;
return(new ChVector(((alignment.nm.matrix.Get33Element(0, 0)) * mx) + ((alignment.nm.matrix.Get33Element(1, 0)) * my) +
((alignment.nm.matrix.Get33Element(2, 0)) * mz),
((alignment.nm.matrix.Get33Element(0, 1)) * mx) + ((alignment.nm.matrix.Get33Element(1, 1)) * my) +
((alignment.nm.matrix.Get33Element(2, 1)) * mz),
((alignment.nm.matrix.Get33Element(0, 2)) * mx) + ((alignment.nm.matrix.Get33Element(1, 2)) * my) +
((alignment.nm.matrix.Get33Element(2, 2)) * mz)));
}
/// Computes the 2nd time derivative of rotation matrix, mAdtdt.
public void Compute_Adtdt(ref ChMatrix33 <Real> mA_dtdt)
{
// [A_dtdt]=2[Fp(q_dtdt)][Fm(q)]'+2[Fp(q_dt)][Fm(q_dt)]'
// ChMatrixNM<IntInterface.Three, IntInterface.Four> ma = new ChMatrixNM<IntInterface.Three, IntInterface.Four>(0);
// ChMatrixNM<IntInterface.Three, IntInterface.Four> mb = new ChMatrixNM<IntInterface.Three, IntInterface.Four>(0);
// ChMatrix33<double> mr = new ChMatrix33<double>(0);
ChFrame <Real> .SetMatrix_Fp(ref ma, coord_dtdt.rot);
ChFrame <Real> .SetMatrix_Fm(ref mb, this.coord.rot);
mr.nm.matrix.MatrMultiplyT(ma.matrix, mb.matrix);
ChFrame <Real> .SetMatrix_Fp(ref ma, coord_dt.rot);
ChFrame <Real> .SetMatrix_Fm(ref mb, coord_dt.rot);
mA_dtdt.nm.matrix.MatrMultiplyT(ma.matrix, mb.matrix);
mA_dtdt.nm.matrix.MatrInc(mr.nm.matrix);
mA_dtdt.nm.matrix.MatrScale(2);
}
/// Fast inversion of small matrices. Result will be in 'matra'.
/// \return Returns the determinant.
public double FastInvert(ChMatrix33 <Real> matra)
{
double det;
double sdet0, sdet1, sdet2;
sdet0 = +(this.nm.matrix.Get33Element(1, 1) * this.nm.matrix.Get33Element(2, 2)) -
(this.nm.matrix.Get33Element(2, 1) * this.nm.matrix.Get33Element(1, 2));
sdet1 = -(this.nm.matrix.Get33Element(1, 0) * this.nm.matrix.Get33Element(2, 2)) +
(this.nm.matrix.Get33Element(2, 0) * this.nm.matrix.Get33Element(1, 2));
sdet2 = +(this.nm.matrix.Get33Element(1, 0) * this.nm.matrix.Get33Element(2, 1)) -
(this.nm.matrix.Get33Element(2, 0) * this.nm.matrix.Get33Element(1, 1));
det = sdet0 * this.nm.matrix.Get33Element(0, 0) + sdet1 * this.nm.matrix.Get33Element(0, 1) + sdet2 * this.nm.matrix.Get33Element(0, 2);
matra.nm.matrix.Set33Element(0, 0, sdet0 / det);
matra.nm.matrix.Set33Element(1, 0, sdet1 / det);
matra.nm.matrix.Set33Element(2, 0, sdet2 / det);
matra.nm.matrix.Set33Element(0, 1, (-(this.nm.matrix.Get33Element(0, 1) * this.nm.matrix.Get33Element(2, 2)) +
(this.nm.matrix.Get33Element(2, 1) * this.nm.matrix.Get33Element(0, 2))) /
det);
matra.nm.matrix.Set33Element(1, 1, (+(this.nm.matrix.Get33Element(0, 0) * this.nm.matrix.Get33Element(2, 2)) -
(this.nm.matrix.Get33Element(2, 0) * this.nm.matrix.Get33Element(0, 2))) /
det);
matra.nm.matrix.Set33Element(2, 1, (-(this.nm.matrix.Get33Element(0, 0) * this.nm.matrix.Get33Element(2, 1)) +
(this.nm.matrix.Get33Element(2, 0) * this.nm.matrix.Get33Element(0, 1))) /
det);
matra.nm.matrix.Set33Element(0, 2, (+(this.nm.matrix.Get33Element(0, 1) * this.nm.matrix.Get33Element(1, 2)) -
(this.nm.matrix.Get33Element(1, 1) * this.nm.matrix.Get33Element(0, 2))) /
det);
matra.nm.matrix.Set33Element(1, 2, (-(this.nm.matrix.Get33Element(0, 0) * this.nm.matrix.Get33Element(1, 2)) +
(this.nm.matrix.Get33Element(1, 0) * this.nm.matrix.Get33Element(0, 2))) /
det);
matra.nm.matrix.Set33Element(2, 2, (+(this.nm.matrix.Get33Element(0, 0) * this.nm.matrix.Get33Element(1, 1)) -
(this.nm.matrix.Get33Element(1, 0) * this.nm.matrix.Get33Element(0, 1))) /
det);
return(det);
}
// -----------------------------------------------------------------------------
// 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;
}
}
public override void update(double mytime, bool update_assets)
{
// Inherit parent class:
base.update(mytime, update_assets);
// Override the rotational jacobian [Cq] and the rotational residual C,
// by assuming an additional hidden frame that rotates about frame2:
if (this.Body1 != null && this.Body2 != null)
{
ChFrame <double> aframe1 = ChFrame <double> .BitShiftRight(this.frame1, (this.Body1));
ChFrame <double> aframe2 = ChFrame <double> .BitShiftRight(this.frame2, (this.Body2));
ChFrame <double> aframe12 = new ChFrame <double>();
aframe2.TransformParentToLocal(aframe1, aframe12);
ChFrame <double> aframe2rotating = new ChFrame <double>();
double aux_rotation;
if (this.avoid_angle_drift)
{
aux_rotation = this.aux_dt + this.rot_offset;
}
else
{
// to have it aligned to current rot, to allow C=0.
aux_rotation = aframe12.GetRot().Q_to_Rotv().z;
}
aframe2rotating.SetRot(aframe2.GetRot() * ChQuaternion.Q_from_AngAxis2(aux_rotation, ChVector.VECT_Z));
// TODO this needs to be addressed, with it it causes rotation problems, seems to work fine without the TransformParentToLocal?
ChFrame <double> aframe12rotating = new ChFrame <double>();
// aframe2rotating.TransformParentToLocal(aframe1, aframe12rotating);
ChMatrix33 <double> Jw1 = new ChMatrix33 <double>();
ChMatrix33 <double> Jw2 = new ChMatrix33 <double>();
ChMatrix33 <double> mtempM = new ChMatrix33 <double>();
ChMatrix33 <double> mtempQ = new ChMatrix33 <double>();
ChMatrix33 <double> abs_plane_rotating = aframe2rotating.GetA();
Jw1.nm.matrix.MatrTMultiply(abs_plane_rotating.nm.matrix, Body1.GetA().nm.matrix);
Jw2.nm.matrix.MatrTMultiply(abs_plane_rotating.nm.matrix, Body2.GetA().nm.matrix);
Jw2.nm.matrix.MatrNeg();
// TODO this also needs to be addressed, with it it causes rotation problems/
// Premultiply by Jw1 and Jw2 by 0.5*[Fp(q_resid)]' to get residual as imaginary part of a quaternion.
/* mtempM.Set_X_matrix((aframe12rotating.GetRot().GetVector()) * 0.5);
* mtempM[0, 0] = 0.5 * aframe12rotating.GetRot().e0;
* mtempM[1, 1] = 0.5 * aframe12rotating.GetRot().e0;
* mtempM[2, 2] = 0.5 * aframe12rotating.GetRot().e0;
* mtempQ.MatrTMultiply(mtempM, Jw1);
* Jw1 = mtempQ;
* mtempQ.MatrTMultiply(mtempM, Jw2);
* Jw2 = mtempQ;*/
int nc = 0;
if (c_x)
{
nc++;
}
if (c_y)
{
nc++;
}
if (c_z)
{
nc++;
}
if (c_rx)
{
this.C.matrix.ElementN(nc) = aframe12rotating.GetRot().e1;
this.mask.Constr_N(nc).Get_Cq_a().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_b().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jw1.nm.matrix, 0, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jw2.nm.matrix, 0, 0, 1, 3, 0, 3);
nc++;
}
if (c_ry)
{
this.C.matrix.ElementN(nc) = aframe12rotating.GetRot().e2;
this.mask.Constr_N(nc).Get_Cq_a().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_b().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jw1.nm.matrix, 1, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jw2.nm.matrix, 1, 0, 1, 3, 0, 3);
nc++;
}
if (c_rz)
{
this.C.matrix.ElementN(nc) = aframe12rotating.GetRot().e3;
this.mask.Constr_N(nc).Get_Cq_a().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_b().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jw1.nm.matrix, 2, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jw2.nm.matrix, 2, 0, 1, 3, 0, 3);
nc++;
}
}
}
/// 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 update(double mytime, bool update_assets)
{
// Inherit parent class:
base.update(mytime, update_assets);
// Override the rotational jacobian [Cq] and the rotational residual C,
// by assuming an additional hidden frame that rotates about frame2:
if (this.Body1 != null && this.Body2 != null)
{
ChFrame <double> aframe1 = this.frame1.BitShiftRight(this.Body1);
ChFrame <double> aframe2 = this.frame2.BitShiftRight(this.Body2);
ChFrame <double> aframe12 = new ChFrame <double>();// ChFrame<double>.FNULL;
aframe2.TransformParentToLocal(aframe1, aframe12);
ChFrame <double> aframe2rotating = new ChFrame <double>();// ChFrame<double>.FNULL;
double aux_rotation;
aux_rotation = m_func.Get_y(mytime) + rot_offset;
aframe2rotating.SetRot(aframe2.GetRot() * ChQuaternion.Q_from_AngAxis2(aux_rotation, ChVector.VECT_Z));
ChFrame <double> aframe12rotating = new ChFrame <double>();
// aframe2rotating.TransformParentToLocal(aframe1, aframe12rotating);
ChMatrix33 <double> Jw1 = new ChMatrix33 <double>(0), Jw2 = new ChMatrix33 <double>(0);
ChMatrix33 <double> mtempM = new ChMatrix33 <double>(0), mtempQ = new ChMatrix33 <double>(0);
ChMatrix33 <double> abs_plane_rotating = aframe2rotating.GetA();
Jw1.nm.matrix.MatrTMultiply(abs_plane_rotating.nm.matrix, Body1.GetA().nm.matrix);
Jw2.nm.matrix.MatrTMultiply(abs_plane_rotating.nm.matrix, Body2.GetA().nm.matrix);
Jw2.nm.matrix.MatrNeg();
// Premultiply by Jw1 and Jw2 by 0.5*[Fp(q_resid)]' to get residual as imaginary part of a quaternion.
/* mtempM.Set_X_matrix((aframe12rotating.GetRot().GetVector()) * 0.5);
* mtempM.nm.matrix[0, 0] = 0.5 * aframe12rotating.GetRot().e0;
* mtempM.nm.matrix[1, 1] = 0.5 * aframe12rotating.GetRot().e0;
* mtempM.nm.matrix[2, 2] = 0.5 * aframe12rotating.GetRot().e0;
* mtempQ.nm.matrix.MatrTMultiply(mtempM.nm.matrix, Jw1.nm.matrix);
* Jw1 = mtempQ;
* mtempQ.nm.matrix.MatrTMultiply(mtempM.nm.matrix, Jw2.nm.matrix);
* Jw2 = mtempQ;*/
int nc = 0;
if (c_x)
{
nc++;
}
if (c_y)
{
nc++;
}
if (c_z)
{
nc++;
}
if (c_rx)
{
this.C.matrix.ElementN(nc) = aframe12rotating.GetRot().e1;
this.mask.Constr_N(nc).Get_Cq_a().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_b().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jw1.nm.matrix, 0, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jw2.nm.matrix, 0, 0, 1, 3, 0, 3);
nc++;
}
if (c_ry)
{
this.C.matrix.ElementN(nc) = aframe12rotating.GetRot().e2;
this.mask.Constr_N(nc).Get_Cq_a().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_b().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jw1.nm.matrix, 1, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jw2.nm.matrix, 1, 0, 1, 3, 0, 3);
nc++;
}
if (c_rz)
{
this.C.matrix.ElementN(nc) = aframe12rotating.GetRot().e3;
this.mask.Constr_N(nc).Get_Cq_a().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_b().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jw1.nm.matrix, 2, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jw2.nm.matrix, 2, 0, 1, 3, 0, 3);
nc++;
}
}
}
/// Construct from a coordsys
public ChFrame(ChCoordsys mc)
{
coord = new ChCoordsys(mc);
Amatrix = new ChMatrix33 <Real>(mc.rot);
}
/// Copy constructor, build from another frame
public ChFrame(ChFrame <Real> other)
{
coord = other.coord;
Amatrix = other.Amatrix;
}
/// Default constructor, or construct from pos and rot (as a quaternion)
public ChFrame(ChVector mv,
ChQuaternion mq)
{
coord = new ChCoordsys(mv, mq);
Amatrix = new ChMatrix33 <Real>(mq);
}
/// Construct from pos and rotation (as a 3x3 matrix)
public ChFrame(ChVector mv, ChMatrix33 <Real> ma)
{
coord = new ChCoordsys(mv, ma.Get_A_quaternion());
Amatrix = new ChMatrix33 <Real>(ma);
}
// public static ChMatrix33<double> MNULL = new ChMatrix33<double>(0);
//
// CONSTRUCTORS
//
/// Default constructor builds a 3x3 matrix with zeroes.
/* public ChMatrix33() {
* }*/
/// Copy constructor
public ChMatrix33(ChMatrix33 <Real> msource)
{
nm = new ChMatrixNM <IntInterface.Three, IntInterface.Three>(0);
nm.matrix = new ChMatrix();
}
/// Impose the rotation as a 3x3 matrix.
/// Note: the rotation matrix must be already orthogonal!
public virtual void SetRot(ChMatrix33 <Real> mA)
{
coord.rot = mA.Get_A_quaternion();
Amatrix.nm.matrix.CopyFromMatrix(mA.nm.matrix);
}
//
// UPDATING FUNCTIONS
//
/// Override _all_ time, jacobian etc. updating.
public override void update(double mytime, bool update_assets = true)
{
// Inherit time changes of parent class (ChLink), basically doing nothing :)
base.update(mytime, update_assets);
if (this.Body1 != null && this.Body2 != null)
{
this.mask.SetTwoBodiesVariables(Body1.Variables(), Body2.Variables());
ChFrame <double> aframe = ChFrame <double> .BitShiftRight(this.frame1, (this.Body1));
ChVector p1_abs = aframe.GetPos();
ChFrame <double> aframe2 = ChFrame <double> .BitShiftRight(this.frame2, (this.Body2));
ChVector p2_abs = aframe2.GetPos();
ChFrame <double> bframe = new ChFrame <double>();
(this.Body2).TransformParentToLocal(aframe, bframe);
this.frame2.TransformParentToLocal(bframe, aframe);
// Now 'aframe' contains the position/rotation of frame 1 respect to frame 2, in frame 2 coords.
//***TODO*** check if it is faster to do aframe2.TransformParentToLocal(aframe, bframe); instead of two transforms above
ChMatrix33 <double> Jx1 = new ChMatrix33 <double>(0);
ChMatrix33 <double> Jx2 = new ChMatrix33 <double>(0);
ChMatrix33 <double> Jr1 = new ChMatrix33 <double>(0);
ChMatrix33 <double> Jr2 = new ChMatrix33 <double>(0);
ChMatrix33 <double> Jw1 = new ChMatrix33 <double>(0);
ChMatrix33 <double> Jw2 = new ChMatrix33 <double>(0);
ChMatrix33 <double> mtempM = new ChMatrix33 <double>(0);
ChMatrix33 <double> mtempQ = new ChMatrix33 <double>(0);
ChMatrix33 <double> abs_plane = new ChMatrix33 <double>(0);
abs_plane.nm.matrix.MatrMultiply(Body2.GetA().nm.matrix, frame2.GetA().nm.matrix);
Jx1.nm.matrix.CopyFromMatrixT(abs_plane.nm.matrix);
Jx2.nm.matrix.CopyFromMatrixT(abs_plane.nm.matrix);
Jx2.nm.matrix.MatrNeg();
Jw1.nm.matrix.MatrTMultiply(abs_plane.nm.matrix, Body1.GetA().nm.matrix);
Jw2.nm.matrix.MatrTMultiply(abs_plane.nm.matrix, Body2.GetA().nm.matrix);
mtempM.Set_X_matrix(frame1.GetPos());
Jr1.nm.matrix.MatrMultiply(Jw1.nm.matrix, mtempM.nm.matrix);
Jr1.nm.matrix.MatrNeg();
mtempM.Set_X_matrix(frame2.GetPos());
Jr2.nm.matrix.MatrMultiply(Jw2.nm.matrix, mtempM.nm.matrix);
ChVector p2p1_base2 = (Body2.GetA()).MatrT_x_Vect(ChVector.Vsub(p1_abs, p2_abs));
mtempM.Set_X_matrix(p2p1_base2);
mtempQ.nm.matrix.MatrTMultiply(frame2.GetA().nm.matrix, mtempM.nm.matrix);
Jr2.nm.matrix.MatrInc(mtempQ.nm.matrix);
Jw2.nm.matrix.MatrNeg();
// Premultiply by Jw1 and Jw2 by 0.5*[Fp(q_resid)]' to get residual as imaginary part of a quaternion.
// For small misalignment this effect is almost insignificant cause [Fp(q_resid)]=[I],
// but otherwise it is needed (if you want to use the stabilization term - if not, you can live without).
mtempM.Set_X_matrix((aframe.GetRot().GetVector()) * 0.5);
mtempM.nm.matrix[0, 0] = 0.5 * aframe.GetRot().e0;
mtempM.nm.matrix[1, 1] = 0.5 * aframe.GetRot().e0;
mtempM.nm.matrix[2, 2] = 0.5 * aframe.GetRot().e0;
mtempQ.nm.matrix.MatrTMultiply(mtempM.nm.matrix, Jw1.nm.matrix);
Jw1 = mtempQ;
mtempQ.nm.matrix.MatrTMultiply(mtempM.nm.matrix, Jw2.nm.matrix);
Jw2 = mtempQ;
int nc = 0;
if (c_x)
{
this.C.matrix.ElementN(nc) = aframe.GetPos().x;
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jx1.nm.matrix, 0, 0, 1, 3, 0, 0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jr1.nm.matrix, 0, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jx2.nm.matrix, 0, 0, 1, 3, 0, 0);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jr2.nm.matrix, 0, 0, 1, 3, 0, 3);
nc++;
}
if (c_y)
{
this.C.matrix.ElementN(nc) = aframe.GetPos().y;
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jx1.nm.matrix, 1, 0, 1, 3, 0, 0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jr1.nm.matrix, 1, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jx2.nm.matrix, 1, 0, 1, 3, 0, 0);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jr2.nm.matrix, 1, 0, 1, 3, 0, 3);
nc++;
}
if (c_z)
{
this.C.matrix.ElementN(nc) = aframe.GetPos().z;
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jx1.nm.matrix, 2, 0, 1, 3, 0, 0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jr1.nm.matrix, 2, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jx2.nm.matrix, 2, 0, 1, 3, 0, 0);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jr2.nm.matrix, 2, 0, 1, 3, 0, 3);
nc++;
}
if (c_rx)
{
this.C.matrix.ElementN(nc) = aframe.GetRot().e1;
this.mask.Constr_N(nc).Get_Cq_a().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_b().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jw1.nm.matrix, 0, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jw2.nm.matrix, 0, 0, 1, 3, 0, 3);
nc++;
}
if (c_ry)
{
this.C.matrix.ElementN(nc) = aframe.GetRot().e2;
this.mask.Constr_N(nc).Get_Cq_a().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_b().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jw1.nm.matrix, 1, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jw2.nm.matrix, 1, 0, 1, 3, 0, 3);
nc++;
}
if (c_rz)
{
this.C.matrix.ElementN(nc) = aframe.GetRot().e3;
this.mask.Constr_N(nc).Get_Cq_a().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_b().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jw1.nm.matrix, 2, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jw2.nm.matrix, 2, 0, 1, 3, 0, 3);
nc++;
}
}
}
/// Set the inertia matrix
public void SetBodyInertia(ChMatrix33 <double> minertia)
{
inertia.nm.matrix.CopyFromMatrix(minertia.nm.matrix);
inertia.FastInvert(inv_inertia);
}
// 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;
}
//
// 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;
}
}
/// Construct from pos and rotation (as a 3x3 matrix)
public ChFrameMoving(ChVector mv, ChMatrix33 <Real> ma) : base(mv, ma)
{
coord_dt.rot = coord_dtdt.rot = new ChQuaternion(0, 0, 0, 0);
}
