void ChLinkDistance::Update (double mytime) { // Inherit time changes of parent class (ChLink), basically doing nothing :) ChLink::UpdateTime(mytime); // compute jacobians ChVector<> AbsDist = Body1->Point_Body2World(&pos1)-Body2->Point_Body2World(&pos2); curr_dist = AbsDist.Length(); ChVector<> D2abs = Vnorm(AbsDist); ChVector<> D2relB = Body2->Dir_World2Body(&D2abs); ChVector<> D2relA = Body1->Dir_World2Body(&D2abs); ChVector<> CqAx = D2abs; ChVector<> CqBx = -D2abs; ChVector<> CqAr = -Vcross(D2relA,pos1); ChVector<> CqBr = Vcross(D2relB,pos2); Cx.Get_Cq_a()->ElementN(0)=(float)CqAx.x; Cx.Get_Cq_a()->ElementN(1)=(float)CqAx.y; Cx.Get_Cq_a()->ElementN(2)=(float)CqAx.z; Cx.Get_Cq_a()->ElementN(3)=(float)CqAr.x; Cx.Get_Cq_a()->ElementN(4)=(float)CqAr.y; Cx.Get_Cq_a()->ElementN(5)=(float)CqAr.z; Cx.Get_Cq_b()->ElementN(0)=(float)CqBx.x; Cx.Get_Cq_b()->ElementN(1)=(float)CqBx.y; Cx.Get_Cq_b()->ElementN(2)=(float)CqBx.z; Cx.Get_Cq_b()->ElementN(3)=(float)CqBr.x; Cx.Get_Cq_b()->ElementN(4)=(float)CqBr.y; Cx.Get_Cq_b()->ElementN(5)=(float)CqBr.z; //***TO DO*** C_dt? C_dtdt? (may be never used..) }
int ChPathSteeringControllerXT::CalcCurvatureCode(ChVector<>& a, ChVector<>& b) { // a[] is a unit vector pointing to the left vehicle side // b[] is a unit vector pointing to the instantanous curve center a.z() = 0; a.Normalize(); b.z() = 0; b.Normalize(); // In a left turn the distance between the two points will be nearly zero // in a right turn the distance will be around 2, at least > 1 ChVector<> ab = a - b; double ltest = ab.Length(); // What is a straight line? We define a threshold radius R_threshold // if the actual curvature is greater than 1/R_treshold, we are in a curve // otherwise we take this point as part of a straight line // m_pcurvature is always >= 0 if(m_pcurvature <= 1.0/m_R_threshold) { return 0; // -> straight line } if(ltest < 1.0) { return 1; // left bending curve } return -1; // right bending curve }
void ChLinkDistance::Update(double mytime, bool update_assets) { // Inherit time changes of parent class (ChLink), basically doing nothing :) ChLink::Update(mytime, update_assets); // compute jacobians ChVector<> AbsDist = Body1->TransformPointLocalToParent(pos1) - Body2->TransformPointLocalToParent(pos2); curr_dist = AbsDist.Length(); ChVector<> D2abs = Vnorm(AbsDist); ChVector<> D2relB = Body2->TransformDirectionParentToLocal(D2abs); ChVector<> D2relA = Body1->TransformDirectionParentToLocal(D2abs); ChVector<> CqAx = D2abs; ChVector<> CqBx = -D2abs; ChVector<> CqAr = -Vcross(D2relA, pos1); ChVector<> CqBr = 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..) }
int ChLinkDistance::Initialize(std::shared_ptr<ChBodyFrame> mbody1, std::shared_ptr<ChBodyFrame> mbody2, bool pos_are_relative, ChVector<> mpos1, ChVector<> mpos2, bool auto_distance, double mdistance) { Body1 = mbody1.get(); Body2 = mbody2.get(); 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; }
// Get the quaternion from a source vector and a destination vector which specifies // the rotation from one to the other. The vectors do not need to be normalized. ChQuaternion<double> Q_from_Vect_to_Vect(const ChVector<double>& fr_vect, const ChVector<double>& to_vect) { const double ANGLE_TOLERANCE = 1e-6; ChQuaternion<double> quat; double halfang; double sinhalf; ChVector<double> axis; double lenXlen = fr_vect.Length() * to_vect.Length(); axis = fr_vect % to_vect; double sinangle = ChClamp(axis.Length() / lenXlen, -1.0, +1.0); double cosangle = ChClamp(fr_vect ^ to_vect / lenXlen, -1.0, +1.0); // Consider three cases: Parallel, Opposite, non-collinear if (std::abs(sinangle) == 0.0 && cosangle > 0) { // fr_vect & to_vect are parallel quat.e0() = 1.0; quat.e1() = 0.0; quat.e2() = 0.0; quat.e3() = 0.0; } else if (std::abs(sinangle) < ANGLE_TOLERANCE && cosangle < 0) { // fr_vect & to_vect are opposite, i.e. ~180 deg apart axis = fr_vect.GetOrthogonalVector() + (-to_vect).GetOrthogonalVector(); axis.Normalize(); quat.e0() = 0.0; quat.e1() = ChClamp(axis.x(), -1.0, +1.0); quat.e2() = ChClamp(axis.y(), -1.0, +1.0); quat.e3() = ChClamp(axis.z(), -1.0, +1.0); } else { // fr_vect & to_vect are not co-linear case axis.Normalize(); halfang = 0.5 * ChAtan2(sinangle, cosangle); sinhalf = sin(halfang); quat.e0() = cos(halfang); quat.e1() = ChClamp(axis.x(), -1.0, +1.0); quat.e2() = ChClamp(axis.y(), -1.0, +1.0); quat.e3() = ChClamp(axis.z(), -1.0, +1.0); } return (quat); }
int ChLinkDistance::Initialize(ChSharedPtr<ChBody>& mbody1, ///< first body to link ChSharedPtr<ChBody>& mbody2, ///< second body to link bool pos_are_relative,///< true: following posit. are considered relative to bodies. false: pos.are absolute ChVector<> mpos1, ///< position of distance endpoint, for 1st body (rel. or abs., see flag above) ChVector<> mpos2, ///< position of distance endpoint, for 2nd body (rel. or abs., see flag above) bool auto_distance,///< if true, initializes the imposed distance as the distance between mpos1 and mpos2 double mdistance ///< imposed distance (no need to define, if auto_distance=true.) ) { this->Body1 = mbody1.get_ptr(); this->Body2 = mbody2.get_ptr(); this->Cx.SetVariables(&this->Body1->Variables(),&this->Body2->Variables()); if (pos_are_relative) { this->pos1 = mpos1; this->pos2 = mpos2; } else { this->pos1 = this->Body1->Point_World2Body(&mpos1); this->pos2 = this->Body2->Point_World2Body(&mpos2); } ChVector<> AbsDist = Body1->Point_Body2World(&pos1)-Body2->Point_Body2World(&pos2); this->curr_dist = AbsDist.Length(); if (auto_distance) { this->distance = this->curr_dist; } else { this->distance = mdistance; } return true; }
double ChPathSteeringControllerXT::CalcHeadingError(ChVector<>& a, ChVector<>& b) { double ang = 0.0; // test for velocity > 0 m_vel.z() = 0; m_vel.Normalize(); double speed = m_vel.Length(); if(speed < 1) { // vehicle is standing still, we take the chassis orientation a.z() = 0; b.z() = 0; a.Normalize(); b.Normalize(); } else { // vehicle is running, we take the {x,y} velocity vector a = m_vel; b.z() = 0; b.Normalize(); } // it might happen to cruise against the path definition (end->begin instead of begin->end), // then the the tangent points backwards to driving direction // the distance |ab| is > 1 then ChVector<> ab = a - b; double ltest = ab.Length(); ChVector<> vpc; if(ltest < 1) { vpc = Vcross(a,b); } else { vpc = Vcross(a,-b); } ang = std::asin(vpc.z()); return ang; }
void ChLinkSpring::Initialize(std::shared_ptr<ChBody> mbody1, std::shared_ptr<ChBody> mbody2, bool pos_are_relative, ChVector<> mpos1, ChVector<> mpos2, bool auto_rest_length, double mrest_length) { // First, initialize as all constraint with markers. // In this case, create the two markers also!. ChLinkMarkers::Initialize(mbody1, mbody2, CSYSNORM); if (pos_are_relative) { marker1->Impose_Rel_Coord(ChCoordsys<>(mpos1, QUNIT)); marker2->Impose_Rel_Coord(ChCoordsys<>(mpos2, QUNIT)); } else { marker1->Impose_Abs_Coord(ChCoordsys<>(mpos1, QUNIT)); marker2->Impose_Abs_Coord(ChCoordsys<>(mpos2, QUNIT)); } ChVector<> AbsDist = marker1->GetAbsCoord().pos - marker2->GetAbsCoord().pos; dist = AbsDist.Length(); spr_restlength = auto_rest_length ? dist : mrest_length; }