public ChShaftsBody(ChShaftsBody other) { torque_react = other.torque_react; shaft_dir = other.shaft_dir; shaft = null; body = null; }
/// Initialization based on passing two vectors (point + dir) on the /// two bodies, they will represent the X axes of the two frames (Y and Z will /// be built from the X vector via Gram Schmidt orthonormalization). /// Use the other ChLinkMateGeneric::Initialize() if you want to set the two frames directly. public virtual void Initialize(ChBodyFrame mbody1, //< first body to link ChBodyFrame mbody2, //< second body to link bool pos_are_relative, //< true: following pos. are relative to bodies. ChVector mpt1, //< origin of slave frame 1, for 1st body (rel. or abs., see flag above) ChVector mpt2, //< origin of master frame 2, for 2nd body (rel. or abs., see flag above) ChVector mnorm1, //< X axis of slave plane, for 1st body (rel. or abs., see flag above) ChVector mnorm2 //< X axis of master plane, for 2nd body (rel. or abs., see flag above) ) { Debug.Assert(mbody1 != mbody2); this.Body1 = mbody1; this.Body2 = mbody2; // this.SetSystem(mbody1.GetSystem()); this.mask.SetTwoBodiesVariables(Body1.Variables(), Body2.Variables()); ChVector mx = new ChVector(0, 0, 0); ChVector my = new ChVector(0, 0, 0); ChVector mz = new ChVector(0, 0, 0); ChVector mN = new ChVector(0, 0, 0); ChMatrix33 <double> mrot = new ChMatrix33 <double>(); ChFrame <double> mfr1 = new ChFrame <double>(); ChFrame <double> mfr2 = new ChFrame <double>(); if (pos_are_relative) { mN = mnorm1; mN.DirToDxDyDz(ref mx, ref my, ref mz, new ChVector(0, 1, 0)); mrot.Set_A_axis(mx, my, mz); mfr1.SetRot(mrot); mfr1.SetPos(mpt1); mN = mnorm2; mN.DirToDxDyDz(ref mx, ref my, ref mz, new ChVector(0, 1, 0)); mrot.Set_A_axis(mx, my, mz); mfr2.SetRot(mrot); mfr2.SetPos(mpt2); } else { ChVector temp = ChVector.VECT_Z; // from abs to body-rel mN = this.Body1.TransformDirectionParentToLocal(mnorm1); mN.DirToDxDyDz(ref mx, ref my, ref mz, temp); mrot.Set_A_axis(mx, my, mz); mfr1.SetRot(mrot); mfr1.SetPos(this.Body1.TransformPointParentToLocal(mpt1)); mN = this.Body2.TransformDirectionParentToLocal(mnorm2); mN.DirToDxDyDz(ref mx, ref my, ref mz, temp); mrot.Set_A_axis(mx, my, mz); mfr2.SetRot(mrot); mfr2.SetPos(this.Body2.TransformPointParentToLocal(mpt2)); } this.frame1 = mfr1; this.frame2 = mfr2; }
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; }
/// 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; }
// TRANSFORMATIONS, USING POSITION AND ROTATION QUATERNION /// This function transforms a point from the parent coordinate /// system to a local coordinate system, whose relative position /// is given by the 'origin' translation and 'alignment' quaternion q. /// Since the function is static, you do not need a ChTransform object, for example /// use it as: mresult=ChTransform<>::TransformParentToLocal(mpar, morig, malign) /// \return The point in local coordinate, as local=q*[(parent-origin)]*q public static ChVector TransformParentToLocal( ChVector parent, //< point to transform, given in parent coordinates ChVector origin, //< origin of frame respect to parent, in parent coords, ChQuaternion alignment //< rotation of frame respect to parent, in parent coords. ) { // It could be simply "return alignment.RotateBack(parent-origin);" // but for faster execution do this: double e0e0 = alignment.e0 * alignment.e0; double e1e1 = alignment.e1 * alignment.e1; double e2e2 = alignment.e2 * alignment.e2; double e3e3 = alignment.e3 * alignment.e3; double e0e1 = -alignment.e0 * alignment.e1; double e0e2 = -alignment.e0 * alignment.e2; double e0e3 = -alignment.e0 * alignment.e3; double e1e2 = alignment.e1 * alignment.e2; double e1e3 = alignment.e1 * alignment.e3; double e2e3 = alignment.e2 * alignment.e3; double dx = parent.x - origin.x; double dy = parent.y - origin.y; double dz = parent.z - origin.z; return(new ChVector(((e0e0 + e1e1) * 2.0 - 1.0) * dx + ((e1e2 - e0e3) * 2.0) * dy + ((e1e3 + e0e2) * 2.0) * dz, ((e1e2 + e0e3) * 2.0) * dx + ((e0e0 + e2e2) * 2.0 - 1.0) * dy + ((e2e3 - e0e1) * 2.0) * dz, ((e1e3 - e0e2) * 2.0) * dx + ((e2e3 + e0e1) * 2.0) * dy + ((e0e0 + e3e3) * 2.0 - 1.0) * dz)); }
public ChVector(ChVector other) //: this() { //data = new double[3]; this.x = other.x; this.y = other.y; this.z = other.z; }
public static ChVector Vnorm(ChVector va) { ChVector result = new ChVector(va); result.Normalize(); return(result); }
public ChVector Cross(ChVector other) { ChVector v = ChVector.VNULL; v.Cross(this, other); return(v); }
// // FUNCTIONS // /// Initialize again this constraint. public virtual void Reset(Ta mobjA, //< ChContactable object A Tb mobjB, //< ChContactable object B collision.ChCollisionInfo cinfo //< data for the contact pair ) { //Debug.Assert(mobjA); //Debug.Assert(mobjB); this.objA = mobjA; this.objB = mobjB; this.p1 = cinfo.vpA; this.p2 = cinfo.vpB; this.normal = cinfo.vN; this.norm_dist = cinfo.distance; this.eff_radius = cinfo.eff_radius; // Contact plane ChVector Vx = new ChVector(0, 0, 0); ChVector Vy = new ChVector(0, 0, 0); ChVector Vz = new ChVector(0, 0, 0); ChVector.XdirToDxDyDz(normal, ChVector.VECT_Y, ref Vx, ref Vy, ref Vz); contact_plane.Set_A_axis(Vx, Vy, Vz); }
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 static ChVector operator %(ChVector a, ChVector other) { ChVector v = new ChVector(0, 0, 0); v.Cross(a, other); return(v); }
/// 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 ChVector GetNormalized() { ChVector v = new ChVector(this); v.Normalize(); return(v); }
public void Q_to_AngAxis(ref double a_angle, ref ChVector a_axis) { double sin_squared = this.e1 * this.e1 + this.e2 * this.e2 + this.e3 * this.e3; // For non-zero rotation if (sin_squared > 0) { double sin_theta = Math.Sqrt(sin_squared); a_angle = 2.0 * Math.Atan2(sin_theta, e0); double k = 1.0 / sin_theta; a_axis.x = this.e1 * k; a_axis.y = this.e2 * k; a_axis.z = this.e3 * k; a_axis.Normalize(); } else { // For almost zero rotation a_angle = 0.0; a_axis.x = 1; // e1 * 2.0; a_axis.y = 0; // e2 * 2.0; a_axis.z = 0; // e3 * 2.0; } // Ensure that angle is always in [-PI...PI] range var PI = (ChMaths.CH_C_PI); if (a_angle > PI) { a_angle -= 2 * PI; } else if (a_angle < -PI) { a_angle += 2 * PI; } }
/// Set the quaternion ddq/dtdt. Inputs: the axis of ang. acceleration 'axis' (assuming it is already /// normalized and expressed in absolute coords), the angular acceleration 'angle_dtdt' (scalar value), /// the rotation expressed as a quaternion 'quat' and th rotation speed 'q_dt'. public void Qdtdt_from_AngAxis(ChQuaternion q, ChQuaternion q_dt, double angle_dtdt, ChVector axis) { this.Qdtdt_from_Aabs2(angle_dtdt * axis, q, q_dt); }
protected ChVector react_torque = new ChVector(0, 0, 0); //< store the torque reactions, expressed in local coordinate system of link; public ChLink() { //Body1 = null; //Body2 = null; react_force = new ChVector(0, 0, 0); react_torque = new ChVector(0, 0, 0); }
public ChLinkDistance() { pos1 = new ChVector(0, 0, 0); pos2 = new ChVector(0, 0, 0); distance = 0; curr_dist = 0; }
/// Get angle of rotation about axis of finite rotation public double GetRotAngle() { ChVector vtmp = new ChVector(0, 0, 0); double angle = 0; coord.rot.Q_to_AngAxis(ref angle, ref vtmp); return(angle); }
public ChMatrix33(ChVector vec)// : this() { nm = new ChMatrixNM <IntInterface.Three, IntInterface.Three>(0); // nm.matrix = new ChMatrix(); this.nm.matrix.Set33Element(0, 0, vec.x); this.nm.matrix.Set33Element(1, 1, vec.y); this.nm.matrix.Set33Element(2, 2, vec.z); }
/// Sets the rotation matrix from an gale of rotation and an axis, /// defined in _absolute_ coords. NOTE, axis must be normalized! public void Set_A_AngAxis(double angle, //< angle of rotation, in radians ChVector axis) //< axis of rotation, normalized { ChQuaternion mr = new ChQuaternion(0, 0, 0, 0); // ChQuaternion.QNULL; mr.Q_from_AngAxis(angle, axis); this.Set_A_quaternion(mr); }
public ChQuaternion(double s, ChVector v) : this() { // data = new double[4]; this.e0 = s; this.e1 = v.x; this.e2 = v.y; this.e3 = v.z; }
// Get the time derivative from a quaternion, a speed of rotation and an axis, defined in _abs_ coords. public static ChQuaternion Qdt_from_AngAxis(ChQuaternion quat, double angle_dt, ChVector axis) { ChVector W; W = ChVector.Vmul(axis, angle_dt); return(Qdt_from_Wabs(W, quat)); }
public ChLink(ChLink other) : base(other) { Body1 = null; Body2 = null; react_force = other.react_force; react_torque = other.react_torque; }
public override void IntStateScatterReactions(int off_L, ChVectorDynamic <double> L) { react_force = new ChVector(0, 0, 0); // Do not update 'intuitive' react force and torque here: just set as 0. react_torque = new ChVector(0, 0, 0); // Child classes implementations should compute them. // if (react != null) react.matrix.PasteClippedMatrix(L.matrix, off_L, 0, react.matrix.GetRows(), 1, 0, 0); }
/// Multiplies this matrix by a vector, like in coordinate rotation [M]*v. /// \return The result of the multiplication, i.e. a vector. public ChVector Matr_x_Vect(ChVector va) { return(new ChVector(this.nm.matrix.Get33Element(0, 0) * va.x + this.nm.matrix.Get33Element(0, 1) * va.y + this.nm.matrix.Get33Element(0, 2) * va.z, this.nm.matrix.Get33Element(1, 0) * va.x + this.nm.matrix.Get33Element(1, 1) * va.y + this.nm.matrix.Get33Element(1, 2) * va.z, this.nm.matrix.Get33Element(2, 0) * va.x + this.nm.matrix.Get33Element(2, 1) * va.y + this.nm.matrix.Get33Element(2, 2) * va.z)); }
// Get the quaternion first derivative from the vector of angular acceleration with a specified in _absolute_ coords. public static ChQuaternion Qdtdt_from_Aabs(ChVector a, ChQuaternion q, ChQuaternion q_dt) { ChQuaternion ret = new ChQuaternion(1, 0, 0, 0); //QNULL; ret.Qdtdt_from_Aabs2(a, q, q_dt); return(ret); }
/// Given a 3x3 rotation matrix, returns the versor of Y axis. public ChVector Get_A_Yaxis() { ChVector Y = new ChVector(0, 0, 0);// ChVector.VNULL; Y.x = this.nm.matrix.Get33Element(0, 1); Y.y = this.nm.matrix.Get33Element(1, 1); Y.z = this.nm.matrix.Get33Element(2, 1); return(Y); }
/// Given a 3x3 rotation matrix, returns the versor of Z axis. public ChVector Get_A_Zaxis() { ChVector Z = new ChVector(0, 0, 0); //ChVector.VNULL; Z.x = this.nm.matrix.Get33Element(0, 2); Z.y = this.nm.matrix.Get33Element(1, 2); Z.z = this.nm.matrix.Get33Element(2, 2); return(Z); }
public override void ConstraintsFetch_react(double factor = 1) { // From constraints to react vector: react_force.x = -Cx.Get_l_i() * factor; react_force.y = 0; react_force.z = 0; react_torque = new ChVector(0, 0, 0); }
/// Given a 3x3 rotation matrix, returns the versor of X axis. public ChVector Get_A_Xaxis() { ChVector X = new ChVector(0, 0, 0);// ChVector.VNULL; X.x = this.nm.matrix.Get33Element(0, 0); X.y = this.nm.matrix.Get33Element(1, 0); X.z = this.nm.matrix.Get33Element(2, 0); return(X); }