C++ ChMatrix33::Get_A_quaternion方法代码示例

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

ChQuaternion<double> Angle_to_Quat(AngleSet angset, const ChVector<double>& mangles) {
    ChQuaternion<double> res;
    ChMatrix33<> Acoord;

    switch (angset) {
        case AngleSet::EULERO:
            Acoord.Set_A_Eulero(mangles);
            break;
        case AngleSet::CARDANO:
            Acoord.Set_A_Cardano(mangles);
            break;
        case AngleSet::HPB:
            Acoord.Set_A_Hpb(mangles);
            break;
        case AngleSet::RXYZ:
            Acoord.Set_A_Rxyz(mangles);
            break;
        case AngleSet::RODRIGUEZ:
            Acoord.Set_A_Rodriguez(mangles);
            break;
        default:
            break;
    }
    res = Acoord.Get_A_quaternion();
    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;
}

void ChLinkPulley::UpdateTime (double mytime)
{
    // First, inherit to parent class
    ChLinkLock::UpdateTime(mytime);

	ChFrame<double> abs_shaft1;
	ChFrame<double> abs_shaft2;

	((ChFrame<double>*)Body1)->TrasformLocalToParent(local_shaft1, abs_shaft1);
	((ChFrame<double>*)Body2)->TrasformLocalToParent(local_shaft2, abs_shaft2);

	ChVector<> dcc_w = Vsub(Get_shaft_pos2(),
                            Get_shaft_pos1());

		// compute actual rotation of the two wheels (relative to truss).
    Vector md1 = abs_shaft1.GetA()->MatrT_x_Vect(dcc_w);
    md1.z = 0;  md1 = Vnorm (md1);
    Vector md2 = abs_shaft2.GetA()->MatrT_x_Vect(dcc_w);
    md2.z = 0;  md2 = Vnorm (md2);

	double periodic_a1 = ChAtan2(md1.x, md1.y);
	double periodic_a2 = ChAtan2(md2.x, md2.y);
	double old_a1 = a1; 
	double old_a2 = a2;
	double turns_a1 = floor (old_a1 / CH_C_2PI);
	double turns_a2 = floor (old_a2 / CH_C_2PI);
	double a1U = turns_a1 * CH_C_2PI + periodic_a1 + CH_C_2PI;
	double a1M = turns_a1 * CH_C_2PI + periodic_a1;
	double a1L = turns_a1 * CH_C_2PI + periodic_a1 - CH_C_2PI;
	a1 = a1M;
	if (fabs(a1U - old_a1) < fabs(a1M - old_a1))
		a1 = a1U;
	if (fabs(a1L - a1) < fabs(a1M - a1))
		a1 = a1L;
	double a2U = turns_a2 * CH_C_2PI + periodic_a2 + CH_C_2PI;
	double a2M = turns_a2 * CH_C_2PI + periodic_a2;
	double a2L = turns_a2 * CH_C_2PI + periodic_a2 - CH_C_2PI;
	a2 = a2M;
	if (fabs(a2U - old_a2) < fabs(a2M - old_a2))
		a2 = a2U;
	if (fabs(a2L - a2) < fabs(a2M - a2))
		a2 = a2L;

	     // correct marker positions if phasing is not correct
	double m_delta =0;
    if (this->checkphase)
    {
		double realtau = tau; 
		//if (this->epicyclic) 
		//	realtau = -tau;
        
        m_delta = a1 - phase - (a2/realtau);

        if (m_delta> CH_C_PI) m_delta -= (CH_C_2PI);		 // range -180..+180 is better than 0...360
        if (m_delta> (CH_C_PI/4.0)) m_delta = (CH_C_PI/4.0); // phase correction only in +/- 45°
        if (m_delta<-(CH_C_PI/4.0)) m_delta =-(CH_C_PI/4.0);
		//***TODO***
    }


    // Move markers 1 and 2 to align them as pulley ends

	ChVector<> d21_w = dcc_w - Get_shaft_dir1()* Vdot (Get_shaft_dir1(), dcc_w);
	ChVector<> D21_w = Vnorm(d21_w);

	this->shaft_dist = d21_w.Length();
	
	ChVector<> U1_w = Vcross(Get_shaft_dir1(), D21_w);

	double gamma1 = acos( (r1-r2) / shaft_dist);

	ChVector<> Ru_w =  D21_w*cos(gamma1) + U1_w*sin(gamma1);
	ChVector<> Rl_w =  D21_w*cos(gamma1) - U1_w*sin(gamma1);

	this->belt_up1  = Get_shaft_pos1()+ Ru_w*r1;
	this->belt_low1 = Get_shaft_pos1()+ Rl_w*r1;
	this->belt_up2  = Get_shaft_pos1()+ d21_w + Ru_w*r2;
	this->belt_low2 = Get_shaft_pos1()+ d21_w + Rl_w*r2;

		// marker alignment
	ChMatrix33<> maU;
	ChMatrix33<> maL;

	ChVector<> Dxu = Vnorm(belt_up2 - belt_up1);
	ChVector<> Dyu = Ru_w;
	ChVector<> Dzu = Vnorm (Vcross(Dxu, Dyu));
	Dyu = Vnorm (Vcross(Dzu, Dxu));
	maU.Set_A_axis(Dxu,Dyu,Dzu);

            // ! Require that the BDF routine of marker won't handle speed and acc.calculus of the moved marker 2!
    marker2->SetMotionType(ChMarker::M_MOTION_EXTERNAL);
    marker1->SetMotionType(ChMarker::M_MOTION_EXTERNAL);

	ChCoordsys<> newmarkpos;

        // move marker1 in proper positions
    newmarkpos.pos = this->belt_up1;
    newmarkpos.rot = maU.Get_A_quaternion();
    marker1->Impose_Abs_Coord(newmarkpos);        //move marker1 into teeth position
        // move marker2 in proper positions
//.........这里部分代码省略.........

//.........这里部分代码省略.........
          {
            minimum_overlap = overlap[i];
            minimum_axis = i;
            flip_axis[i] = false;
          }
        }
        if(min_proj_b[i] <= min_proj_a[i] &&  min_proj_a[i] <= max_proj_b[i])
        {
          overlap[i] = ChMin(overlap[i], -(max_proj_b[i] - min_proj_a[i]) );
          if(overlap[i]>minimum_overlap)
          {
            minimum_overlap = overlap[i];
            minimum_axis = i;
            flip_axis[i] = true;
          }
        }
      }
      if(minimum_overlap>envelope)
        return 0;
      //--- Take care of normals, so they point in the correct direction.
      for(i=0;i<15;++i)
      {
        if(flip_axis[i])
          axis[i] = - axis[i];
      }
      //--- At this point we know that a projection along axis[minimum_axis] with
      //--- value minimum_overlap will lead to non-penetration of the two boxes. We
      //--- just need to generate the contact points!!!
      unsigned int corners_inside = 0;
      unsigned int corners_B_in_A = 0;
      unsigned int corners_A_in_B = 0;
      bool AinB[8];
      bool BinA[8];
      Coordsys WCStoA(p_a, R_a.Get_A_quaternion());
	  Coordsys WCStoB(p_b, R_b.Get_A_quaternion());
      Vector eps_a = ext_a + Vector(envelope,envelope,envelope);
      Vector eps_b = ext_b + Vector(envelope,envelope,envelope);
      for(i=0;i<8;++i)
      {
		Vector a_in_B = WCStoB.TransformParentToLocal(a[i]);//ChTransform<>::TransformParentToLocal(a[i], p_a, R_a);
		// = WCStoB.TransformParentToLocal(a[i]);
        Vector abs_a(fabs(a_in_B.x),fabs(a_in_B.y),fabs(a_in_B.z) ) ;
        if(abs_a <= eps_b)
        {
          ++corners_inside;
          ++corners_A_in_B;
          AinB[i] = true;
        }
        else
          AinB[i] = false;
        Vector b_in_A = WCStoA.TransformParentToLocal(b[i]);//= ChTransform<>::TransformParentToLocal(b[i], p_b, R_b);
		// = WCStoA.TransformParentToLocal(b[i]);
        Vector abs_b(fabs(b_in_A.x),fabs(b_in_A.y),fabs(b_in_A.z) );
        if(abs_b <= eps_a)
        {
          ++corners_inside;
          ++corners_B_in_A;
          BinA[i] = true;
        }
        else
          BinA[i] = false;
      }
      //--- This may indicate an edge-edge case
      if(minimum_axis >= 6)
      {
        //--- However the edge-edge case may not be the best choice,

// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
void ChPitmanArm::Initialize(std::shared_ptr<ChBodyAuxRef> chassis,
                             const ChVector<>& location,
                             const ChQuaternion<>& rotation) {
    m_position = ChCoordsys<>(location, rotation);

    // Chassis orientation (expressed in absolute frame)
    // Recall that the suspension reference frame is aligned with the chassis.
    ChQuaternion<> chassisRot = chassis->GetFrame_REF_to_abs().GetRot();

    // Express the steering reference frame in the absolute coordinate system.
    ChFrame<> steering_to_abs(location, rotation);
    steering_to_abs.ConcatenatePreTransformation(chassis->GetFrame_REF_to_abs());

    // Transform all points and directions to absolute frame.
    std::vector<ChVector<>> points(NUM_POINTS);
    std::vector<ChVector<>> dirs(NUM_DIRS);

    for (int i = 0; i < NUM_POINTS; i++) {
        ChVector<> rel_pos = getLocation(static_cast<PointId>(i));
        points[i] = steering_to_abs.TransformPointLocalToParent(rel_pos);
    }

    for (int i = 0; i < NUM_DIRS; i++) {
        ChVector<> rel_dir = getDirection(static_cast<DirectionId>(i));
        dirs[i] = steering_to_abs.TransformDirectionLocalToParent(rel_dir);
    }

    // Unit vectors for orientation matrices.
    ChVector<> u;
    ChVector<> v;
    ChVector<> w;
    ChMatrix33<> rot;

    // Create and initialize the steering link body
    m_link = std::shared_ptr<ChBody>(chassis->GetSystem()->NewBody());
    m_link->SetNameString(m_name + "_link");
    m_link->SetPos(points[STEERINGLINK]);
    m_link->SetRot(steering_to_abs.GetRot());
    m_link->SetMass(getSteeringLinkMass());
    if (m_vehicle_frame_inertia) {
        ChMatrix33<> inertia = TransformInertiaMatrix(getSteeringLinkInertiaMoments(), getSteeringLinkInertiaProducts(),
                                                      chassisRot, steering_to_abs.GetRot());
        m_link->SetInertia(inertia);
    } else {
        m_link->SetInertiaXX(getSteeringLinkInertiaMoments());
        m_link->SetInertiaXY(getSteeringLinkInertiaProducts());
    }
    chassis->GetSystem()->AddBody(m_link);

    m_pP = m_link->TransformPointParentToLocal(points[UNIV]);
    m_pI = m_link->TransformPointParentToLocal(points[REVSPH_S]);
    m_pTP = m_link->TransformPointParentToLocal(points[TIEROD_PA]);
    m_pTI = m_link->TransformPointParentToLocal(points[TIEROD_IA]);

    // Create and initialize the Pitman arm body
    m_arm = std::shared_ptr<ChBody>(chassis->GetSystem()->NewBody());
    m_arm->SetNameString(m_name + "_arm");
    m_arm->SetPos(points[PITMANARM]);
    m_arm->SetRot(steering_to_abs.GetRot());
    m_arm->SetMass(getPitmanArmMass());
    if (m_vehicle_frame_inertia) {
        ChMatrix33<> inertia = TransformInertiaMatrix(getPitmanArmInertiaMoments(), getPitmanArmInertiaProducts(),
                                                      chassisRot, steering_to_abs.GetRot());
        m_arm->SetInertia(inertia);
    } else {
        m_arm->SetInertiaXX(getPitmanArmInertiaMoments());
        m_arm->SetInertiaXY(getPitmanArmInertiaProducts());
    }
    chassis->GetSystem()->AddBody(m_arm);

    // Cache points for arm visualization (expressed in the arm frame)
    m_pC = m_arm->TransformPointParentToLocal(points[REV]);
    m_pL = m_arm->TransformPointParentToLocal(points[UNIV]);

    // Create and initialize the revolute joint between chassis and Pitman arm.
    // Note that this is modeled as a ChLinkEngine to allow driving it with
    // imposed rotation (steering input).
    // The z direction of the joint orientation matrix is dirs[REV_AXIS], assumed
    // to be a unit vector.
    u = points[PITMANARM] - points[REV];
    v = Vcross(dirs[REV_AXIS], u);
    v.Normalize();
    u = Vcross(v, dirs[REV_AXIS]);
    rot.Set_A_axis(u, v, dirs[REV_AXIS]);

    m_revolute = std::make_shared<ChLinkMotorRotationAngle>();
    m_revolute->SetNameString(m_name + "_revolute");
    m_revolute->Initialize(chassis, m_arm, ChFrame<>(points[REV], rot.Get_A_quaternion()));
    auto motor_fun = std::make_shared<ChFunction_Setpoint>();
    m_revolute->SetAngleFunction(motor_fun);
    chassis->GetSystem()->AddLink(m_revolute);

    // Create and initialize the universal joint between the Pitman arm and steering link.
    // The x and y directions of the joint orientation matrix are given by
    // dirs[UNIV_AXIS_ARM] and dirs[UNIV_AXIS_LINK], assumed to be unit vectors
    // and orthogonal.
    w = Vcross(dirs[UNIV_AXIS_ARM], dirs[UNIV_AXIS_LINK]);
    rot.Set_A_axis(dirs[UNIV_AXIS_ARM], dirs[UNIV_AXIS_LINK], w);
//.........这里部分代码省略.........