예제 #1
0
// -----------------------------------------------------------------------------
// 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());
}
예제 #2
0
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;
}
예제 #3
0
// -----------------------------------------------------------------------------
// 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;
}
예제 #4
0
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
    newmarkpos.pos = this->belt_up2;
    newmarkpos.rot = maU.Get_A_quaternion();
    marker2->Impose_Abs_Coord(newmarkpos);        //move marker2 into teeth position

	double phase_correction_up = m_delta*r1;
	double phase_correction_low = - phase_correction_up;
	double hU = Vlenght(belt_up2- belt_up1)  + phase_correction_up;
	double hL = Vlenght(belt_low2- belt_low1) + phase_correction_low;

        // imposed relative positions/speeds
    deltaC.pos = ChVector<>(-hU, 0, 0);
    deltaC_dt.pos = VNULL;
    deltaC_dtdt.pos = VNULL;

    deltaC.rot = QUNIT;             // no relative rotations imposed!
    deltaC_dt.rot = QNULL;
    deltaC_dtdt.rot = QNULL;
}
예제 #5
0
    static unsigned int BoxBoxContacts(
      Vector&  p_a, ChMatrix33<>& R_a, Vector const & ext_a,
      Vector&  p_b, ChMatrix33<>& R_b, Vector const & ext_b,
      double const & envelope,
      Vector * p,
	  Vector & n,
	  double * distances
      )
    {
      assert(p);
      assert(distances);

	  unsigned int cnt = 0;

      //--- Sign lookup table, could be precomputed!!!
      Vector sign[8];
      for(unsigned int mask=0;mask<8;++mask)
      {
        sign[mask](0) = (mask&0x0001)?1:-1;
        sign[mask](1) = ((mask>>1)&0x0001)?1:-1;
        sign[mask](2) = ((mask>>2)&0x0001)?1:-1;
      }
      //--- extract axis of boxes in WCS
      Vector A[3];
      A[0].x = R_a(0,0);   A[0].y = R_a(1,0);   A[0].z = R_a(2,0);
      A[1].x = R_a(0,1);   A[1].y = R_a(1,1);   A[1].z = R_a(2,1);
      A[2].x = R_a(0,2);   A[2].y = R_a(1,2);   A[2].z = R_a(2,2);
      Vector B[3];
      B[0].x = R_b(0,0);   B[0].y = R_b(1,0);   B[0].z = R_b(2,0);
      B[1].x = R_b(0,1);   B[1].y = R_b(1,1);   B[1].z = R_b(2,1);
      B[2].x = R_b(0,2);   B[2].y = R_b(1,2);   B[2].z = R_b(2,2);

      //--- To compat numerical round-offs, these tend to favor edge-edge
      //--- cases, when one really rather wants a face-case. Truncating
      //--- seems to let the algorithm pick face cases over edge-edge
      //--- cases.
      unsigned int i;

      for( i=0;i<3;++i)
        for(unsigned int j=0;j<3;++j)
        {
          if(fabs(A[i](j))<10e-7)
            A[i](j) = 0.;
          if(fabs(B[i](j))<10e-7)
            B[i](j) = 0.;
        }

      Vector a[8];
      Vector b[8];
      //--- corner points of boxes in WCS
      for( i=0;i<8;++i)
      {
        a[i] = A[2]*(sign[i](2)*ext_a(2)) + A[1]*(sign[i](1)*ext_a(1)) + A[0]*(sign[i](0)*ext_a(0)) + p_a;
        b[i] = B[2]*(sign[i](2)*ext_b(2)) + B[1]*(sign[i](1)*ext_b(1)) + B[0]*(sign[i](0)*ext_b(0)) + p_b;
	  }
      //--- Potential separating axes in WCS
      Vector axis[15];
      axis[0] = A[0];
      axis[1] = A[1];
      axis[2] = A[2];
      axis[3] = B[0];
      axis[4] = B[1];
      axis[5] = B[2];
      axis[6].Cross(A[0],B[0]);
      if(axis[6](0)==0 && axis[6](1)==0 && axis[6](2)==0)
        axis[6] = A[0];
      else
        axis[6] /= sqrt(axis[6].Dot(axis[6]));
      axis[7].Cross(A[0],B[1]);
      if(axis[7](0)==0 && axis[7](1)==0 && axis[7](2)==0)
        axis[7] = A[0];
      else
        axis[7] /= sqrt(axis[7].Dot(axis[7]));
      axis[8].Cross(A[0],B[2]);
      if(axis[8](0)==0 && axis[8](1)==0 && axis[8](2)==0)
        axis[8] = A[0];
      else
        axis[8] /= sqrt(axis[8].Dot(axis[8]));
      axis[9].Cross(A[1],B[0]);
      if(axis[9](0)==0 && axis[9](1)==0 && axis[9](2)==0)
        axis[9] = A[1];
      else
        axis[9] /= sqrt(axis[9].Dot(axis[9]));
      axis[10].Cross(A[1],B[1]);
      if(axis[10](0)==0 && axis[10](1)==0 && axis[10](2)==0)
        axis[10] = A[1];
      else
        axis[10] /= sqrt(axis[10].Dot(axis[10]));
      axis[11].Cross(A[1],B[2]);
      if(axis[11](0)==0 && axis[11](1)==0 && axis[11](2)==0)
        axis[11] = A[1];
      else
        axis[11] /= sqrt(axis[11].Dot(axis[11]));
      axis[12].Cross(A[2],B[0]);
      if(axis[12](0)==0 && axis[12](1)==0 && axis[12](2)==0)
        axis[12] = A[2];
      else
        axis[12] /= sqrt(axis[12].Dot(axis[12]));
      axis[13].Cross(A[2],B[1]);
      if(axis[13](0)==0 && axis[13](1)==0 && axis[13](2)==0)
        axis[13] = A[2];
      else
        axis[13] /= sqrt(axis[13].Dot(axis[13]));
      axis[14].Cross(A[2],B[2]);
      if(axis[14](0)==0 && axis[14](1)==0 && axis[14](2)==0)
        axis[14] = A[2];
      else
        axis[14] /= sqrt(axis[14].Dot(axis[14]));
      //--- project vertices of boxes onto separating axis
      double min_proj_a[15];
      double min_proj_b[15];
      double max_proj_a[15];
      double max_proj_b[15];
      for(i=0;i<15;++i)
      {
        min_proj_a[i] = min_proj_b[i] = 10e30;
        max_proj_a[i] = max_proj_b[i] = -10e30;
      }
      for(i=0;i<15;++i)
      {
        for(unsigned int j=0;j<8;++j)
        {
          double proj_a = a[j].Dot(axis[i]);
          double proj_b = b[j].Dot(axis[i]);
          min_proj_a[i] = ChMin(min_proj_a[i],proj_a);
          max_proj_a[i] = ChMax(max_proj_a[i],proj_a);
          min_proj_b[i] = ChMin(min_proj_b[i],proj_b);
          max_proj_b[i] = ChMax(max_proj_b[i],proj_b);
        }
        //--- test for valid separation axis if so return
        if (min_proj_a[i] > (max_proj_b[i]+envelope) ||   min_proj_b[i] > (max_proj_a[i]+envelope))
          return 0;
      }
      //--- Compute box overlaps along all 15 separating axes, and determine
      //--- minimum overlap
      double overlap[15];
      double minimum_overlap = -10e30;
      unsigned int minimum_axis = 15;
      bool flip_axis[15];
      //--- Notice that edge-edge cases are testet last, so face cases
      //--- are favored over edge-edge cases
      for(i=0;i<15;++i)
      {
        flip_axis[i] = false;
        overlap[i] = 10e30;
        if(max_proj_a[i] <= min_proj_b[i])
        {
          overlap[i] = ChMin( overlap[i], min_proj_b[i] - max_proj_a[i] );
          if(overlap[i]>minimum_overlap)
          {
            minimum_overlap = overlap[i];
            minimum_axis = i;
            flip_axis[i] = false;
          }
        }
        if(max_proj_b[i] <= min_proj_a[i])
        {
          overlap[i] = ChMin( overlap[i], min_proj_a[i] - max_proj_b[i] );
          if(overlap[i]>minimum_overlap)
          {
            minimum_overlap = overlap[i];
            minimum_axis = i;
            flip_axis[i] = true;
          }
        }
        if(min_proj_a[i] <= min_proj_b[i] &&  min_proj_b[i] <= max_proj_a[i])
        {
          overlap[i] = ChMin( overlap[i], -(max_proj_a[i] - min_proj_b[i]) );
          if(overlap[i]>minimum_overlap)
          {
            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,
        //--- so if we find a corner point of one box being inside
        //--- the other, we fall back to use the face case with
        //--- minimum overlap.
        if(corners_inside)//--- Actually we only need to test end-points of edge for inclusion (4 points instead of 16!!!).
        {
          minimum_overlap = -10e30;
          minimum_axis = 15;
          for(unsigned int i=0;i<6;++i)
          {
            if(overlap[i]>minimum_overlap)
            {
              minimum_overlap = overlap[i];
              minimum_axis = i;
            }
          }
        }
      }

      //--- now we can safely pick the contact normal, since we
      //--- know wheter we have a face-case or edge-edge case.
      n = axis[minimum_axis];

      //--- This is definitely an edge-edge case
      if(minimum_axis>=6)
      {
        //--- Find a point p_a on the edge from box A.
        for(unsigned int i=0;i<3;++i)
          if(n.Dot(A[i]) > 0.)
            p_a += ext_a(i)*A[i];
          else
            p_a -= ext_a(i)*A[i];
        //--- Find a point p_b on the edge from box B.
        for(int ci=0;ci<3;++ci)
          if(n.Dot(B[ci]) < 0.)
            p_b += ext_b(ci)*B[ci];
          else
            p_b -= ext_b(ci)*B[ci];

        //--- Determine the indices of two unit edge direction vectors (columns
        //--- of rotation matrices in WCS).
        int columnA = ((minimum_axis)-6)/3;
        int columnB = ((minimum_axis)-6)%3;
        double s,t;
        //--- Compute the edge-paramter values s and t corresponding to the closest
        //--- points between the two infinite lines parallel to the two edges.
        ClosestPointsBetweenLines()(p_a,A[columnA],p_b,B[columnB],s,t);
        //--- Use the edge parameter values to compute the closest
        //--- points between the two edges.
        p_a += A[columnA]*s;
        p_b += B[columnB]*t;
        //--- Let the contact point be given by the mean of the closest points.
        p[0] = (p_a + p_b)*.5;
        distances[0] = overlap[minimum_axis];
        return 1;
      }
      //--- This is a face-``something else'' case, we actually already have taken
      //--- care of all corner points, but there might be some edge-edge crossings
      //--- generating contact points



      //--- Make sure that we work in the frame of the box that defines the contact
      //--- normal. This coordinate frame is nice, because the contact-face is a axis
      //--- aligned rectangle. We will refer to this frame as the reference frame, and
      //--- use the letter 'r' or 'R' for it. The other box is named the incident box,
      //--- its closest face towards the reference face is called the incidient face, and
      //--- is denoted by the letter 'i' or 'I'.
      Vector * R_r,* R_i;  //--- Box direction vectors in WCS
      Vector ext_r,ext_i;          //--- Box extents
      Vector p_r,p_i;              //--- Box centers in WCS
      bool * incident_inside;    //--- corner inside state of incident box.
      if (minimum_axis  < 3)
      {
        //--- This means that box A is defining the reference frame
        R_r = A;
        R_i = B;
        p_r = p_a;
        p_i = p_b;
        ext_r = ext_a;
        ext_i = ext_b;
        incident_inside = BinA;
      }
      else
      {
        //--- This means that box B is defining the reference frame
        R_r = B;
        R_i = A;
        p_r = p_b;
        p_i = p_a;
        ext_r = ext_b;
        ext_i = ext_a;
        incident_inside = AinB;
      }
      //--- Following vectors are used for computing the corner points of the incident
      //--- face. At first they are used to determine the axis of the incidient box
      //--- pointing towards the reference box.
      //---
      //--- n_r_wcs = normal pointing away from reference frame in WCS coordinates.
      //--- n_r = normal vector of reference face dotted with axes of incident box.
      //--- abs_n_r = absolute values of n_r.
      Vector n_r_wcs,n_r,abs_n_r;
      if (minimum_axis < 3)
      {
        n_r_wcs = n;
      }
      else
      {
        n_r_wcs = -n;
      }

      //--- Each of these is a measure for how much the axis' of the incident box
      //--- points in the direction of n_r_wcs. The largest absolute value give
      //--- us the axis along which we will find the closest face towards the reference
      //--- box. The sign will tell us if we should take the positive or negative
      //--- face to get the closest incident face.
      n_r(0) = R_i[0].Dot(n_r_wcs);
      n_r(1) = R_i[1].Dot(n_r_wcs);
      n_r(2) = R_i[2].Dot(n_r_wcs);
      abs_n_r(0) = fabs (n_r(0));
      abs_n_r(1) = fabs (n_r(1));
      abs_n_r(2) = fabs (n_r(2));
      //--- Find the largest compontent of abs_n_r: This corresponds to the normal
      //--- for the indident face. The axis number is stored in a3. the other
      //--- axis numbers of the indicent face are stored in a1,a2.
      int a1,a2,a3;
      if (abs_n_r(1) > abs_n_r(0))
      {
        if (abs_n_r(1) > abs_n_r(2))
        {
          a1 = 2; a2 = 0; a3 = 1;
        }
        else
        {
          a1 = 0; a2 = 1; a3 = 2;
        }
      }
      else
      {
        if (abs_n_r(0) > abs_n_r(2))
        {
          a1 = 1; a2 = 2; a3 = 0;
        }
        else
        {
          a1 = 0; a2 = 1; a3 = 2;
        }
      }
      //--- Now we have information enough to determine the incidient face, that means we can
      //--- compute the center point of incident face in WCS coordinates.

      int plus_sign[3];
      Vector center_i_wcs;
      if (n_r(a3) < 0)
      {
        center_i_wcs = p_i + ext_i(a3) * R_i[a3];
        plus_sign[a3] = 1;
      }
      else
      {
        center_i_wcs = p_i - ext_i(a3) * R_i[a3];
        plus_sign[a3] = 0;
      }
      //--- Compute difference of center point of incident face with center of reference coordinates.
      Vector center_ir = center_i_wcs - p_r;
      //--- Find the normal and non-normal axis numbers of the reference box
      int code1,code2,code3;
      if (minimum_axis < 3)
        code3 = minimum_axis;  //012
      else
        code3 = minimum_axis-3;  //345
      if (code3==0)
      {
        code1 = 1;
        code2 = 2;
      }
      else if (code3==1)
      {
        code1 = 2;
        code2 = 0;
      }
      else
      {
        code1 = 0;
        code2 = 1;
      }
      //--- Find the four corners of the incident face, in reference-face coordinates
      double quad[8]; //--- 2D coordinate of incident face (stored as x,y pairs).
      bool inside[4];     //--- inside state of the four coners of the quad
      //--- Project center_ri onto reference-face coordinate system (has origo
      //--- at the center of the reference face, and the two orthogonal unit vectors
      //--- denoted by R_r[code1] and R_r[code2] spaning the face-plane).
      double c1 = R_r[code1].Dot( center_ir);
      double c2 = R_r[code2].Dot( center_ir);
      //--- Compute the projections of the axis spanning the incidient
      //--- face, onto the axis spanning the reference face.
      //---
      //--- This will allow us to determine the coordinates in the reference-face
      //--- when we step along a direction of the incident face given by either
      //--- a1 or a2.
      double m11 = R_r[code1].Dot( R_i[a1]);
      double m12 = R_r[code1].Dot( R_i[a2]);
      double m21 = R_r[code2].Dot( R_i[a1]);
      double m22 = R_r[code2].Dot( R_i[a2]);
      {
        double k1 = m11 * ext_i(a1);
        double k2 = m21 * ext_i(a1);
        double k3 = m12 * ext_i(a2);
        double k4 = m22 * ext_i(a2);

        plus_sign[a1] = 0;
        plus_sign[a2] = 0;
        unsigned int mask = ( (plus_sign[a1]<<a1) |  (plus_sign[a2]<<a2) |  (plus_sign[a3]<<a3));
        inside[0] = incident_inside[ mask ];

        quad[0] = c1 - k1 - k3;
        quad[1] = c2 - k2 - k4;

        plus_sign[a1] = 0;
        plus_sign[a2] = 1;
        mask = (plus_sign[a1]<<a1 |  plus_sign[a2]<<a2 |  plus_sign[a3]<<a3);
        inside[1] = incident_inside[ mask ];

        quad[2] = c1 - k1 + k3;
        quad[3] = c2 - k2 + k4;

        plus_sign[a1] = 1;
        plus_sign[a2] = 1;
        mask = (plus_sign[a1]<<a1 |  plus_sign[a2]<<a2 |  plus_sign[a3]<<a3);
        inside[2] = incident_inside[ mask ];

        quad[4] = c1 + k1 + k3;
        quad[5] = c2 + k2 + k4;

        plus_sign[a1] = 1;
        plus_sign[a2] = 0;
        mask = (plus_sign[a1]<<a1 |  plus_sign[a2]<<a2 |  plus_sign[a3]<<a3);
        inside[3] = incident_inside[ mask ];

        quad[6] = c1 + k1 - k3;
        quad[7] = c2 + k2 - k4;
      }
      //--- find the size of the reference face
      double rect[2];
      rect[0] = ext_r(code1);
      rect[1] = ext_r(code2);

      //--- Intersect the edges of the incident and the reference face
      double crossings[16];
      unsigned int edge_crossings = RectQuadEdgeIntersectionTest()(envelope,rect,quad,inside,crossings);
      assert(edge_crossings<=8);

      if(!corners_inside && !edge_crossings)
        return 0;

      //--- Convert the intersection points into reference-face coordinates,
      //--- and compute the contact position and depth for each point.
      double det1 = 1./(m11*m22 - m12*m21);
      m11 *= det1;
      m12 *= det1;
      m21 *= det1;
      m22 *= det1;

      for (unsigned int j=0; j < edge_crossings; ++j)
      {
        //--- Get coordinates of edge-edge crossing point in reference face coordinate system.
        double p0 = crossings[j*2] - c1;
        double p1 = crossings[j*2+1] - c2;
        //--- Compute intersection point in (almost) WCS. Actually we have
        //--- displaced origin to center of reference frame box
        double k1 =  m22*p0 - m12*p1;
        double k2 = -m21*p0 + m11*p1;
        Vector point = center_ir + k1*R_i[a1] + k2*R_i[a2];
        //--- Depth of intersection point
        double depth = n_r_wcs.Dot(point) - ext_r(code3);
        if(depth<envelope)
        {
          p[cnt] = point + p_r;//--- Move origin from center of reference frame box to WCS
          distances[cnt] = depth;
          ++cnt;
        }
      }
      //      assert((corners_inside + cnt)<=8);//--- If not we are in serious trouble!!!
      //--- I think there is a special case, if corners_inside = 8 and
      //--- corners_in_A = 4 and corners_in_B = 4, then there really
      //--- can only be 4 contacts???

      if(corners_inside)
      {
        unsigned int start_corner_A = cnt;
        unsigned int end_corner_A = cnt;

        //--- Compute Displacement of contact plane from origin of WCS, the
        //--- contact plane is equal to the face plane of the reference box
        double w =   ext_r(code3) +  n_r_wcs.Dot(p_r);

        if(corners_A_in_B)
        {
          for (unsigned int i=0; i < 8; ++i)
          {
            if(AinB[i])
            {
              Vector point = a[i];
              double depth = n_r_wcs.Dot(point) - w;
              if(depth<envelope)
              {
                p[cnt] = point;
                distances[cnt] = depth;
                ++cnt;
              }
            }
          }
          end_corner_A = cnt;
        }
        if(corners_B_in_A)
        {
          for (unsigned int i=0; i < 8; ++i)
          {
            if(BinA[i])
            {
              Vector point = b[i];
              bool redundant = false;
              for(unsigned int j=start_corner_A;j<end_corner_A;++j)
              {
                if( p[j].Equals(point,envelope) )
                {
                  redundant = true;
                  break;
                }
              }
              if(redundant)
                continue;
              double depth = n_r_wcs.Dot(point) - w;
              if(depth<envelope)
              {
                p[cnt] = point;
                distances[cnt] = depth;
                ++cnt;
              }
            }
          }
        }
      }
      //      assert(cnt<=8);//--- If not we are in serious trouble!!!
      return cnt;
    };
예제 #6
0
// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
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);

    m_universal = std::make_shared<ChLinkUniversal>();
    m_universal->SetNameString(m_name + "_universal");
    m_universal->Initialize(m_arm, m_link, ChFrame<>(points[UNIV], rot.Get_A_quaternion()));
    chassis->GetSystem()->AddLink(m_universal);

    // Create and initialize the revolute-spherical joint (massless idler arm).
    // The length of the idler arm is the distance between the two hardpoints.
    // The z direction of the revolute joint orientation matrix is
    // dirs[REVSPH_AXIS], assumed to be a unit vector.
    double distance = (points[REVSPH_S] - points[REVSPH_R]).Length();

    u = points[REVSPH_S] - points[REVSPH_R];
    v = Vcross(dirs[REVSPH_AXIS], u);
    v.Normalize();
    u = Vcross(v, dirs[REVSPH_AXIS]);
    rot.Set_A_axis(u, v, dirs[REVSPH_AXIS]);

    m_revsph = std::make_shared<ChLinkRevoluteSpherical>();
    m_revsph->SetNameString(m_name + "_revsph");
    m_revsph->Initialize(chassis, m_link, ChCoordsys<>(points[REVSPH_R], rot.Get_A_quaternion()), distance);
    chassis->GetSystem()->AddLink(m_revsph);
}