/// 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()); }