/// Specialized initialization for springs, given the two bodies to be connected,
/// the positions of the two anchor endpoints of the spring (each expressed
/// in body or abs. coordinates) and the imposed rest length of the spring.
/// NOTE! As in ChLinkMarkers::Initialize(), the two markers are automatically
/// created and placed inside the two connected bodies.
public void Initialize(
ChBody mbody1, //< first body to link
ChBody mbody2, //< second body to link
bool pos_are_relative, //< true: following pos. are relative to bodies
ChVector mpos1, //< position of spring endpoint, for 1st body (rel. or abs., see flag above)
ChVector mpos2, //< position of spring endpoint, for 2nd body (rel. or abs., see flag above)
bool auto_rest_length = true, //< if true, initializes the rest-length as the distance between mpos1 and mpos2
double mrest_length = 0 //< imposed rest_length (no need to define, if auto_rest_length=true.)
)
{
// First, initialize as all constraint with markers.
// In this case, create the two markers also!.
base.Initialize(mbody1, mbody2, new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0)));
if (pos_are_relative)
{
marker1.Impose_Rel_Coord(new ChCoordsys(mpos1, new ChQuaternion(1, 0, 0, 0)));
marker2.Impose_Rel_Coord(new ChCoordsys(mpos2, new ChQuaternion(1, 0, 0, 0)));
}
else
{
marker1.Impose_Abs_Coord(new ChCoordsys(mpos1, new ChQuaternion(1, 0, 0, 0)));
marker2.Impose_Abs_Coord(new ChCoordsys(mpos2, new ChQuaternion(1, 0, 0, 0)));
}
ChVector AbsDist = marker1.GetAbsCoord().pos - marker2.GetAbsCoord().pos;
dist = AbsDist.Length();
spr_restlength = auto_rest_length ? dist : mrest_length;
}
/// Override _all_ time, jacobian etc. updating.
/// In detail, it computes jacobians, violations, etc. and stores
/// results in inner structures.
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);
// compute jacobians
ChVector AbsDist = Body1.TransformPointLocalToParent(pos1) - Body2.TransformPointLocalToParent(pos2);
curr_dist = AbsDist.Length();
ChVector D2abs = ChVector.Vnorm(AbsDist);
ChVector D2relB = Body2.TransformDirectionParentToLocal(D2abs);
ChVector D2relA = Body1.TransformDirectionParentToLocal(D2abs);
ChVector CqAx = D2abs;
ChVector CqBx = -D2abs;
ChVector CqAr = -ChVector.Vcross(D2relA, pos1);
ChVector CqBr = ChVector.Vcross(D2relB, pos2);
Cx.Get_Cq_a().ElementN(0) = CqAx.x;
Cx.Get_Cq_a().ElementN(1) = CqAx.y;
Cx.Get_Cq_a().ElementN(2) = CqAx.z;
Cx.Get_Cq_a().ElementN(3) = CqAr.x;
Cx.Get_Cq_a().ElementN(4) = CqAr.y;
Cx.Get_Cq_a().ElementN(5) = CqAr.z;
Cx.Get_Cq_b().ElementN(0) = CqBx.x;
Cx.Get_Cq_b().ElementN(1) = CqBx.y;
Cx.Get_Cq_b().ElementN(2) = CqBx.z;
Cx.Get_Cq_b().ElementN(3) = CqBr.x;
Cx.Get_Cq_b().ElementN(4) = CqBr.y;
Cx.Get_Cq_b().ElementN(5) = CqBr.z;
//***TO DO*** C_dt? C_dtdt? (may be never used..)
}
public void DirToDxDyDz(ref ChVector Vx,
ref ChVector Vy,
ref ChVector Vz,
ChVector Vsingular)
{
// set Vx.
if (this.IsNull())
{
Vx = new ChVector(1, 0, 0);
}
else
{
Vx = this.GetNormalized();
}
Vz.Cross(Vx, Vsingular);
double zlen = Vz.Length();
// if near singularity, change the singularity reference vector.
if (zlen < 0.0001)
{
ChVector mVsingular = new ChVector(0, 0, 0);
if (Mathfx.Abs(Vsingular.x) < 0.9)
{
mVsingular = new ChVector(1, 0, 0);
}
else if (Mathfx.Abs(Vsingular.y) < 0.9)
{
mVsingular = new ChVector(0, 1, 0);
}
else if (Mathfx.Abs(Vsingular.z) < 0.9)
{
mVsingular = new ChVector(0, 0, 1);
}
Vz.Cross(Vx, mVsingular);
zlen = Vz.Length(); // now should be nonzero length.
}
// normalize Vz.
Vz.Scale(1 / zlen);
// compute Vy.
Vy.Cross(Vz, Vx);
}
/// 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);
}
/// Initialize this constraint, given the two bodies to be connected, the
/// positions of the two anchor endpoints of the distance (each expressed
/// in body or abs. coordinates) and the imposed distance.
public virtual bool Initialize(
ChBodyFrame mbody1, //< first frame to link
ChBodyFrame mbody2, //< second frame to link
bool pos_are_relative, //< true: following pos. are relative to bodies
ChVector mpos1, //< pos. of distance endpoint, for 1st body (rel. or abs., see flag above)
ChVector mpos2, //< pos. of distance endpoint, for 2nd body (rel. or abs., see flag above)
bool auto_distance = true, //< if true, initializes the imposed distance as the distance between mpos1 and mpos2
double mdistance = 0 //< imposed distance (no need to define, if auto_distance=true.)
)
{
Body1 = mbody1;
Body2 = mbody2;
Cx.SetVariables(Body1.Variables(), Body2.Variables());
if (pos_are_relative)
{
pos1 = mpos1;
pos2 = mpos2;
}
else
{
pos1 = Body1.TransformPointParentToLocal(mpos1);
pos2 = Body2.TransformPointParentToLocal(mpos2);
}
ChVector AbsDist = Body1.TransformPointLocalToParent(pos1) - Body2.TransformPointLocalToParent(pos2);
curr_dist = AbsDist.Length();
if (auto_distance)
{
distance = curr_dist;
}
else
{
distance = mdistance;
}
return(true);
}
// -----------------------------------------------------------------------------
// 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;
}
/// Calculate contact force, expressed in absolute coordinates.
public ChVector CalculateForce(
double delta, //< overlap in normal direction
ChVector normal_dir, //< normal contact direction (expressed in global frame)
ChVector vel1, //< velocity of contact point on objA (expressed in global frame)
ChVector vel2, //< velocity of contact point on objB (expressed in global frame)
ChMaterialCompositeSMC mat //< composite material for contact pair
)
{
// Set contact force to zero if no penetration.
if (delta <= 0)
{
return(new ChVector(0, 0, 0));
}
// Extract parameters from containing system
ChSystemSMC sys = (ChSystemSMC)(this.container.GetSystem());
double dT = sys.GetStep();
bool use_mat_props = sys.UsingMaterialProperties();
ChSystemSMC.ContactForceModel contact_model = sys.GetContactForceModel();
ChSystemSMC.AdhesionForceModel adhesion_model = sys.GetAdhesionForceModel();
ChSystemSMC.TangentialDisplacementModel tdispl_model = sys.GetTangentialDisplacementModel();
// Relative velocity at contact
ChVector relvel = vel2 - vel1;
double relvel_n_mag = relvel.Dot(normal_dir);
ChVector relvel_n = relvel_n_mag * normal_dir;
ChVector relvel_t = relvel - relvel_n;
double relvel_t_mag = relvel_t.Length();
ChBody oA = (this.objA as ChBody);
ChBody oB = (this.objB as ChBody);
// Calculate effective mass
double eff_mass = oA.GetContactableMass() * oB.GetContactableMass() /
(oA.GetContactableMass() + oB.GetContactableMass());
// Calculate stiffness and viscous damping coefficients.
// All models use the following formulas for normal and tangential forces:
// Fn = kn * delta_n - gn * v_n
// Ft = kt * delta_t - gt * v_t
double kn = 0;
double kt = 0;
double gn = 0;
double gt = 0;
double eps = double.Epsilon;
switch (contact_model)
{
case ChSystemSMC.ContactForceModel.Hooke:
if (use_mat_props)
{
double tmp_k = (16.0 / 15) * Math.Sqrt(this.eff_radius) * mat.E_eff;
double v2 = sys.GetCharacteristicImpactVelocity() * sys.GetCharacteristicImpactVelocity();
double loge = (mat.cr_eff < eps) ? Math.Log(eps) : Math.Log(mat.cr_eff);
loge = (mat.cr_eff > 1 - eps) ? Math.Log(1 - eps) : loge;
double tmp_g = 1 + Math.Pow(ChMaths.CH_C_PI / loge, 2);
kn = tmp_k * Math.Pow(eff_mass * v2 / tmp_k, 1.0 / 5);
kt = kn;
gn = Math.Sqrt(4 * eff_mass * kn / tmp_g);
gt = gn;
}
else
{
kn = mat.kn;
kt = mat.kt;
gn = eff_mass * mat.gn;
gt = eff_mass * mat.gt;
}
break;
case ChSystemSMC.ContactForceModel.Hertz:
if (use_mat_props)
{
double sqrt_Rd = Math.Sqrt(this.eff_radius * delta);
double Sn = 2 * mat.E_eff * sqrt_Rd;
double St = 8 * mat.G_eff * sqrt_Rd;
double loge = (mat.cr_eff < eps) ? Math.Log(eps) : Math.Log(mat.cr_eff);
double beta = loge / Math.Sqrt(loge * loge + ChMaths.CH_C_PI * ChMaths.CH_C_PI);
kn = (2.0 / 3) * Sn;
kt = St;
gn = -2 * Math.Sqrt(5.0 / 6) * beta * Math.Sqrt(Sn * eff_mass);
gt = -2 * Math.Sqrt(5.0 / 6) * beta * Math.Sqrt(St * eff_mass);
}
else
{
double tmp = this.eff_radius * Math.Sqrt(delta);
kn = tmp * mat.kn;
kt = tmp * mat.kt;
gn = tmp * eff_mass * mat.gn;
gt = tmp * eff_mass * mat.gt;
}
break;
case ChSystemSMC.ContactForceModel.PlainCoulomb:
if (use_mat_props)
{
double sqrt_Rd = Math.Sqrt(delta);
double Sn = 2 * mat.E_eff * sqrt_Rd;
double St = 8 * mat.G_eff * sqrt_Rd;
double loge = (mat.cr_eff < eps) ? Math.Log(eps) : Math.Log(mat.cr_eff);
double beta = loge / Math.Sqrt(loge * loge + ChMaths.CH_C_PI * ChMaths.CH_C_PI);
kn = (2.0 / 3) * Sn;
gn = -2 * Math.Sqrt(5.0 / 6) * beta * Math.Sqrt(Sn * eff_mass);
}
else
{
double tmp = Math.Sqrt(delta);
kn = tmp * mat.kn;
gn = tmp * mat.gn;
}
kt = 0;
gt = 0;
{
double forceNb = kn * delta - gn * relvel_n_mag;
if (forceNb < 0)
{
forceNb = 0;
}
double forceTb = mat.mu_eff * Math.Tanh(5.0 * relvel_t_mag) * forceNb;
switch (adhesion_model)
{
case ChSystemSMC.AdhesionForceModel.Constant:
forceNb -= mat.adhesion_eff;
break;
case ChSystemSMC.AdhesionForceModel.DMT:
forceNb -= mat.adhesionMultDMT_eff * Math.Sqrt(this.eff_radius);
break;
}
ChVector forceb = forceNb * normal_dir;
if (relvel_t_mag >= sys.GetSlipVelocityThreshold())
{
forceb -= (forceTb / relvel_t_mag) * relvel_t;
}
return(forceb);
}
}
// Tangential displacement (magnitude)
double delta_t = 0;
switch (tdispl_model)
{
case ChSystemSMC.TangentialDisplacementModel.OneStep:
delta_t = relvel_t_mag * dT;
break;
case ChSystemSMC.TangentialDisplacementModel.MultiStep:
//// TODO: implement proper MultiStep mode
delta_t = relvel_t_mag * dT;
break;
default:
break;
}
// Calculate the magnitudes of the normal and tangential contact forces
double forceN = kn * delta - gn * relvel_n_mag;
double forceT = kt * delta_t + gt * relvel_t_mag;
// If the resulting normal contact force is negative, the two shapes are moving
// away from each other so fast that no contact force is generated.
if (forceN < 0)
{
forceN = 0;
forceT = 0;
}
// Include adhesion force
switch (adhesion_model)
{
case ChSystemSMC.AdhesionForceModel.Constant:
forceN -= mat.adhesion_eff;
break;
case ChSystemSMC.AdhesionForceModel.DMT:
forceN -= mat.adhesionMultDMT_eff * Math.Sqrt(this.eff_radius);
break;
}
// Coulomb law
forceT = Math.Min(forceT, mat.mu_eff * Math.Abs(forceN));
// Accumulate normal and tangential forces
ChVector force = forceN * normal_dir;
if (relvel_t_mag >= sys.GetSlipVelocityThreshold())
{
force -= (forceT / relvel_t_mag) * relvel_t;
}
return(force);
}
/// Gets the instant force vector -or torque vector- modulus.
public double GetForceMod()
{
return(force.Length());
}
//
// 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;
}
}
public static double Vlength(ChVector va)
{
return(va.Length());
}
