/// 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; }
public void Set_eng_mode(eCh_eng_mode mset) { if (Get_learn()) { Set_learn(false); // reset learn state when changing mode } if (eng_mode != mset) { eng_mode = mset; switch (eng_mode) { case eCh_eng_mode.ENG_MODE_ROTATION: case eCh_eng_mode.ENG_MODE_SPEED: case eCh_eng_mode.ENG_MODE_KEY_ROTATION: ((ChLinkMaskLF)mask).Constr_E3().SetMode(eChConstraintMode.CONSTRAINT_LOCK); break; case eCh_eng_mode.ENG_MODE_KEY_POLAR: ((ChLinkMaskLF)mask).Constr_E1().SetMode(eChConstraintMode.CONSTRAINT_LOCK); ((ChLinkMaskLF)mask).Constr_E2().SetMode(eChConstraintMode.CONSTRAINT_LOCK); ((ChLinkMaskLF)mask).Constr_E3().SetMode(eChConstraintMode.CONSTRAINT_LOCK); break; case eCh_eng_mode.ENG_MODE_TORQUE: ((ChLinkMaskLF)mask).Constr_E3().SetMode(eChConstraintMode.CONSTRAINT_FREE); break; case eCh_eng_mode.ENG_MODE_TO_POWERTRAIN_SHAFT: ((ChLinkMaskLF)mask).Constr_E3().SetMode(eChConstraintMode.CONSTRAINT_FREE); innerconstraint1 = gameObject.AddComponent <ChShaftsBody>(); innerconstraint2 = gameObject.AddComponent <ChShaftsBody>(); innershaft1 = gameObject.AddComponent <ChShaft>(); innershaft2 = gameObject.AddComponent <ChShaft>(); SetUpMarkers(marker1, marker2); // to initialize innerconstraint1 innerconstraint2 break; } ChangedLinkMask(); // update all from new mask } if (eng_mode == eCh_eng_mode.ENG_MODE_KEY_ROTATION && rot_funct.Get_Type() != ChFunction.FunctionType.FUNCT_CONST) { rot_funct = new ChFunction_Const(); } if (eng_mode == eCh_eng_mode.ENG_MODE_KEY_POLAR) { if (rot_funct.Get_Type() != ChFunction.FunctionType.FUNCT_CONST) { rot_funct = new ChFunction_Const(); } if (rot_funct_x.Get_Type() != ChFunction.FunctionType.FUNCT_CONST) { rot_funct_x = new ChFunction_Const(); } if (rot_funct_y.Get_Type() != ChFunction.FunctionType.FUNCT_CONST) { rot_funct_y = new ChFunction_Const(); } } }
// // 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); } }
/// 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); } }