C++ ChVector类代码示例

// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
const ChVector<> M113a_Suspension::GetLocation(PointId which) {
    ChVector<> point;

    switch (which) {
        case ARM:
            point = ChVector<>(0.213, -0.12, 0.199);
            break;
        case ARM_WHEEL:
            point = ChVector<>(0, -0.12, 0);
            break;
        case ARM_CHASSIS:
            point = ChVector<>(0.232, -0.12, 0.217);
            break;
        case SHOCK_A:
            point = ChVector<>(0, -0.12, 0);
            break;
        case SHOCK_C:
            point = ChVector<>(0, -0.12, 1);
            break;
        default:
            point = ChVector<>(0, 0, 0);
            break;
    }

    if (m_side == RIGHT)
        point.y() *= -1;

    return point;
}

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 X axis of a coordsystem, given the quaternion which
// represents the alignment of the coordsystem.
ChVector<double> VaxisXfromQuat(const ChQuaternion<double>& quat) {
    ChVector<double> res;
    res.x() = (pow(quat.e0(), 2) + pow(quat.e1(), 2)) * 2 - 1;
    res.y() = ((quat.e1() * quat.e2()) + (quat.e0() * quat.e3())) * 2;
    res.z() = ((quat.e1() * quat.e3()) - (quat.e0() * quat.e2())) * 2;
    return res;
}

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..)
}

// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
const ChVector<> M113_Idler::GetLocation(PointId which) {
    ChVector<> point;

    switch (which) {
        case WHEEL:
            point = ChVector<>(0, 0, 0);
            break;
        case CARRIER:
            point = ChVector<>(0, -0.1, 0);
            break;
        case CARRIER_CHASSIS:
            point = ChVector<>(0, -0.2, 0);
            break;
        case TSDA_CARRIER:
            point = ChVector<>(0, -0.2, 0);
            break;
        case TSDA_CHASSIS:
            point = ChVector<>(0.5, -0.2, 0);
            break;
        default:
            point = ChVector<>(0, 0, 0);
            break;
    }

    if (GetVehicleSide() == RIGHT)
        point.y() *= -1;

    return point;
}

// Given the imaginary (vectorial) {e1 e2 e3} part of a quaternion time derivative,
// find the entire quaternion q = {e0, e1, e2, e3}.
// Note: singularities are possible.
ChQuaternion<double> ImmQ_dt_complete(const ChQuaternion<double>& mq, const ChVector<double>& qimm_dt) {
    ChQuaternion<double> mqdt;
    mqdt.e1() = qimm_dt.x();
    mqdt.e2() = qimm_dt.y();
    mqdt.e3() = qimm_dt.z();
    mqdt.e0() = (-mq.e1() * mqdt.e1() - mq.e2() * mqdt.e2() - mq.e3() * mqdt.e3()) / mq.e0();
    return mqdt;
}

// Given the imaginary (vectorial) {e1 e2 e3} part of a quaternion,
// find the entire quaternion q = {e0, e1, e2, e3}.
// Note: singularities are possible.
ChQuaternion<double> ImmQ_complete(const ChVector<double>& qimm) {
    ChQuaternion<double> mq;
    mq.e1() = qimm.x();
    mq.e2() = qimm.y();
    mq.e3() = qimm.z();
    mq.e0() = sqrt(1 - mq.e1() * mq.e1() - mq.e2() * mq.e2() - mq.e3() * mq.e3());
    return mq;
}

void ChModelBullet::GetAABB(ChVector<>& bbmin, ChVector<>& bbmax) const
{
	btVector3 btmin;
	btVector3 btmax;
	if (bt_collision_object->getCollisionShape())
		bt_collision_object->getCollisionShape()->getAabb( bt_collision_object->getWorldTransform(), btmin, btmax);
	bbmin.Set(btmin.x(), btmin.y(), btmin.z());
	bbmax.Set(btmax.x(), btmax.y(), btmax.z());
}

void AddWall(std::shared_ptr<ChBody> body, const ChVector<>& dim, const ChVector<>& loc) {
    body->GetCollisionModel()->AddBox(dim.x(), dim.y(), dim.z(), loc);

    auto box = std::make_shared<ChBoxShape>();
    box->GetBoxGeometry().Size = dim;
    box->GetBoxGeometry().Pos = loc;
    box->SetColor(ChColor(1, 0, 0));
    box->SetFading(0.6f);
    body->AddAsset(box);
}

// Get the quaternion time derivative from the vector of angular speed, with w specified in _local_ coords.
ChQuaternion<double> Qdt_from_Wrel(const ChVector<double>& w, const ChQuaternion<double>& q) {
    ChQuaternion<double> qw;
    double half = 0.5;

    qw.e0() = 0;
    qw.e1() = w.x();
    qw.e2() = w.y();
    qw.e3() = w.z();

    return Qscale(Qcross(q, qw), half);  // {q_dt} = 1/2 {q}*{0,w_rel}
}

