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