/// Compute all forces in a contiguous array. /// Used in finite-difference Jacobian approximation. public void CalculateQ(ChState stateA_x, //< state positions for objA ChStateDelta stateA_w, //< state velocities for objA ChState stateB_x, //< state positions for objB ChStateDelta stateB_w, //< state velocities for objB ChMaterialCompositeSMC mat, //< composite material for contact pair ref ChVectorDynamic <double> Q //< output generalized forces ) { ChBody oA = (this.objA as ChBody); ChBody oB = (this.objB as ChBody); // Express contact points in local frames. // We assume that these points remain fixed to their respective contactable objects. ChVector p1_loc = oA.GetCsysForCollisionModel().TransformPointParentToLocal(this.p1); ChVector p2_loc = oB.GetCsysForCollisionModel().TransformPointParentToLocal(this.p2); // Express the local points in global frame ChVector p1_abs = oA.GetContactPoint(p1_loc, stateA_x); ChVector p2_abs = oB.GetContactPoint(p2_loc, stateB_x); /* * Note: while this can be somewhat justified for a ChBody, it will not work * for a mesh vertex for instance... * * // Project the points onto the unperturbed normal line * p1_abs = this.p1 + Vdot(p1_abs - this.p1, this.normal) * this.normal; * p2_abs = this.p2 + Vdot(p2_abs - this.p2, this.normal) * this.normal; */ // Calculate normal direction (expressed in global frame) ChVector normal_dir = (p1_abs - p2_abs).GetNormalized(); // Calculate penetration depth double delta = (p1_abs - p2_abs).Length(); // If the normal direction flipped sign, change sign of delta if (ChVector.Vdot(normal_dir, this.normal) < 0) { delta = -delta; } // Calculate velocity of contact points (expressed in global frame) ChVector vel1 = oA.GetContactPointSpeed(p1_loc, stateA_x, stateA_w); ChVector vel2 = oB.GetContactPointSpeed(p2_loc, stateB_x, stateB_w); // Compute the contact force. ChVector force = CalculateForce(delta, normal_dir, vel1, vel2, mat); // Compute and load the generalized contact forces. oA.ContactForceLoadQ(-force, p1_abs, stateA_x, ref Q, 0); oB.ContactForceLoadQ(force, p2_abs, stateB_x, ref Q, oA.ContactableGet_ndof_w()); }
/// Initialize this joint by specifying the two bodies to be connected, a point /// and a direction on body1 defining the revolute joint, and a point on the /// second body defining the spherical joint. If local = true, it is assumed /// that these quantities are specified in the local body frames. Otherwise, /// it is assumed that they are specified in the absolute frame. The imposed /// distance between the two points can be either inferred from the provided /// configuration (auto_distance = true) or specified explicitly. public void Initialize(ChBodyFrame body1, //< first frame (revolute side) ChBodyFrame body2, //< second frame (spherical side) bool local, //< true if data given in body local frames ChVector pos1, //< point on first frame (center of revolute) ChVector dir1, //< direction of revolute on first frame ChVector pos2, //< point on second frame (center of spherical) bool auto_distance = true, //< true if imposed distance equal to |pos1 - po2| double distance = 0 //< imposed distance (used only if auto_distance = false) ) { Body1 = body1; Body2 = body2; m_cnstr_dist.SetVariables(Body1.Variables(), Body2.Variables()); m_cnstr_dot.SetVariables(Body1.Variables(), Body2.Variables()); ChVector pos1_abs; ChVector pos2_abs; ChVector dir1_abs; if (local) { m_pos1 = pos1; m_pos2 = pos2; m_dir1 = ChVector.Vnorm(dir1); pos1_abs = Body1.TransformPointLocalToParent(m_pos1); pos2_abs = Body2.TransformPointLocalToParent(m_pos2); dir1_abs = Body1.TransformDirectionLocalToParent(m_dir1); } else { pos1_abs = pos1; pos2_abs = pos2; dir1_abs = ChVector.Vnorm(dir1); m_pos1 = Body1.TransformPointParentToLocal(pos1_abs); m_pos2 = Body2.TransformPointParentToLocal(pos2_abs); m_dir1 = Body1.TransformDirectionParentToLocal(dir1_abs); } ChVector d12_abs = pos2_abs - pos1_abs; m_cur_dist = d12_abs.Length(); m_dist = auto_distance ? m_cur_dist : distance; m_cur_dot = ChVector.Vdot(d12_abs, dir1_abs); }
/// Include the rotational spring custom torque. public override void UpdateForces(double time) { // Allow the base class to update itself (possibly adding its own forces). base.UpdateForces(time); // Invoke the provided functor to evaluate torque. // NOTE: we implicitly assume that the kinematics are CONSISTENT with this // type of link! double angle = relAngle; double angle_dt = ChVector.Vdot(relWvel, relAxis); if (m_torque_fun != null) { m_torque = (m_torque_fun)[time, relAngle, angle_dt, this]; } // Add to existing torque. C_torque += ChVector.Vmul(relAxis, m_torque); }
/// Initialize this joint by specifying the two bodies to be connected and the /// joint frames on each body. If local = true, it is assumed that these quantities /// are specified in the local body frames. Otherwise, it is assumed that they are /// specified in the absolute frame. public void Initialize(ChBodyFrame body1, //< first body frame ChBodyFrame body2, //< second body frame bool local, //< true if data given in body local frames ChFrame <double> frame1, //< joint frame on body 1 ChFrame <double> frame2 //< joint frame on body 2 ) { Body1 = body1; Body2 = body2; m_cnstr_x.SetVariables(Body1.Variables(), Body2.Variables()); m_cnstr_y.SetVariables(Body1.Variables(), Body2.Variables()); m_cnstr_z.SetVariables(Body1.Variables(), Body2.Variables()); m_cnstr_dot.SetVariables(Body1.Variables(), Body2.Variables()); ChFrame <double> frame1_abs; ChFrame <double> frame2_abs; if (local) { m_frame1 = frame1; m_frame2 = frame2; frame1_abs = ChFrame <double> .BitShiftRight(frame1, Body1); frame2_abs = ChFrame <double> .BitShiftRight(frame2, Body2); } else { ((ChFrame <double>)Body1).TransformParentToLocal(frame1, m_frame1); ((ChFrame <double>)Body2).TransformParentToLocal(frame2, m_frame2); frame1_abs = frame1; frame2_abs = frame2; } m_u1_tilde.Set_X_matrix(m_frame1.GetA().Get_A_Xaxis()); m_v2_tilde.Set_X_matrix(m_frame2.GetA().Get_A_Yaxis()); m_C.matrix.SetElement(0, 0, frame2_abs.coord.pos.x - frame1_abs.coord.pos.x); m_C.matrix.SetElement(1, 0, frame2_abs.coord.pos.y - frame1_abs.coord.pos.y); m_C.matrix.SetElement(2, 0, frame2_abs.coord.pos.z - frame1_abs.coord.pos.z); m_C.matrix.SetElement(3, 0, ChVector.Vdot(frame1_abs.GetA().Get_A_Xaxis(), frame2_abs.GetA().Get_A_Yaxis())); }
/// Updates motion laws, marker positions, etc. public override void UpdateTime(double mytime) { // First, inherit to parent class base.UpdateTime(mytime); ChFrame <double> abs_shaft1 = ChFrame <double> .FNULL; //new ChFrame<double>(); ChFrame <double> abs_shaft2 = ChFrame <double> .FNULL; //new ChFrame<double>(); ((ChFrame <double>)Body1).TransformLocalToParent(local_shaft1, abs_shaft1); ((ChFrame <double>)Body2).TransformLocalToParent(local_shaft2, abs_shaft2); ChVector dcc_w = ChVector.Vsub(Get_shaft_pos2(), Get_shaft_pos1()); // compute actual rotation of the two wheels (relative to truss). ChVector md1 = abs_shaft1.GetA().MatrT_x_Vect(dcc_w); md1.z = 0; md1 = ChVector.Vnorm(md1); ChVector md2 = abs_shaft2.GetA().MatrT_x_Vect(dcc_w); md2.z = 0; md2 = ChVector.Vnorm(md2); double periodic_a1 = ChMaths.ChAtan2(md1.x, md1.y); double periodic_a2 = ChMaths.ChAtan2(md2.x, md2.y); double old_a1 = a1; double old_a2 = a2; double turns_a1 = Math.Floor(old_a1 / ChMaths.CH_C_2PI); double turns_a2 = Math.Floor(old_a2 / ChMaths.CH_C_2PI); double a1U = turns_a1 * ChMaths.CH_C_2PI + periodic_a1 + ChMaths.CH_C_2PI; double a1M = turns_a1 * ChMaths.CH_C_2PI + periodic_a1; double a1L = turns_a1 * ChMaths.CH_C_2PI + periodic_a1 - ChMaths.CH_C_2PI; a1 = a1M; if (Math.Abs(a1U - old_a1) < Math.Abs(a1M - old_a1)) { a1 = a1U; } if (Math.Abs(a1L - a1) < Math.Abs(a1M - a1)) { a1 = a1L; } double a2U = turns_a2 * ChMaths.CH_C_2PI + periodic_a2 + ChMaths.CH_C_2PI; double a2M = turns_a2 * ChMaths.CH_C_2PI + periodic_a2; double a2L = turns_a2 * ChMaths.CH_C_2PI + periodic_a2 - ChMaths.CH_C_2PI; a2 = a2M; if (Math.Abs(a2U - old_a2) < Math.Abs(a2M - old_a2)) { a2 = a2U; } if (Math.Abs(a2L - a2) < Math.Abs(a2M - a2)) { a2 = a2L; } // correct marker positions if phasing is not correct double m_delta = 0; if (checkphase) { double realtau = tau; m_delta = a1 - phase - (a2 / realtau); if (m_delta > ChMaths.CH_C_PI) { m_delta -= (ChMaths.CH_C_2PI); // range -180..+180 is better than 0...360 } if (m_delta > (ChMaths.CH_C_PI / 4.0)) { m_delta = (ChMaths.CH_C_PI / 4.0); // phase correction only in +/- 45° } if (m_delta < -(ChMaths.CH_C_PI / 4.0)) { m_delta = -(ChMaths.CH_C_PI / 4.0); } //***TODO*** } // Move markers 1 and 2 to align them as pulley ends ChVector d21_w = dcc_w - Get_shaft_dir1() * ChVector.Vdot(Get_shaft_dir1(), dcc_w); ChVector D21_w = ChVector.Vnorm(d21_w); shaft_dist = d21_w.Length(); ChVector U1_w = ChVector.Vcross(Get_shaft_dir1(), D21_w); double gamma1 = Math.Acos((r1 - r2) / shaft_dist); ChVector Ru_w = D21_w * Math.Cos(gamma1) + U1_w * Math.Sin(gamma1); ChVector Rl_w = D21_w * Math.Cos(gamma1) - U1_w * Math.Sin(gamma1); belt_up1 = Get_shaft_pos1() + Ru_w * r1; belt_low1 = Get_shaft_pos1() + Rl_w * r1; belt_up2 = Get_shaft_pos1() + d21_w + Ru_w * r2; belt_low2 = Get_shaft_pos1() + d21_w + Rl_w * r2; // marker alignment ChMatrix33 <double> maU = new ChMatrix33 <double>(0); ChMatrix33 <double> maL = new ChMatrix33 <double>(0); ChVector Dxu = ChVector.Vnorm(belt_up2 - belt_up1); ChVector Dyu = Ru_w; ChVector Dzu = ChVector.Vnorm(ChVector.Vcross(Dxu, Dyu)); Dyu = ChVector.Vnorm(ChVector.Vcross(Dzu, Dxu)); maU.Set_A_axis(Dxu, Dyu, Dzu); // ! Require that the BDF routine of marker won't handle speed and acc.calculus of the moved marker 2! marker2.SetMotionType(ChMarker.eChMarkerMotion.M_MOTION_EXTERNAL); marker1.SetMotionType(ChMarker.eChMarkerMotion.M_MOTION_EXTERNAL); ChCoordsys newmarkpos = new ChCoordsys(); // move marker1 in proper positions newmarkpos.pos = this.belt_up1; newmarkpos.rot = maU.Get_A_quaternion(); marker1.Impose_Abs_Coord(newmarkpos); // move marker1 into teeth position // move marker2 in proper positions newmarkpos.pos = this.belt_up2; newmarkpos.rot = maU.Get_A_quaternion(); marker2.Impose_Abs_Coord(newmarkpos); // move marker2 into teeth position double phase_correction_up = m_delta * r1; double phase_correction_low = -phase_correction_up; double hU = ChVector.Vlength(belt_up2 - belt_up1) + phase_correction_up; double hL = ChVector.Vlength(belt_low2 - belt_low1) + phase_correction_low; // imposed relative positions/speeds deltaC.pos = new ChVector(-hU, 0, 0); deltaC_dt.pos = ChVector.VNULL; deltaC_dtdt.pos = ChVector.VNULL; deltaC.rot = ChQuaternion.QUNIT; // no relative rotations imposed! deltaC_dt.rot = ChQuaternion.QNULL; deltaC_dtdt.rot = ChQuaternion.QNULL; }
/// Updates motion laws, etc. for the impose rotation / impose speed modes public override void UpdateTime(double mytime) { // First, inherit to parent class base.UpdateTime(mytime); if (!IsActive()) { return; } // DEFAULTS compute rotation vars... // by default for torque control.. motion_axis = ChVector.VECT_Z; // motion axis is always the marker2 Z axis (in m2 relative coords) mot_rot = relAngle; mot_rot_dt = ChVector.Vdot(relWvel, motion_axis); mot_rot_dtdt = ChVector.Vdot(relWacc, motion_axis); mot_rerot = mot_rot / mot_tau; mot_rerot_dt = mot_rot_dt / mot_tau; mot_rerot_dtdt = mot_rot_dtdt / mot_tau; // nothing more to do here for torque control if (eng_mode == eCh_eng_mode.ENG_MODE_TORQUE) { return; } // If LEARN MODE, just record motion if (learn) { deltaC.pos = ChVector.VNULL; deltaC_dt.pos = ChVector.VNULL; deltaC_dtdt.pos = ChVector.VNULL; if (!(limit_Rx.Get_active() || limit_Ry.Get_active() || limit_Rz.Get_active())) { deltaC.rot = ChQuaternion.QUNIT; deltaC_dt.rot = ChQuaternion.QNULL; deltaC_dtdt.rot = ChQuaternion.QNULL; } if (eng_mode == eCh_eng_mode.ENG_MODE_ROTATION) { if (rot_funct.Get_Type() != ChFunction.FunctionType.FUNCT_RECORDER) { rot_funct = new ChFunction_Recorder(); } // record point double rec_rot = relAngle; // ***TO DO*** compute also rotations with cardano mode? if (impose_reducer) { rec_rot = rec_rot / mot_tau; } ChFunction_Recorder rec = (ChFunction_Recorder)rot_funct; rec.AddPoint(mytime, rec_rot, 1); // x=t } if (eng_mode == eCh_eng_mode.ENG_MODE_SPEED) { if (spe_funct.Get_Type() != ChFunction.FunctionType.FUNCT_RECORDER) { spe_funct = new ChFunction_Recorder(); } // record point double rec_spe = ChVector.Vlength(relWvel); // ***TO DO*** compute also with cardano mode? if (impose_reducer) { rec_spe = rec_spe / mot_tau; } ChFunction_Recorder rec = (ChFunction_Recorder)spe_funct; rec.AddPoint(mytime, rec_spe, 1); // x=t } } if (learn) { return; // no need to go on further...--.>>> } // Impose relative positions/speeds deltaC.pos = ChVector.VNULL; deltaC_dt.pos = ChVector.VNULL; deltaC_dtdt.pos = ChVector.VNULL; if (eng_mode == eCh_eng_mode.ENG_MODE_ROTATION) { if (impose_reducer) { mot_rerot = rot_funct.Get_y(ChTime); mot_rerot_dt = rot_funct.Get_y_dx(ChTime); mot_rerot_dtdt = rot_funct.Get_y_dxdx(ChTime); mot_rot = mot_rerot * mot_tau; mot_rot_dt = mot_rerot_dt * mot_tau; mot_rot_dtdt = mot_rerot_dtdt * mot_tau; } else { mot_rot = rot_funct.Get_y(ChTime); mot_rot_dt = rot_funct.Get_y_dx(ChTime); mot_rot_dtdt = rot_funct.Get_y_dxdx(ChTime); mot_rerot = mot_rot / mot_tau; mot_rerot_dt = mot_rot_dt / mot_tau; mot_rerot_dtdt = mot_rot_dtdt / mot_tau; } deltaC.rot = ChQuaternion.Q_from_AngAxis2(mot_rot, motion_axis); deltaC_dt.rot = ChQuaternion.Qdt_from_AngAxis(deltaC.rot, mot_rot_dt, motion_axis); deltaC_dtdt.rot = ChQuaternion.Qdtdt_from_AngAxis(mot_rot_dtdt, motion_axis, deltaC.rot, deltaC_dt.rot); } if (eng_mode == eCh_eng_mode.ENG_MODE_SPEED) { if (impose_reducer) { mot_rerot_dt = spe_funct.Get_y(ChTime); mot_rerot_dtdt = spe_funct.Get_y_dx(ChTime); mot_rot_dt = mot_rerot_dt * mot_tau; mot_rot_dtdt = mot_rerot_dtdt * mot_tau; } else { mot_rot_dt = spe_funct.Get_y(ChTime); mot_rot_dtdt = spe_funct.Get_y_dx(ChTime); mot_rerot_dt = mot_rot_dt / mot_tau; mot_rerot_dtdt = mot_rot_dtdt / mot_tau; } deltaC.rot = ChQuaternion.Qnorm(GetRelM().rot); // just keep current position, -assume always good after integration-. ChMatrix33 <double> relA = new ChMatrix33 <double>(0); relA.Set_A_quaternion(GetRelM().rot); // ..but adjust to keep Z axis aligned to shaft, anyway! ChVector displaced_z_axis = relA.Get_A_Zaxis(); ChVector adjustment = ChVector.Vcross(displaced_z_axis, ChVector.VECT_Z); deltaC.rot = ChQuaternion.Q_from_AngAxis2(ChVector.Vlength(adjustment), ChVector.Vnorm(adjustment)) % deltaC.rot; deltaC_dt.rot = ChQuaternion.Qdt_from_AngAxis(deltaC.rot, mot_rot_dt, motion_axis); deltaC_dtdt.rot = ChQuaternion.Qdtdt_from_AngAxis(mot_rot_dtdt, motion_axis, deltaC.rot, deltaC_dt.rot); } }
public override void UpdateForces(double mytime) { // First, inherit to parent class base.UpdateForces(mytime); if (this.IsDisabled()) { return; } // then, if not sticking, if (this.brake_torque != 0) { if (brake_mode == eChBrmode.BRAKE_ROTATION) { if (((ChLinkMaskLF)mask).Constr_E3().IsActive() == false) { int mdir; ChVector mv_torque = ChVector.Vmul(ChVector.VECT_Z, this.brake_torque); mdir = 0; // clockwise torque if (ChVector.Vdot(this.relWvel, mv_torque) > 0.0) { mv_torque = ChVector.Vmul(mv_torque, -1.0); // keep torque always opposed to ang speed. mdir = 1; // counterclockwise torque } if (mdir != this.last_dir) { this.must_stick = true; } this.last_dir = mdir; // +++ADD TO LINK TORQUE VECTOR C_torque = ChVector.Vadd(C_torque, mv_torque); } } if (brake_mode == eChBrmode.BRAKE_TRANSLATEX) { if (((ChLinkMaskLF)mask).Constr_X().IsActive() == false) { int mdir; ChVector mv_force = ChVector.Vmul(ChVector.VECT_X, this.brake_torque); mdir = 0; // F-. rear motion: frontfacing break force if (this.relM_dt.pos.x > 0.0) { mv_force = ChVector.Vmul(mv_force, -1.0); // break force always opposed to speed mdir = 1; // F<-- backfacing breakforce for front motion } if (mdir != this.last_dir) { this.must_stick = true; } this.last_dir = mdir; // +++ADD TO LINK TORQUE VECTOR C_force = ChVector.Vadd(C_force, mv_force); } } } // turn off sticking feature if stick ration not > 1.0 if (this.stick_ratio <= 1.0) { must_stick = false; } }
/// Get the master coordinate system for the assets (this will return the /// absolute coordinate system of the 'master' marker2) // public override ChFrame<double> GetAssetsFrame(int nclone = 0) { return marker2.GetAbsFrame(); } // // UPDATING FUNCTIONS // /// Updates auxiliary vars relM, relM_dt, relM_dtdt, /// dist, dist_dt et simila. public virtual void UpdateRelMarkerCoords() { // FOR ALL THE 6(or3) COORDINATES OF RELATIVE MOTION OF THE TWO MARKERS: // COMPUTE THE relM, relM_dt relM_dtdt COORDINATES, AND AUXILIARY DATA (distance,etc.) ChVector PQw = ChVector.Vsub(marker1.GetAbsCoord().pos, marker2.GetAbsCoord().pos); ChVector PQw_dt = ChVector.Vsub(marker1.GetAbsCoord_dt().pos, marker2.GetAbsCoord_dt().pos); ChVector PQw_dtdt = ChVector.Vsub(marker1.GetAbsCoord_dtdt().pos, marker2.GetAbsCoord_dtdt().pos); dist = ChVector.Vlength(PQw); // distance between origins, modulus dist_dt = ChVector.Vdot(ChVector.Vnorm(PQw), PQw_dt); // speed between origins, modulus. ChVector vtemp1; // for intermediate calculus ChVector vtemp2; ChQuaternion qtemp1; // ChMatrixNM<IntInterface.Three, IntInterface.Four> relGw = new ChMatrixNM<IntInterface.Three, IntInterface.Four>(0); /* ChQuaternion q_AD; * ChQuaternion q_BC; * ChQuaternion q_8; * ChVector q_4;*/ ChQuaternion temp1 = marker1.FrameMoving.GetCoord_dt().rot; ChQuaternion temp2 = marker2.FrameMoving.GetCoord_dt().rot; if (ChQuaternion.Qnotnull(temp1) || ChQuaternion.Qnotnull(temp2)) { q_AD = // q'qqq + qqqq' ChQuaternion.Qadd(ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord_dt().rot), ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.GetBody().BodyFrame.GetCoord().rot), ChQuaternion.Qcross((marker1.GetBody().BodyFrame.GetCoord().rot), (marker1.FrameMoving.GetCoord().rot)))), ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord().rot), ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.GetBody().BodyFrame.GetCoord().rot), ChQuaternion.Qcross((marker1.GetBody().BodyFrame.GetCoord().rot), (marker1.FrameMoving.GetCoord_dt().rot))))); } else { //q_AD = ChQuaternion.QNULL; q_BC = // qq'qq + qqq'q ChQuaternion.Qadd(ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord().rot), ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.GetBody().BodyFrame.GetCoord_dt().rot), ChQuaternion.Qcross((marker1.GetBody().BodyFrame.GetCoord().rot), (marker1.FrameMoving.GetCoord().rot)))), ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord().rot), ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.GetBody().BodyFrame.GetCoord().rot), ChQuaternion.Qcross((marker1.GetBody().BodyFrame.GetCoord_dt().rot), (marker1.FrameMoving.GetCoord().rot))))); } // q_8 = q''qqq + 2q'q'qq + 2q'qq'q + 2q'qqq' // + 2qq'q'q + 2qq'qq' + 2qqq'q' + qqqq'' temp2 = marker2.FrameMoving.GetCoord_dtdt().rot; if (ChQuaternion.Qnotnull(temp2)) { q_8 = ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord_dtdt().rot), ChQuaternion.Qcross(ChQuaternion.Qconjugate(Body2.GetCoord().rot), ChQuaternion.Qcross(Body1.GetCoord().rot, marker1.FrameMoving.GetCoord().rot))); // q_dtdt'm2 * q'o2 * q,o1 * q,m1 } else { //q_8 = ChQuaternion.QNULL; temp1 = marker1.FrameMoving.GetCoord_dtdt().rot; } if (ChQuaternion.Qnotnull(temp1)) { qtemp1 = ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord().rot), ChQuaternion.Qcross(ChQuaternion.Qconjugate(Body2.GetCoord().rot), ChQuaternion.Qcross(Body1.GetCoord().rot, marker1.FrameMoving.GetCoord_dtdt().rot))); // q'm2 * q'o2 * q,o1 * q_dtdt,m1 q_8 = ChQuaternion.Qadd(q_8, qtemp1); } temp2 = marker2.FrameMoving.GetCoord_dt().rot; if (ChQuaternion.Qnotnull(temp2)) { qtemp1 = ChQuaternion.Qcross( ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord_dt().rot), ChQuaternion.Qcross(ChQuaternion.Qconjugate(Body2.GetCoord_dt().rot), ChQuaternion.Qcross(Body1.GetCoord().rot, marker1.FrameMoving.GetCoord().rot))); qtemp1 = ChQuaternion.Qscale(qtemp1, 2); // 2( q_dt'm2 * q_dt'o2 * q,o1 * q,m1) q_8 = ChQuaternion.Qadd(q_8, qtemp1); } temp2 = marker2.FrameMoving.GetCoord_dt().rot; if (ChQuaternion.Qnotnull(temp2)) { qtemp1 = ChQuaternion.Qcross( ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord_dt().rot), ChQuaternion.Qcross(ChQuaternion.Qconjugate(Body2.GetCoord().rot), ChQuaternion.Qcross(Body1.GetCoord_dt().rot, marker1.FrameMoving.GetCoord().rot))); qtemp1 = ChQuaternion.Qscale(qtemp1, 2); // 2( q_dt'm2 * q'o2 * q_dt,o1 * q,m1) q_8 = ChQuaternion.Qadd(q_8, qtemp1); } temp1 = marker1.FrameMoving.GetCoord_dt().rot; temp2 = marker2.FrameMoving.GetCoord_dt().rot; if (ChQuaternion.Qnotnull(temp2) && ChQuaternion.Qnotnull(temp1)) { qtemp1 = ChQuaternion.Qcross( ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord_dt().rot), ChQuaternion.Qcross(ChQuaternion.Qconjugate(Body2.GetCoord().rot), ChQuaternion.Qcross(Body1.GetCoord().rot, marker1.FrameMoving.GetCoord_dt().rot))); qtemp1 = ChQuaternion.Qscale(qtemp1, 2); // 2( q_dt'm2 * q'o2 * q,o1 * q_dt,m1) q_8 = ChQuaternion.Qadd(q_8, qtemp1); } qtemp1 = ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord().rot), ChQuaternion.Qcross(ChQuaternion.Qconjugate(Body2.GetCoord_dt().rot), ChQuaternion.Qcross(Body1.GetCoord_dt().rot, marker1.FrameMoving.GetCoord().rot))); qtemp1 = ChQuaternion.Qscale(qtemp1, 2); // 2( q'm2 * q_dt'o2 * q_dt,o1 * q,m1) q_8 = ChQuaternion.Qadd(q_8, qtemp1); temp1 = marker1.FrameMoving.GetCoord_dt().rot; if (ChQuaternion.Qnotnull(temp1)) { qtemp1 = ChQuaternion.Qcross( ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord().rot), ChQuaternion.Qcross(ChQuaternion.Qconjugate(Body2.GetCoord_dt().rot), ChQuaternion.Qcross(Body1.GetCoord().rot, marker1.FrameMoving.GetCoord_dt().rot))); qtemp1 = ChQuaternion.Qscale(qtemp1, 2); // 2( q'm2 * q_dt'o2 * q,o1 * q_dt,m1) q_8 = ChQuaternion.Qadd(q_8, qtemp1); } temp1 = marker1.FrameMoving.GetCoord_dt().rot; if (ChQuaternion.Qnotnull(temp1)) { qtemp1 = ChQuaternion.Qcross( ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord().rot), ChQuaternion.Qcross(ChQuaternion.Qconjugate(Body2.GetCoord().rot), ChQuaternion.Qcross(Body1.GetCoord_dt().rot, marker1.FrameMoving.GetCoord_dt().rot))); qtemp1 = ChQuaternion.Qscale(qtemp1, 2); // 2( q'm2 * q'o2 * q_dt,o1 * q_dt,m1) q_8 = ChQuaternion.Qadd(q_8, qtemp1); } // q_4 = [Adtdt]'[A]'q + 2[Adt]'[Adt]'q // + 2[Adt]'[A]'qdt + 2[A]'[Adt]'qdt // ChMatrix33<double> m2_Rel_A_dt = new ChMatrix33<double>(0); marker2.FrameMoving.Compute_Adt(ref m2_Rel_A_dt); // ChMatrix33<double> m2_Rel_A_dtdt = new ChMatrix33<double>(0); marker2.FrameMoving.Compute_Adtdt(ref m2_Rel_A_dtdt); vtemp1 = Body2.GetA_dt().MatrT_x_Vect(PQw); vtemp2 = m2_Rel_A_dt.MatrT_x_Vect(vtemp1); q_4 = ChVector.Vmul(vtemp2, 2); // 2[Aq_dt]'[Ao2_dt]'*Qpq,w vtemp1 = Body2.GetA().MatrT_x_Vect(PQw_dt); vtemp2 = m2_Rel_A_dt.MatrT_x_Vect(vtemp1); vtemp2 = ChVector.Vmul(vtemp2, 2); // 2[Aq_dt]'[Ao2]'*Qpq,w_dt q_4 = ChVector.Vadd(q_4, vtemp2); vtemp1 = Body2.GetA_dt().MatrT_x_Vect(PQw_dt); vtemp2 = marker2.FrameMoving.GetA().MatrT_x_Vect(vtemp1); vtemp2 = ChVector.Vmul(vtemp2, 2); // 2[Aq]'[Ao2_dt]'*Qpq,w_dt q_4 = ChVector.Vadd(q_4, vtemp2); vtemp1 = Body2.GetA().MatrT_x_Vect(PQw); vtemp2 = m2_Rel_A_dtdt.MatrT_x_Vect(vtemp1); q_4 = ChVector.Vadd(q_4, vtemp2); // [Aq_dtdt]'[Ao2]'*Qpq,w // ----------- RELATIVE MARKER COORDINATES // relM.pos relM.pos = marker2.FrameMoving.GetA().MatrT_x_Vect(Body2.GetA().MatrT_x_Vect(PQw)); // relM.rot relM.rot = ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord().rot), ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.GetBody().BodyFrame.GetCoord().rot), ChQuaternion.Qcross((marker1.GetBody().BodyFrame.GetCoord().rot), (marker1.FrameMoving.GetCoord().rot)))); // relM_dt.pos relM_dt.pos = ChVector.Vadd(ChVector.Vadd(m2_Rel_A_dt.MatrT_x_Vect(Body2.GetA().MatrT_x_Vect(PQw)), marker2.FrameMoving.GetA().MatrT_x_Vect(Body2.GetA_dt().MatrT_x_Vect(PQw))), marker2.FrameMoving.GetA().MatrT_x_Vect(Body2.GetA().MatrT_x_Vect(PQw_dt))); // relM_dt.rot relM_dt.rot = ChQuaternion.Qadd(q_AD, q_BC); // relM_dtdt.pos relM_dtdt.pos = ChVector.Vadd(ChVector.Vadd(marker2.FrameMoving.GetA().MatrT_x_Vect(Body2.GetA_dtdt().MatrT_x_Vect(PQw)), marker2.FrameMoving.GetA().MatrT_x_Vect(Body2.GetA().MatrT_x_Vect(PQw_dtdt))), q_4); // relM_dtdt.rot qtemp1 = ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord().rot), ChQuaternion.Qcross(ChQuaternion.Qconjugate(Body2.GetCoord_dtdt().rot), ChQuaternion.Qcross(Body1.GetCoord().rot, marker1.FrameMoving.GetCoord().rot))); // ( q'm2 * q_dtdt'o2 * q,o1 * q,m1) relM_dtdt.rot = ChQuaternion.Qadd(q_8, qtemp1); qtemp1 = ChQuaternion.Qcross(ChQuaternion.Qconjugate(marker2.FrameMoving.GetCoord().rot), ChQuaternion.Qcross(ChQuaternion.Qconjugate(Body2.GetCoord().rot), ChQuaternion.Qcross(Body1.GetCoord_dtdt().rot, marker1.FrameMoving.GetCoord().rot))); // ( q'm2 * q'o2 * q_dtdt,o1 * q,m1) relM_dtdt.rot = ChQuaternion.Qadd(relM_dtdt.rot, qtemp1); // = q_8 + qq''qq + qqq''q // ... and also "user-friendly" relative coordinates: // relAngle and relAxis ChQuaternion.Q_to_AngAxis(relM.rot, ref relAngle, ref relAxis); // flip rel rotation axis if jerky sign if (relAxis.z < 0) { relAxis = ChVector.Vmul(relAxis, -1); relAngle = -relAngle; } // rotation axis relRotaxis = ChVector.Vmul(relAxis, relAngle); // relWvel ChFrame <double> .SetMatrix_Gw(ref relGw, relM.rot); // relGw.Set_Gw_matrix(relM.rot); relWvel = relGw.matrix.Matr34_x_Quat(relM_dt.rot); // relWacc relWacc = relGw.matrix.Matr34_x_Quat(relM_dtdt.rot); }
// // UPDATING FUNCTIONS // /// Perform the update of this joint at the specified time: compute jacobians /// and constraint violations, cache in internal structures public override void update(double time, bool update_assets = true) { // Inherit time changes of parent class base.UpdateTime(time); // Express the joint frames in absolute frame ChFrame <double> frame1_abs = m_frame1.BitShiftRight(Body1); ChFrame <double> frame2_abs = m_frame2.BitShiftRight(Body2); // Calculate violations of the spherical constraints m_C.matrix.SetElement(0, 0, frame2_abs.coord.pos.x - frame1_abs.coord.pos.x); m_C.matrix.SetElement(1, 0, frame2_abs.coord.pos.y - frame1_abs.coord.pos.y); m_C.matrix.SetElement(2, 0, frame2_abs.coord.pos.z - frame1_abs.coord.pos.z); // Compute Jacobian of the spherical constraints // pos2_abs - pos1_abs = 0 { ChMatrix33 <double> tilde1 = new ChMatrix33 <double>(0); ChMatrix33 <double> tilde2 = new ChMatrix33 <double>(0); tilde1.Set_X_matrix(m_frame1.coord.pos); tilde2.Set_X_matrix(m_frame2.coord.pos); ChMatrix33 <double> Phi_pi1 = Body1.GetA() * tilde1; ChMatrix33 <double> Phi_pi2 = Body2.GetA() * tilde2; m_cnstr_x.Get_Cq_a().ElementN(0) = -1; m_cnstr_x.Get_Cq_b().ElementN(0) = +1; m_cnstr_x.Get_Cq_a().ElementN(1) = 0; m_cnstr_x.Get_Cq_b().ElementN(1) = 0; m_cnstr_x.Get_Cq_a().ElementN(2) = 0; m_cnstr_x.Get_Cq_b().ElementN(2) = 0; m_cnstr_x.Get_Cq_a().ElementN(3) = Phi_pi1.nm.matrix[0, 0]; m_cnstr_x.Get_Cq_b().ElementN(3) = -Phi_pi2.nm.matrix[0, 0]; m_cnstr_x.Get_Cq_a().ElementN(4) = Phi_pi1.nm.matrix[0, 1]; m_cnstr_x.Get_Cq_b().ElementN(4) = -Phi_pi2.nm.matrix[0, 1]; m_cnstr_x.Get_Cq_a().ElementN(5) = Phi_pi1.nm.matrix[0, 2]; m_cnstr_x.Get_Cq_b().ElementN(5) = -Phi_pi2.nm.matrix[0, 2]; m_cnstr_y.Get_Cq_a().ElementN(0) = 0; m_cnstr_y.Get_Cq_b().ElementN(0) = 0; m_cnstr_y.Get_Cq_a().ElementN(1) = -1; m_cnstr_y.Get_Cq_b().ElementN(1) = +1; m_cnstr_y.Get_Cq_a().ElementN(2) = 0; m_cnstr_y.Get_Cq_b().ElementN(2) = 0; m_cnstr_y.Get_Cq_a().ElementN(3) = Phi_pi1.nm.matrix[1, 0]; m_cnstr_y.Get_Cq_b().ElementN(3) = -Phi_pi2.nm.matrix[1, 0]; m_cnstr_y.Get_Cq_a().ElementN(4) = Phi_pi1.nm.matrix[1, 1]; m_cnstr_y.Get_Cq_b().ElementN(4) = -Phi_pi2.nm.matrix[1, 1]; m_cnstr_y.Get_Cq_a().ElementN(5) = Phi_pi1.nm.matrix[1, 2]; m_cnstr_y.Get_Cq_b().ElementN(5) = -Phi_pi2.nm.matrix[1, 2]; m_cnstr_z.Get_Cq_a().ElementN(0) = 0; m_cnstr_z.Get_Cq_b().ElementN(0) = 0; m_cnstr_z.Get_Cq_a().ElementN(1) = 0; m_cnstr_z.Get_Cq_b().ElementN(1) = 0; m_cnstr_z.Get_Cq_a().ElementN(2) = -1; m_cnstr_z.Get_Cq_b().ElementN(2) = +1; m_cnstr_z.Get_Cq_a().ElementN(3) = Phi_pi1.nm.matrix[2, 0]; m_cnstr_z.Get_Cq_b().ElementN(3) = -Phi_pi2.nm.matrix[2, 0]; m_cnstr_z.Get_Cq_a().ElementN(4) = Phi_pi1.nm.matrix[2, 1]; m_cnstr_z.Get_Cq_b().ElementN(4) = -Phi_pi2.nm.matrix[2, 1]; m_cnstr_z.Get_Cq_a().ElementN(5) = Phi_pi1.nm.matrix[2, 2]; m_cnstr_z.Get_Cq_b().ElementN(5) = -Phi_pi2.nm.matrix[2, 2]; } // Calculate violation of the dot constraint ChVector u1 = frame1_abs.GetA().Get_A_Xaxis(); ChVector v2 = frame2_abs.GetA().Get_A_Yaxis(); m_C.matrix.SetElement(3, 0, ChVector.Vdot(u1, v2)); // Compute Jacobian of the dot constraint // dot(u1_abs, v2_abs) = 0 { ChMatrix33 <double> mat1 = Body1.GetA() * m_u1_tilde; ChMatrix33 <double> mat2 = Body2.GetA() * m_v2_tilde; ChVector Phi_pi1 = mat1.MatrT_x_Vect(v2); ChVector Phi_pi2 = mat2.MatrT_x_Vect(u1); m_cnstr_dot.Get_Cq_a().ElementN(0) = 0; m_cnstr_dot.Get_Cq_a().ElementN(1) = 0; m_cnstr_dot.Get_Cq_a().ElementN(2) = 0; m_cnstr_dot.Get_Cq_a().ElementN(3) = -Phi_pi1.x; m_cnstr_dot.Get_Cq_a().ElementN(4) = -Phi_pi1.y; m_cnstr_dot.Get_Cq_a().ElementN(5) = -Phi_pi1.z; m_cnstr_dot.Get_Cq_b().ElementN(0) = 0; m_cnstr_dot.Get_Cq_b().ElementN(1) = 0; m_cnstr_dot.Get_Cq_b().ElementN(2) = 0; m_cnstr_dot.Get_Cq_b().ElementN(3) = -Phi_pi2.x; m_cnstr_dot.Get_Cq_b().ElementN(4) = -Phi_pi2.y; m_cnstr_dot.Get_Cq_b().ElementN(5) = -Phi_pi2.z; } }
// // UPDATING FUNCTIONS // /// Perform the update of this joint at the specified time: compute jacobians, /// raint violations, etc. and cache in internal structures public override void update(double time, bool update_assets = true) { // Inherit time changes of parent class (ChLink) base.update(time, update_assets); // Express the body locations and direction in absolute frame ChVector pos1_abs = Body1.TransformPointLocalToParent(m_pos1); ChVector pos2_abs = Body2.TransformPointLocalToParent(m_pos2); ChVector dir1_abs = Body1.TransformDirectionLocalToParent(m_dir1); ChVector d12_abs = pos2_abs - pos1_abs; // Update current distance and dot product m_cur_dist = d12_abs.Length(); m_cur_dot = ChVector.Vdot(d12_abs, dir1_abs); // Calculate a unit vector in the direction d12, expressed in absolute frame // Then express it in the two body frames ChVector u12_abs = d12_abs / m_cur_dist; ChVector u12_loc1 = Body1.TransformDirectionParentToLocal(u12_abs); ChVector u12_loc2 = Body2.TransformDirectionParentToLocal(u12_abs); // Express the direction vector in the frame of body 2 ChVector dir1_loc2 = Body2.TransformDirectionParentToLocal(dir1_abs); // Cache violation of the distance constraint m_C.matrix.SetElement(0, 0, m_cur_dist - m_dist); // Compute Jacobian of the distance constraint // ||pos2_abs - pos1_abs|| - dist = 0 { ChVector Phi_r1 = -u12_abs; ChVector Phi_pi1 = ChVector.Vcross(u12_loc1, m_pos1); m_cnstr_dist.Get_Cq_a().ElementN(0) = Phi_r1.x; m_cnstr_dist.Get_Cq_a().ElementN(1) = Phi_r1.y; m_cnstr_dist.Get_Cq_a().ElementN(2) = Phi_r1.z; m_cnstr_dist.Get_Cq_a().ElementN(3) = Phi_pi1.x; m_cnstr_dist.Get_Cq_a().ElementN(4) = Phi_pi1.y; m_cnstr_dist.Get_Cq_a().ElementN(5) = Phi_pi1.z; ChVector Phi_r2 = u12_abs; ChVector Phi_pi2 = -ChVector.Vcross(u12_loc2, m_pos2); m_cnstr_dist.Get_Cq_b().ElementN(0) = Phi_r2.x; m_cnstr_dist.Get_Cq_b().ElementN(1) = Phi_r2.y; m_cnstr_dist.Get_Cq_b().ElementN(2) = Phi_r2.z; m_cnstr_dist.Get_Cq_b().ElementN(3) = Phi_pi2.x; m_cnstr_dist.Get_Cq_b().ElementN(4) = Phi_pi2.y; m_cnstr_dist.Get_Cq_b().ElementN(5) = Phi_pi2.z; } // Cache violation of the dot constraint m_C.matrix.SetElement(1, 0, m_cur_dot); // Compute Jacobian of the dot constraint // dot(dir1_abs, pos2_abs - pos1_abs) = 0 { ChVector Phi_r1 = -dir1_abs; ChVector Phi_pi1 = ChVector.Vcross(m_dir1, m_pos1) - ChVector.Vcross(u12_loc1, m_pos1); m_cnstr_dot.Get_Cq_a().ElementN(0) = Phi_r1.x; m_cnstr_dot.Get_Cq_a().ElementN(1) = Phi_r1.y; m_cnstr_dot.Get_Cq_a().ElementN(2) = Phi_r1.z; m_cnstr_dot.Get_Cq_a().ElementN(3) = Phi_pi1.x; m_cnstr_dot.Get_Cq_a().ElementN(4) = Phi_pi1.y; m_cnstr_dot.Get_Cq_a().ElementN(5) = Phi_pi1.z; ChVector Phi_r2 = dir1_abs; ChVector Phi_pi2 = -ChVector.Vcross(dir1_loc2, m_pos2); m_cnstr_dot.Get_Cq_b().ElementN(0) = Phi_r2.x; m_cnstr_dot.Get_Cq_b().ElementN(1) = Phi_r2.y; m_cnstr_dot.Get_Cq_b().ElementN(2) = Phi_r2.z; m_cnstr_dot.Get_Cq_b().ElementN(3) = Phi_pi2.x; m_cnstr_dot.Get_Cq_b().ElementN(4) = Phi_pi2.y; m_cnstr_dot.Get_Cq_b().ElementN(5) = Phi_pi2.z; } }
/// Get the current relative angular speed about the common rotation axis. public double GetRotSpringSpeed() { return(ChVector.Vdot(relWvel, relAxis)); }
// Updates motion laws, marker positions, etc. public override void UpdateTime(double mytime) { // First, inherit to parent class base.UpdateTime(mytime); // Move markers 1 and 2 to align them as gear teeth ChMatrix33 <double> ma1 = new ChMatrix33 <double>(0); ChMatrix33 <double> ma2 = new ChMatrix33 <double>(0); ChMatrix33 <double> mrotma = new ChMatrix33 <double>(0); ChMatrix33 <double> marot_beta = new ChMatrix33 <double>(0); ChVector mx; ChVector my; ChVector mz; ChVector mr; ChVector mmark1; ChVector mmark2; ChVector lastX; ChVector vrota; ChCoordsys newmarkpos = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0)); ChFrame <double> abs_shaft1 = ChFrame <double> .FNULL; // new ChFrame<double>(); ChFrame <double> abs_shaft2 = ChFrame <double> .FNULL; //new ChFrame<double>(); ((ChFrame <double>)Body1).TransformLocalToParent(local_shaft1, abs_shaft1); ((ChFrame <double>)Body2).TransformLocalToParent(local_shaft2, abs_shaft2); ChVector vbdist = ChVector.Vsub(Get_shaft_pos1(), Get_shaft_pos2()); // ChVector Trad1 = ChVector.Vnorm(ChVector.Vcross(Get_shaft_dir1(), ChVector.Vnorm(ChVector.Vcross(Get_shaft_dir1(), vbdist)))); // ChVector Trad2 = ChVector.Vnorm(ChVector.Vcross(ChVector.Vnorm(ChVector.Vcross(Get_shaft_dir2(), vbdist)), Get_shaft_dir2())); double dist = ChVector.Vlength(vbdist); // compute actual rotation of the two wheels (relative to truss). ChVector md1 = abs_shaft1.GetA().MatrT_x_Vect(-vbdist); md1.z = 0; md1 = ChVector.Vnorm(md1); ChVector md2 = abs_shaft2.GetA().MatrT_x_Vect(-vbdist); md2.z = 0; md2 = ChVector.Vnorm(md2); double periodic_a1 = ChMaths.ChAtan2(md1.x, md1.y); double periodic_a2 = ChMaths.ChAtan2(md2.x, md2.y); double old_a1 = a1; double old_a2 = a2; double turns_a1 = Math.Floor(old_a1 / ChMaths.CH_C_2PI); double turns_a2 = Math.Floor(old_a2 / ChMaths.CH_C_2PI); double a1U = turns_a1 * ChMaths.CH_C_2PI + periodic_a1 + ChMaths.CH_C_2PI; double a1M = turns_a1 * ChMaths.CH_C_2PI + periodic_a1; double a1L = turns_a1 * ChMaths.CH_C_2PI + periodic_a1 - ChMaths.CH_C_2PI; a1 = a1M; if (Math.Abs(a1U - old_a1) < Math.Abs(a1M - old_a1)) { a1 = a1U; } if (Math.Abs(a1L - a1) < Math.Abs(a1M - a1)) { a1 = a1L; } double a2U = turns_a2 * ChMaths.CH_C_2PI + periodic_a2 + ChMaths.CH_C_2PI; double a2M = turns_a2 * ChMaths.CH_C_2PI + periodic_a2; double a2L = turns_a2 * ChMaths.CH_C_2PI + periodic_a2 - ChMaths.CH_C_2PI; a2 = a2M; if (Math.Abs(a2U - old_a2) < Math.Abs(a2M - old_a2)) { a2 = a2U; } if (Math.Abs(a2L - a2) < Math.Abs(a2M - a2)) { a2 = a2L; } // compute new markers coordsystem alignment my = ChVector.Vnorm(vbdist); mz = Get_shaft_dir1(); mx = ChVector.Vnorm(ChVector.Vcross(my, mz)); mr = ChVector.Vnorm(ChVector.Vcross(mz, mx)); mz = ChVector.Vnorm(ChVector.Vcross(mx, my)); ChVector mz2, mx2, mr2, my2; my2 = my; mz2 = Get_shaft_dir2(); mx2 = ChVector.Vnorm(ChVector.Vcross(my2, mz2)); mr2 = ChVector.Vnorm(ChVector.Vcross(mz2, mx2)); ma1.Set_A_axis(mx, my, mz); // rotate csys because of beta vrota.x = 0.0; vrota.y = beta; vrota.z = 0.0; mrotma.Set_A_Rxyz(vrota); marot_beta.nm.matrix.MatrMultiply(ma1.nm.matrix, mrotma.nm.matrix); // rotate csys because of alpha vrota.x = 0.0; vrota.y = 0.0; vrota.z = alpha; if (react_force.x < 0) { vrota.z = alpha; } else { vrota.z = -alpha; } mrotma.Set_A_Rxyz(vrota); ma1.nm.matrix.MatrMultiply(marot_beta.nm.matrix, mrotma.nm.matrix); ma2.nm.matrix.CopyFromMatrix(ma1.nm.matrix); // is a bevel gear? double be = Math.Acos(ChVector.Vdot(Get_shaft_dir1(), Get_shaft_dir2())); bool is_bevel = true; if (Math.Abs(ChVector.Vdot(Get_shaft_dir1(), Get_shaft_dir2())) > 0.96) { is_bevel = false; } // compute wheel radii so that: // w2 = - tau * w1 if (!is_bevel) { double pardist = ChVector.Vdot(mr, vbdist); double inv_tau = 1.0 / tau; if (!epicyclic) { r2 = pardist - pardist / (inv_tau + 1.0); } else { r2 = pardist - (tau * pardist) / (tau - 1.0); } r1 = r2 * tau; } else { double gamma2; if (!epicyclic) { gamma2 = be / (tau + 1.0); } else { gamma2 = be / (-tau + 1.0); } double al = ChMaths.CH_C_PI - Math.Acos(ChVector.Vdot(Get_shaft_dir2(), my)); double te = ChMaths.CH_C_PI - al - be; double fd = Math.Sin(te) * (dist / Math.Sin(be)); r2 = fd * Math.Tan(gamma2); r1 = r2 * tau; } // compute markers positions, supposing they // stay on the ideal wheel contact point mmark1 = ChVector.Vadd(Get_shaft_pos2(), ChVector.Vmul(mr2, r2)); mmark2 = mmark1; contact_pt = mmark1; // correct marker 1 position if phasing is not correct if (checkphase) { double realtau = tau; if (epicyclic) { realtau = -tau; } double m_delta; m_delta = -(a2 / realtau) - a1 - phase; if (m_delta > ChMaths.CH_C_PI) { m_delta -= (ChMaths.CH_C_2PI); // range -180..+180 is better than 0...360 } if (m_delta > (ChMaths.CH_C_PI / 4.0)) { m_delta = (ChMaths.CH_C_PI / 4.0); // phase correction only in +/- 45° } if (m_delta < -(ChMaths.CH_C_PI / 4.0)) { m_delta = -(ChMaths.CH_C_PI / 4.0); } vrota.x = vrota.y = 0.0; vrota.z = -m_delta; mrotma.Set_A_Rxyz(vrota); // rotate about Z of shaft to correct mmark1 = abs_shaft1.GetA().MatrT_x_Vect(ChVector.Vsub(mmark1, Get_shaft_pos1())); mmark1 = mrotma.Matr_x_Vect(mmark1); mmark1 = ChVector.Vadd(abs_shaft1.GetA().Matr_x_Vect(mmark1), Get_shaft_pos1()); } // Move Shaft 1 along its direction if not aligned to wheel double offset = ChVector.Vdot(Get_shaft_dir1(), (contact_pt - Get_shaft_pos1())); ChVector moff = Get_shaft_dir1() * offset; if (Math.Abs(offset) > 0.0001) { local_shaft1.SetPos(local_shaft1.GetPos() + Body1.TransformDirectionParentToLocal(moff)); } // ! Require that the BDF routine of marker won't handle speed and acc.calculus of the moved marker 2! marker2.SetMotionType(ChMarker.eChMarkerMotion.M_MOTION_EXTERNAL); marker1.SetMotionType(ChMarker.eChMarkerMotion.M_MOTION_EXTERNAL); // move marker1 in proper positions newmarkpos.pos = mmark1; newmarkpos.rot = ma1.Get_A_quaternion(); marker1.Impose_Abs_Coord(newmarkpos); // move marker1 into teeth position // move marker2 in proper positions newmarkpos.pos = mmark2; newmarkpos.rot = ma2.Get_A_quaternion(); marker2.Impose_Abs_Coord(newmarkpos); // move marker2 into teeth position // imposed relative positions/speeds deltaC.pos = ChVector.VNULL; deltaC_dt.pos = ChVector.VNULL; deltaC_dtdt.pos = ChVector.VNULL; deltaC.rot = ChQuaternion.QUNIT; // no relative rotations imposed! deltaC_dt.rot = ChQuaternion.QNULL; deltaC_dtdt.rot = ChQuaternion.QNULL; }