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

// -----------------------------------------------------------------------------
// Calculate kinematics quantities (slip angle, longitudinal slip, camber angle,
// and toe-in angle using the current state of the associated wheel body.
// -----------------------------------------------------------------------------
void ChTire::CalculateKinematics(double time, const WheelState& state, const ChTerrain& terrain) {
    // Wheel normal (expressed in global frame)
    ChVector<> wheel_normal = state.rot.GetYaxis();

    // Terrain normal at wheel location (expressed in global frame)
    ChVector<> Z_dir = terrain.GetNormal(state.pos.x(), state.pos.y());

    // Longitudinal (heading) and lateral directions, in the terrain plane
    ChVector<> X_dir = Vcross(wheel_normal, Z_dir);
    X_dir.Normalize();
    ChVector<> Y_dir = Vcross(Z_dir, X_dir);

    // Tire reference coordinate system
    ChMatrix33<> rot;
    rot.Set_A_axis(X_dir, Y_dir, Z_dir);
    ChCoordsys<> tire_csys(state.pos, rot.Get_A_quaternion());

    // Express wheel linear velocity in tire frame
    ChVector<> V = tire_csys.TransformDirectionParentToLocal(state.lin_vel);
    // Express wheel normal in tire frame
    ChVector<> n = tire_csys.TransformDirectionParentToLocal(wheel_normal);

    // Slip angle
    double abs_Vx = std::abs(V.x());
    double zero_Vx = 1e-4;
    m_slip_angle = (abs_Vx > zero_Vx) ? std::atan(V.y() / abs_Vx) : 0;

    // Longitudinal slip
    m_longitudinal_slip = (abs_Vx > zero_Vx) ? -(V.x() - state.omega * GetRadius()) / abs_Vx : 0;

    // Camber angle
    m_camber_angle = std::atan2(n.z(), n.y());
}

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

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

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

// -----------------------------------------------------------------------------
// Utility function for characterizing the geometric contact between a disc with
// specified center location, normal direction, and radius and the terrain,
// assumed to be specified as a height field (over the x-y domain).
// This function returns false if no contact occurs. Otherwise, it sets the
// contact points on the disc (ptD) and on the terrain (ptT), the normal contact
// direction, and the resulting penetration depth (a positive value).
// -----------------------------------------------------------------------------
bool ChTire::disc_terrain_contact(const ChTerrain& terrain,
                                  const ChVector<>& disc_center,
                                  const ChVector<>& disc_normal,
                                  double disc_radius,
                                  ChCoordsys<>& contact,
                                  double& depth) {
    // Find terrain height below disc center. There is no contact if the disc
    // center is below the terrain or farther away by more than its radius.
    double hc = terrain.GetHeight(disc_center.x(), disc_center.y());
    if (disc_center.z() <= hc || disc_center.z() >= hc + disc_radius)
        return false;

    // Find the lowest point on the disc. There is no contact if the disc is
    // (almost) horizontal.
    ChVector<> dir1 = Vcross(disc_normal, ChVector<>(0, 0, 1));
    double sinTilt2 = dir1.Length2();

    if (sinTilt2 < 1e-3)
        return false;

    // Contact point (lowest point on disc).
    ChVector<> ptD = disc_center + disc_radius * Vcross(disc_normal, dir1 / sqrt(sinTilt2));

    // Find terrain height at lowest point. No contact if lowest point is above
    // the terrain.
    double hp = terrain.GetHeight(ptD.x(), ptD.y());

    if (ptD.z() > hp)
        return false;

    // Approximate the terrain with a plane. Define the projection of the lowest
    // point onto this plane as the contact point on the terrain.
    ChVector<> normal = terrain.GetNormal(ptD.x(), ptD.y());
    ChVector<> longitudinal = Vcross(disc_normal, normal);
    longitudinal.Normalize();
    ChVector<> lateral = Vcross(normal, longitudinal);
    ChMatrix33<> rot;
    rot.Set_A_axis(longitudinal, lateral, normal);

    contact.pos = ptD;
    contact.rot = rot.Get_A_quaternion();

    depth = Vdot(ChVector<>(0, 0, hp - ptD.z()), normal);
    assert(depth > 0);

    return true;
}

// 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 Q_to_AngAxis(const ChQuaternion<double>& quat, double& angle, ChVector<double>& axis) {
    if (fabs(quat.e0()) < 0.99999999) {
        double arg = acos(quat.e0());
        double invsine = 1 / sin(arg);
        ChVector<double> vtemp;
        vtemp.x() = invsine * quat.e1();
        vtemp.y() = invsine * quat.e2();
        vtemp.z() = invsine * quat.e3();
        angle = 2 * arg;
        axis = Vnorm(vtemp);
    } else {
        axis.x() = 1;
        axis.y() = 0;
        axis.z() = 0;
        angle = 0;
    }
}

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

 // output any relevant test rig data here
 void drawWheelOutput() {
     ChVector<> cm = mwheel->wheel->GetPos();
     char messageCM[100];
     sprintf(messageCM, "CM pos, x: %4.4g, y: %4.4g, z: %4.4g", cm.x(), cm.y(), cm.z());
     text_cmPos->setText(core::stringw(messageCM).c_str());
     // wheel CM vel
     ChVector<> cmVel = mwheel->wheel->GetPos_dt();
     char messageV[100];
     sprintf(messageV, "CM vel, x: %4.4g, y: %4.4g, z: %4.4g", cmVel.x(), cmVel.y(), cmVel.z());
     text_cmVel->setText(core::stringw(messageV).c_str());
     // rxn. forces on spindle
     ChVector<> rxnF = mtester->spindle->Get_react_force();
     char messageF[100];
     sprintf(messageF, "spindle Rxn. F, x: %4.3g, y: %4.3g, z: %4.3g", rxnF.x(), rxnF.y(), rxnF.z());
     text_spindleForces->setText(core::stringw(messageF).c_str());
     // rxn. torques on spindle
     ChVector<> rxnT = mtester->spindle->Get_react_torque();
     char messageT[100];
     sprintf(messageT, "spindle Rxn. T, x: %4.3g, y: %4.3g, z: %4.3g", rxnT.x(), rxnT.y(), rxnT.z());
     text_spindleTorque->setText(core::stringw(messageT).c_str());
 }

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

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

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

bool ChCascadeDoc::GetVolumeProperties(const TopoDS_Shape& mshape,   ///< pass the shape here
                                       const double density,         ///< pass the density here
                                       ChVector<>& center_position,  ///< get the position center, respect to shape pos.
                                       ChVector<>& inertiaXX,        ///< get the inertia diagonal terms
                                       ChVector<>& inertiaXY,        ///< get the inertia extradiagonal terms
                                       double& volume,               ///< get the volume
                                       double& mass                  ///< get the mass
                                       ) {
    if (mshape.IsNull())
        return false;

    GProp_GProps mprops;
    GProp_GProps vprops;
    BRepGProp::VolumeProperties(mshape, mprops);
    BRepGProp::VolumeProperties(mshape, vprops);

    mprops.Add(mprops, density);

    mass = mprops.Mass();
    volume = vprops.Mass();
    gp_Pnt G = mprops.CentreOfMass();
    gp_Mat I = mprops.MatrixOfInertia();

    center_position.x() = G.X();
    center_position.y() = G.Y();
    center_position.z() = G.Z();

    inertiaXX.x() = I(1, 1);
    inertiaXX.y() = I(2, 2);
    inertiaXX.z() = I(3, 3);
    inertiaXY.x() = I(1, 2);
    inertiaXY.y() = I(1, 3);
    inertiaXY.z() = I(2, 3);

    return true;
}