C++ ChVector::Length方法代码示例

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;
}