public override void Start() { // shaft1Direction = new Vector3(0, 0, 1); // shaft2Direction = new Vector3(0, 0, 1); // line1 = new GameObject().AddComponent<LineRenderer>(); // line2 = new GameObject().AddComponent<LineRenderer>(); ChCoordsys csys = new ChCoordsys(Utils.ToChrono(transform.position), Utils.ToChrono(transform.rotation)); Initialize(body1, body2, csys); //// TODO: Check that this is correct. var rot1 = UnityEngine.Quaternion.Euler(shaft1Direction); //var rot1 = Quaternion.LookRotation(shaft1Direction.normalized); Set_local_shaft1(new ChFrame <double>(Utils.ToChrono(shaft1Origin), Utils.ToChrono(rot1))); var rot2 = UnityEngine.Quaternion.Euler(shaft2Direction); //var rot2 = Quaternion.LookRotation(shaft2Direction.normalized); Set_local_shaft2(new ChFrame <double>(Utils.ToChrono(shaft2Origin), Utils.ToChrono(rot2))); var height = 2 * transform.localScale.y; var pulley2height = Body2.GetType(); Set_r1(4); Set_r2(2); //ChSystem msystem = FindObjectOfType<ChSystem>(); // msystem.AddLink(this); ChSystem.system.AddLink(this); }
public override void Start() { // shaft1Direction = new Vector3(0, 0, 1); // shaft2Direction = new Vector3(0, 0, 1); ChCoordsys csys = new ChCoordsys(Utils.ToChrono(transform.position), Utils.ToChrono(transform.rotation)); Initialize(body1, body2, csys); //// TODO: Check that this is correct. var rot1 = UnityEngine.Quaternion.Euler(shaft1Direction); //var rot1 = Quaternion.LookRotation(shaft1Direction.normalized); Set_local_shaft1(new ChFrame <double>(Utils.ToChrono(shaft1Origin), Utils.ToChrono(rot1))); var rot2 = UnityEngine.Quaternion.Euler(shaft2Direction); //var rot2 = Quaternion.LookRotation(shaft2Direction.normalized); Set_local_shaft2(new ChFrame <double>(Utils.ToChrono(shaft2Origin), Utils.ToChrono(rot2))); Set_tau(transmissionRatio); Set_epicyclic(epicyclic); Set_checkphase(checkphase); //ChSystem msystem = FindObjectOfType<ChSystem>(); // msystem.AddLink(this); ChSystem.system.AddLink(this); }
/// Use this function after link creation, to initialize the link from /// two joined rigid bodies. /// Both rigid bodies must belong to the same ChSystem. /// Two markers will be created and added to the rigid bodies (later, /// you can use GetMarker1() and GetMarker2() to access them. /// To specify the (absolute) position of link and markers, use 'mpos'. public virtual void Initialize(ChBody mbody1, //< first body to join ChBody mbody2, //< second body to join ChCoordsys mpos //< the current absolute pos.& alignment. ) { Initialize(mbody1, mbody2, false, mpos, mpos); }
public ChLinkMarkers(ChLinkMarkers other) : base(other) { marker1 = null; marker2 = null; markID1 = other.markID1; markID2 = other.markID2; relM = other.relM; // copy vars relM_dt = other.relM_dt; relM_dtdt = other.relM_dtdt; relAngle = other.relAngle; relRotaxis = other.relRotaxis; relAxis = other.relAxis; relWvel = other.relWvel; relWacc = other.relWacc; dist = other.dist; dist_dt = other.dist_dt; C_force = other.C_force; C_torque = other.C_torque; Scr_force = other.Scr_force; Scr_torque = other.Scr_torque; }
public ChMarker(string name, ChBody body, ChCoordsys rel_pos, ChCoordsys rel_pos_dt, ChCoordsys rel_pos_dtdt) { // SetNameString(name); Body = body; motion_X = new ChFunction_Const(0); // default: no motion motion_Y = new ChFunction_Const(0); motion_Z = new ChFunction_Const(0); motion_ang = new ChFunction_Const(0); motion_axis = ChVector.VECT_Z; rest_coord = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0));//ChCoordsys.CSYSNORM; motion_type = eChMarkerMotion.M_MOTION_FUNCTIONS; FrameMoving.SetCoord(rel_pos); FrameMoving.SetCoord_dt(rel_pos_dt); FrameMoving.SetCoord_dtdt(rel_pos_dtdt); last_rel_coord = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0)); //ChCoordsys.CSYSNORM; last_rel_coord_dt = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(0, 0, 0, 0)); // ChCoordsys.CSYSNULL; last_time = 0; UpdateState(); }
public void Start() { ChCoordsys csys = new ChCoordsys(Utils.ToChrono(transform.position), Utils.ToChrono(transform.rotation)); Initialize(body1.BodyFrame, body2.BodyFrame, csys, m_dist); ChSystem.system.AddLink(this); }
/// Get the contact coordinate system, expressed in absolute frame. /// This represents the 'main' reference of the link: reaction forces /// are expressed in this coordinate system. Its origin is point P2. /// (It is the coordinate system of the contact plane and normal) public ChCoordsys GetContactCoords() { ChCoordsys mcsys = ChCoordsys.CSYSNULL; ChQuaternion mrot = this.contact_plane.Get_A_quaternion(); mcsys.rot.Set(mrot.e0, mrot.e1, mrot.e2, mrot.e3); mcsys.pos = this.p2; return(mcsys); }
/// Someone (ex. an ChExternalObject() ) may send this message to /// the marker to tell that time has changed (even if simulation is /// not running! - so it is different from the usual UpdateTime() -) public void UpdatedExternalTime(double prevtime, double mtime) { double mstep = mtime - prevtime; ChCoordsys m_rel_pos_dt = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0)); ChCoordsys m_rel_pos_dtdt = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0)); // do not try to switch on the M_MOTION_KEYFRAMED mode if // we are already in the M_MOTION_EXTERNAL mode, maybe because // a link point-surface is already moving the marker and // it will handle the accelerations by itself if (this.motion_type == eChMarkerMotion.M_MOTION_EXTERNAL) { return; } // otherwise see if a BDF is needed, cause an external 3rd party is moving the marker this.motion_type = eChMarkerMotion.M_MOTION_FUNCTIONS; // if POSITION or ROTATION ("rel_pos") has been changed in acceptable time step... if ((!(ChVector.Vequal(FrameMoving.coord.pos, last_rel_coord.pos)) || !(ChQuaternion.Qequal(FrameMoving.coord.rot, last_rel_coord.rot))) && (Math.Abs(mstep) < 0.1) && (mstep != 0)) { // ... and if motion wasn't caused by motion laws, then it was a keyframed movement! if ((motion_X.Get_y(mtime) == 0) && (motion_Y.Get_y(mtime) == 0) && (motion_Z.Get_y(mtime) == 0) && (motion_ang.Get_y(mtime) == 0) && (motion_X.Get_Type() == ChFunction.FunctionType.FUNCT_CONST) && (motion_Y.Get_Type() == ChFunction.FunctionType.FUNCT_CONST) && (motion_Z.Get_Type() == ChFunction.FunctionType.FUNCT_CONST) && (motion_ang.Get_Type() == ChFunction.FunctionType.FUNCT_CONST)) { // compute the relative speed by BDF ! m_rel_pos_dt.pos = ChVector.Vmul(ChVector.Vsub(FrameMoving.coord.pos, last_rel_coord.pos), 1 / mstep); m_rel_pos_dt.rot = ChQuaternion.Qscale(ChQuaternion.Qsub(FrameMoving.coord.rot, last_rel_coord.rot), 1 / mstep); // compute the relative acceleration by BDF ! m_rel_pos_dtdt.pos = ChVector.Vmul(ChVector.Vsub(m_rel_pos_dt.pos, last_rel_coord_dt.pos), 1 / mstep); m_rel_pos_dtdt.rot = ChQuaternion.Qscale(ChQuaternion.Qsub(m_rel_pos_dt.rot, last_rel_coord_dt.rot), 1 / mstep); // Set the position, speed and acceleration in relative space, // automatically getting also the absolute values, FrameMoving.SetCoord_dt(m_rel_pos_dt); FrameMoving.SetCoord_dtdt(m_rel_pos_dtdt); // update the remaining state variables this.UpdateState(); // remember that the movement of this guy won't need further update // of speed and acc. via motion laws! this.motion_type = eChMarkerMotion.M_MOTION_KEYFRAMED; } } // restore state buffers and that's all. last_time = ChTime; last_rel_coord = (ChCoordsys)FrameMoving.coord; last_rel_coord_dt = (ChCoordsys)FrameMoving.coord_dt; }
/// Set body-relative coord. and update auxiliary variables /// Also, current position becomes the 'resting position' coordinates /// for the current time. public void Impose_Rel_Coord(ChCoordsys m_coord) { ChQuaternion qtemp;// = new ChQuaternion(1, 0, 0, 0); // set the actual coordinates FrameMoving.SetCoord(m_coord); // set the resting position coordinates rest_coord.pos.x = m_coord.pos.x - motion_X.Get_y(ChTime); rest_coord.pos.y = m_coord.pos.y - motion_Y.Get_y(ChTime); rest_coord.pos.z = m_coord.pos.z - motion_Z.Get_y(ChTime); qtemp = ChQuaternion.Q_from_AngAxis2(-(motion_ang.Get_y(ChTime)), motion_axis); rest_coord.rot = ChQuaternion.Qcross(m_coord.rot, qtemp); // ***%%% check // set also the absolute positions, and other. UpdateState(); }
public override void Start() { ChCoordsys pos = ChCoordsys.CSYSNULL; pos.pos.x = transform.position.x; pos.pos.y = transform.position.y; pos.pos.z = transform.position.z; pos.rot.e0 = transform.rotation.w; pos.rot.e1 = transform.rotation.x; pos.rot.e2 = transform.rotation.y; pos.rot.e3 = transform.rotation.z; //ChFrame<double> frame = new ChFrame<double>(ToChrono(transform.position), ToChrono(transform.rotation)); Initialize(body1, body2, pos); Set_shaft_mode(shaft_mode); Set_eng_mode(EngineMode); // Get a handle to the associated function component and set the motor's function var fun_component = this.GetComponent <ChFunction>(); switch (eng_mode) { case eCh_eng_mode.ENG_MODE_ROTATION: if (fun_component != null) { Set_rot_funct(fun_component); } break; case eCh_eng_mode.ENG_MODE_SPEED: if (fun_component != null) { Set_spe_funct(fun_component); } break; case eCh_eng_mode.ENG_MODE_TORQUE: if (fun_component != null) { Set_tor_funct(fun_component); } break; } ChSystem msystem = FindObjectOfType <ChSystem>(); msystem.AddLink(this); //ChSystem.system.AddLink(this); }
/// Set absolute coordinates and update auxiliary variables /// Also, current position becomes the 'resting position' coordinates /// for the current time. public void Impose_Abs_Coord(ChCoordsys m_coord) { ChBody my_body; my_body = GetBody(); ChCoordsys csys;// = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0));//ChCoordsys.CSYSNULL; // coordsys: transform the representation from the parent reference frame // to the local reference frame. csys.pos = ChTransform <double> .TransformParentToLocal(m_coord.pos, my_body.BodyFrame.GetCoord().pos, my_body.BodyFrame.GetA()); csys.rot = ChQuaternion.Qcross(ChQuaternion.Qconjugate(my_body.BodyFrame.GetCoord().rot), m_coord.rot); // apply the imposition on local coordinate and resting coordinate: Impose_Rel_Coord(csys); }
private ChFrameMoving <double> abs_frame = new ChFrameMoving <double>(); //< absolute frame position /* public void Start() * { * FrameMoving = new ChFrameMoving<double>(); * Body = null; * rest_coord = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0));//ChCoordsys.CSYSNORM; * motion_type = eChMarkerMotion.M_MOTION_FUNCTIONS; * motion_axis = ChVector.VECT_Z; * last_rel_coord = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0));//ChCoordsys.CSYSNORM; * last_rel_coord_dt = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(0, 0, 0, 0));// ChCoordsys.CSYSNULL; * last_time = 0; * motion_X = new ChFunction_Const(0); // default: no motion * motion_Y = new ChFunction_Const(0); * motion_Z = new ChFunction_Const(0); * motion_ang = new ChFunction_Const(0); * * UpdateState(); * }*/ public ChMarker() { FrameMoving = new ChFrameMoving <double>(); Body = null; rest_coord = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0));//ChCoordsys.CSYSNORM; motion_type = eChMarkerMotion.M_MOTION_FUNCTIONS; motion_axis = ChVector.VECT_Z; last_rel_coord = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0)); //ChCoordsys.CSYSNORM; last_rel_coord_dt = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(0, 0, 0, 0)); // ChCoordsys.CSYSNULL; last_time = 0; motion_X = new ChFunction_Const(0); // default: no motion motion_Y = new ChFunction_Const(0); motion_Z = new ChFunction_Const(0); motion_ang = new ChFunction_Const(0); UpdateState(); }
public ChLinkMarkers() { marker1 = null; marker2 = null; markID1 = 0; markID2 = 0; relM = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0)); //Coordsys.CSYSNORM; relM_dt = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(0, 0, 0, 0)); //ChCoordsys.CSYSNULL; relM_dtdt = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(0, 0, 0, 0)); // ChCoordsys.CSYSNULL; relAngle = 0; relRotaxis = new ChVector(0, 0, 0); relWvel = new ChVector(0, 0, 0); relWacc = new ChVector(0, 0, 0); C_force = new ChVector(0, 0, 0); C_torque = new ChVector(0, 0, 0); Scr_force = new ChVector(0, 0, 0); Scr_torque = new ChVector(0, 0, 0); }
public void Start() { ChCoordsys csys = new ChCoordsys(Utils.ToChrono(transform.position), Utils.ToChrono(transform.rotation)); Initialize(body1, body2, csys); // Get a handle to the associated function component and set the motor's function var fun_component = this.GetComponent <TorqueFunctor>(); if (fun_component != null) { RegisterTorqueFunctor(fun_component); } ChSystem msystem = FindObjectOfType <ChSystem>(); msystem.AddLink(this); }
public ChMarker(ChMarker other) : base(other) { FrameMoving = new ChFrameMoving <double>(other.FrameMoving); Body = null; motion_X = new ChFunction(other.motion_X.Clone()); motion_Y = new ChFunction(other.motion_Y.Clone()); motion_Z = new ChFunction(other.motion_Z.Clone()); motion_ang = new ChFunction(other.motion_ang.Clone()); motion_axis = other.motion_axis; rest_coord = other.rest_coord; motion_type = other.motion_type; abs_frame = other.abs_frame; last_rel_coord = other.last_rel_coord; last_rel_coord_dt = other.last_rel_coord_dt; last_time = other.last_time; }
/// Initialize this joint by specifying the two bodies to be connected, a /// coordinate system specified in the absolute frame, and the distance of /// the massless connector. The composite joint is ructed such that the /// direction of the revolute joint is aligned with the z axis of the specified /// coordinate system and the spherical joint is at the specified distance /// along the x axis. public void Initialize(ChBodyFrame body1, //< first frame (revolute side) ChBodyFrame body2, //< second frame (spherical side) ChCoordsys csys, //< joint coordinate system (in absolute frame) double distance //< imposed distance ) { Body1 = body1; Body2 = body2; m_cnstr_dist.SetVariables(Body1.Variables(), Body2.Variables()); m_cnstr_dot.SetVariables(Body1.Variables(), Body2.Variables()); ChVector x_Axis = csys.rot.GetXaxis(); ChVector z_axis = csys.rot.GetZaxis(); m_pos1 = Body1.TransformPointParentToLocal(csys.pos); m_dir1 = Body1.TransformDirectionParentToLocal(z_axis); m_pos2 = Body2.TransformPointParentToLocal(csys.pos + distance * x_Axis); m_dist = distance; m_cur_dist = distance; m_cur_dot = 0; }
// ----------------------------------------------------------------------------- // 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; } }
/// Use this function after link creation, to initialize the link from /// two joined rigid bodies. /// Both rigid bodies must belong to the same ChSystem. /// Two markers will be created and added to the rigid bodies (later, /// you can use GetMarker1() and GetMarker2() to access them. /// To specify the (absolute) position of link and markers, use 'mpos'. public virtual void Initialize( ChBody mbody1, //< first body to join ChBody mbody2, //< second body to join bool pos_are_relative, //< if =true, following two positions are relative to bodies. If false, are absolute. ChCoordsys mpos1, //< the position & alignment of 1st marker (relative to body1 cords, or absolute) ChCoordsys mpos2 //< the position & alignment of 2nd marker (relative to body2 cords, or absolute) ) { // Debug.Assert(mbody1 != mbody2); // Debug.Assert(mbody1.GetSystem() == mbody2.GetSystem()); // create markers to add to the two bodies //ChMarker mmark1 = new ChMarker(new ChMarker()); //ChMarker mmark2 = new ChMarker(new ChMarker()); ChMarker mmark1 = gameObject.AddComponent <ChMarker>(); ChMarker mmark2 = gameObject.AddComponent <ChMarker>(); mbody1.AddMarker(mmark1); mbody2.AddMarker(mmark2); ChMarker mm1 = mmark1; ChMarker mm2 = mmark2; ReferenceMarkers(mm1, mm2); if (pos_are_relative) { mmark1.Impose_Rel_Coord(mpos1); mmark2.Impose_Rel_Coord(mpos2); } else { mmark1.Impose_Abs_Coord(mpos1); mmark2.Impose_Abs_Coord(mpos2); } }
// // UPDATING // /// Updates the time.dependant variables (ex: ChFunction objects /// which impose the body-relative motion, etc.) public void UpdateTime(double mytime) { ChCoordsys csys = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0)); ChCoordsys csys_dt = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0)); ChCoordsys csys_dtdt = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0)); ChQuaternion qtemp;// = new ChQuaternion(1, 0, 0, 0); double ang, ang_dt, ang_dtdt; ChTime = mytime; // if a imposed motion (keyframed movement) affects the marker position (example,from R3D animation system), // compute the speed and acceleration values by BDF (example,see the UpdatedExternalTime() function, later) // so the updating via motion laws can be skipped! if (motion_type == eChMarkerMotion.M_MOTION_KEYFRAMED) { return; } // skip relative-position-functions evaluation also if // someone is already handling this from outside.. if (motion_type == eChMarkerMotion.M_MOTION_EXTERNAL) { return; } // positions: // update positions: rel_pos csys.pos.x = motion_X.Get_y(mytime); csys.pos.y = motion_Y.Get_y(mytime); csys.pos.z = motion_Z.Get_y(mytime); if (motion_X.Get_Type() != ChFunction.FunctionType.FUNCT_MOCAP) { csys.pos += rest_coord.pos; } // update speeds: rel_pos_dt csys_dt.pos.x = motion_X.Get_y_dx(mytime); csys_dt.pos.y = motion_Y.Get_y_dx(mytime); csys_dt.pos.z = motion_Z.Get_y_dx(mytime); // update accelerations csys_dtdt.pos.x = motion_X.Get_y_dxdx(mytime); csys_dtdt.pos.y = motion_Y.Get_y_dxdx(mytime); csys_dtdt.pos.z = motion_Z.Get_y_dxdx(mytime); // rotations: ang = motion_ang.Get_y(mytime); ang_dt = motion_ang.Get_y_dx(mytime); ang_dtdt = motion_ang.Get_y_dxdx(mytime); if ((ang != 0) || (ang_dt != 0) || (ang_dtdt != 0)) { // update q ChVector motion_axis_versor = ChVector.Vnorm(motion_axis); qtemp = ChQuaternion.Q_from_AngAxis2(ang, motion_axis_versor); csys.rot = ChQuaternion.Qcross(qtemp, rest_coord.rot); // update q_dt csys_dt.rot = ChQuaternion.Qdt_from_AngAxis(csys.rot, ang_dt, motion_axis_versor); // update q_dtdt csys_dtdt.rot = ChQuaternion.Qdtdt_from_AngAxis(ang_dtdt, motion_axis_versor, csys.rot, csys_dt.rot); } else { csys.rot = FrameMoving.coord.rot; // rel_pos.rot; csys_dt.rot = ChQuaternion.QNULL; csys_dtdt.rot = ChQuaternion.QNULL; } // Set the position, speed and acceleration in relative space, // automatically getting also the absolute values, if (!(csys == this.FrameMoving.coord)) { FrameMoving.SetCoord(csys); } if (!(csys_dt == this.FrameMoving.coord_dt) || !(csys_dt.rot == new ChQuaternion(0, 0, 0, 0))) { FrameMoving.SetCoord_dt(csys_dt); } if (!(csys_dtdt == this.FrameMoving.coord_dtdt) || !(csys_dtdt.rot == new ChQuaternion(0, 0, 0, 0))) { FrameMoving.SetCoord_dtdt(csys_dtdt); } }
/// Set the speed of translation and rotation (as a ChCoordsys) of the marker /// respect to the absolute coordinates. /// NOTE! inner use only, for the moment. public void SetAbsCoord_dtdt(ChCoordsys newpos_dtdt) { abs_frame.SetCoord(newpos_dtdt); }
/// Set the translation and rotation (as a ChCoordsys) of the marker /// respect to the absolute coordinates. /// NOTE! inner use only, for the moment. Use Impose_Abs_Coord() if needed. public void SetAbsCoord(ChCoordsys newpos) { abs_frame.SetCoord(newpos); }
/// Copy constructor, build from another frame public ChFrame(ChFrame <Real> other) { coord = other.coord; Amatrix = other.Amatrix; }
/// Get the link coordinate system in absolute reference. /// This represents the 'main' reference of the link: reaction forces /// and reaction torques are expressed in this coordinate system. /// Child classes should implement this. public override ChCoordsys GetLinkAbsoluteCoords() { return(ChCoordsys.BitShiftRight(GetLinkRelativeCoords(), Body2.GetCoord())); }
public bool Equals(ChCoordsys other, double tol) { return(rot.Equals(other.rot, tol) && pos.Equals(other.pos, tol)); }
/// Returns true if coordsys is identical to other coordsys public bool Equals(ChCoordsys other) { return(rot.Equals(other.rot) && pos.Equals(other.pos)); }
public void PasteCoordsys(ChCoordsys cs, int insrow, int inscol) { PasteVector(cs.pos, insrow, inscol); PasteQuaternion(cs.rot, insrow + 3, inscol); }
/// 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; }
/// Returns true if coordsys is equal to other coordsys, within a tolerance 'tol' /* public bool Equals(ChCoordsys other, double tol) * { * return rot.Equals(other.rot, tol) && pos.Equals(other.pos, tol); * }*/ public static ChCoordsys BitShiftRight(ChCoordsys a, ChCoordsys Fb) { return(Fb.TransformLocalToParent(a)); }
/// This function transforms a coordsys given in 'this' coordinate system to /// the parent coordinate system public ChCoordsys TransformLocalToParent(ChCoordsys local) { return(new ChCoordsys(TransformLocalToParent(local.pos), rot % local.rot)); }
/// Copy constructor public ChCoordsys(ChCoordsys other) : this() { pos = other.pos; rot = other.rot; }