void WrdStyleSheet::GetStyleList(ChUINT2 in_unParagraphStyleIndex, ChVector<ChUINT2>& vecStyles) const
{
	ChUINT2 unStyleIndex = in_unParagraphStyleIndex;
	while (unStyleIndex != 0x0FFF)
    {
		vecStyles.push_back(unStyleIndex);
        const WrdStyle& style = getStyle(unStyleIndex);
        unStyleIndex = style.getBaseStyleIndex();
    }
	if (vecStyles.size() == 0 || vecStyles[vecStyles.size() - 1] != 0)
		vecStyles.push_back(0);
}

// Get the quaternion from an angle of rotation and an axis, defined in _abs_ coords.
// The axis is supposed to be fixed, i.e. it is constant during rotation.
// The 'axis' vector must be normalized.
ChQuaternion<double> Q_from_AngAxis(double angle, const ChVector<double>& axis) {
    ChQuaternion<double> quat;
    double halfang;
    double sinhalf;

    halfang = (angle * 0.5);
    sinhalf = sin(halfang);

    quat.e0() = cos(halfang);
    quat.e1() = axis.x() * sinhalf;
    quat.e2() = axis.y() * sinhalf;
    quat.e3() = axis.z() * sinhalf;
    return (quat);
}

ChVector<double> Q_to_NasaAngles(const ChQuaternion<double>& q1) {
    ChVector<double> mnasa;
    double sqw = q1.e0() * q1.e0();
    double sqx = q1.e1() * q1.e1();
    double sqy = q1.e2() * q1.e2();
    double sqz = q1.e3() * q1.e3();
    // heading
    mnasa.z() = atan2(2.0 * (q1.e1() * q1.e2() + q1.e3() * q1.e0()), (sqx - sqy - sqz + sqw));
    // bank
    mnasa.y() = atan2(2.0 * (q1.e2() * q1.e3() + q1.e1() * q1.e0()), (-sqx - sqy + sqz + sqw));
    // attitude
    mnasa.x() = asin(-2.0 * (q1.e1() * q1.e3() - q1.e2() * q1.e0()));
    return mnasa;
}

ChQuaternion<double> Q_from_NasaAngles(const ChVector<double>& mang) {
    ChQuaternion<double> mq;
    double c1 = cos(mang.z() / 2);
    double s1 = sin(mang.z() / 2);
    double c2 = cos(mang.x() / 2);
    double s2 = sin(mang.x() / 2);
    double c3 = cos(mang.y() / 2);
    double s3 = sin(mang.y() / 2);
    double c1c2 = c1 * c2;
    double s1s2 = s1 * s2;
    mq.e0() = c1c2 * c3 + s1s2 * s3;
    mq.e1() = c1c2 * s3 - s1s2 * c3;
    mq.e2() = c1 * s2 * c3 + s1 * c2 * s3;
    mq.e3() = s1 * c2 * c3 - c1 * s2 * s3;
    return mq;
}

void ChMatterSPH::FillBox(const ChVector<> size,
                          const double spacing,
                          const double initial_density,
                          const ChCoordsys<> boxcoords,
                          const bool do_centeredcube,
                          const double kernel_sfactor,
                          const double randomness) {
    int samples_x = (int)(size.x() / spacing);
    int samples_y = (int)(size.y() / spacing);
    int samples_z = (int)(size.z() / spacing);
    int totsamples = 0;

    double mrandomness = randomness;
    if (do_centeredcube)
        mrandomness = randomness * 0.5;

    for (int ix = 0; ix < samples_x; ix++)
        for (int iy = 0; iy < samples_y; iy++)
            for (int iz = 0; iz < samples_z; iz++) {
                ChVector<> pos(ix * spacing - 0.5 * size.x(), iy * spacing - 0.5 * size.y(),
                               iz * spacing - 0.5 * size.z());
                pos += ChVector<>(mrandomness * ChRandom() * spacing, mrandomness * ChRandom() * spacing,
                                  mrandomness * ChRandom() * spacing);
                AddNode(boxcoords.TransformLocalToParent(pos));
                totsamples++;

                if (do_centeredcube) {
                    ChVector<> pos2 = pos + 0.5 * ChVector<>(spacing, spacing, spacing);
                    pos2 += ChVector<>(mrandomness * ChRandom() * spacing, mrandomness * ChRandom() * spacing,
                                       mrandomness * ChRandom() * spacing);
                    AddNode(boxcoords.TransformLocalToParent(pos2));
                    totsamples++;
                }
            }

    double mtotvol = size.x() * size.y() * size.z();
    double mtotmass = mtotvol * initial_density;
    double nodemass = mtotmass / (double)totsamples;
    double kernelrad = kernel_sfactor * spacing;

    for (unsigned int ip = 0; ip < GetNnodes(); ip++) {
        // downcasting
        std::shared_ptr<ChNodeSPH> mnode(nodes[ip]);
        assert(mnode);

        mnode->SetKernelRadius(kernelrad);
        mnode->SetCollisionRadius(spacing * 0.05);
        mnode->SetMass(nodemass);
    }

    GetMaterial().Set_density(initial_density);
}