/// Perform disc-terrain collision detection.
/// This utility function checks for contact between a disc of specified
/// radius with given position and orientation (specified as the location of
/// its center and a unit vector normal to the disc plane) and the terrain
/// system associated with this tire. It returns true if the disc contacts the
/// terrain and false otherwise. If contact occurs, it returns a coordinate
/// system with the Z axis along the contact normal and the X axis along the
/// "rolling" direction, as well as a positive penetration depth (i.e. the
/// height below the terrain of the lowest point on the disc).
protected static bool disc_terrain_contact(
ChTerrain terrain, ///< [in] reference to terrain system
ChVector disc_center, ///< [in] global location of the disc center
ChVector disc_normal, ///< [in] disc normal, expressed in the global frame
double disc_radius, ///< [in] disc radius
ref ChCoordsys contact, ///< [out] contact coordinate system (relative to the global frame)
ref double depth ///< [out] penetration depth (positive if contact occurred)
)
{
// 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 nhelp = terrain.GetNormal(disc_center.x, disc_center.y);
ChVector dir1 = ChVector.Vcross(disc_normal, nhelp);
double sinTilt2 = dir1.Length2();
if (sinTilt2 < 1e-3)
{
return(false);
}
// Contact point (lowest point on disc).
ChVector ptD = disc_center + disc_radius * ChVector.Vcross(disc_normal, dir1 / Math.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 = ChVector.Vcross(disc_normal, normal);
longitudinal.Normalize();
ChVector lateral = ChVector.Vcross(normal, longitudinal);
ChMatrix33 <double> rot = new ChMatrix33 <double>(0); // Need to nest this.
rot.Set_A_axis(longitudinal, lateral, normal);
contact.pos = ptD;
contact.rot = rot.Get_A_quaternion();
depth = ChVector.Vdot(new ChVector(0, 0, hp - ptD.z), normal);
//assert(depth > 0);
return(true);
}
/// Calculate distance between a point p and a line identified
/// with segment dA,dB. Returns distance. Also, the mu value reference
/// tells if the nearest projection of point on line falls into segment (for mu 0...1)
/// \return the distance
public static double PointLineDistance(
ref ChVector p, //< point to be measured
ref ChVector dA, //< a point on the line
ref ChVector dB, //< another point on the line
ref double mu, //< parametric coord: if in 0..1 interval, projection is between dA and dB
ref bool is_insegment //< returns true if projected point is between dA and dB
)
{
mu = -1.0;
is_insegment = false;
double mdist = 10e34;
ChVector vseg = ChVector.Vsub(dB, dA);
ChVector vdir = ChVector.Vnorm(vseg);
ChVector vray = ChVector.Vsub(p, dA);
mdist = ChVector.Vlength(ChVector.Vcross(vray, vdir));
mu = ChVector.Vdot(vray, vdir) / ChVector.Vlength(vseg);
if ((mu >= 0) && (mu <= 1.0))
{
is_insegment = true;
}
return(mdist);
}
// compute triangle normal
public bool Normal(ref ChVector N)
{
ChVector u;
u = ChVector.Vsub(p2, p1);
ChVector v;
v = ChVector.Vsub(p3, p1);
ChVector n;
n = ChVector.Vcross(u, v);
double len = ChVector.Vlength(n);
if (Mathfx.Abs(len) > EPS_TRIDEGENERATE)
{
N = ChVector.Vmul(n, (1.0 / len));
}
else
{
return(false);
}
return(true);
}
/// Given point B and a generic triangle, computes the distance from the triangle plane,
/// returning also the projection of point on the plane and other infos
/// \return the signed distance
public static double PointTriangleDistance(ChVector B, //< point to be measured
ref ChVector A1, //< point of triangle
ref ChVector A2, //< point of triangle
ref ChVector A3, //< point of triangle
ref double mu, //< returns U parametric coord of projection
ref double mv, //< returns V parametric coord of projection
ref bool is_into, //< returns true if projection falls on the triangle
ref ChVector Bprojected //< returns the position of the projected point
)
{
// defaults
is_into = false;
mu = mv = -1;
double mdistance = 10e22;
ChVector Dx, Dy, Dz, T1, T1p;
Dx = ChVector.Vsub(A2, A1);
Dz = ChVector.Vsub(A3, A1);
Dy = ChVector.Vcross(Dz, Dx);
double dylen = ChVector.Vlength(Dy);
if (Mathfx.Abs(dylen) < EPS_TRIDEGENERATE) // degenerate triangle
{
return(mdistance);
}
Dy = ChVector.Vmul(Dy, 1.0 / dylen);
ChMatrix33 <double> mA = new ChMatrix33 <double>(0);
ChMatrix33 <double> mAi = new ChMatrix33 <double>(0);
mA.Set_A_axis(Dx, Dy, Dz);
// invert triangle coordinate matrix -if singular matrix, was degenerate triangle-.
if (Mathfx.Abs(mA.FastInvert(mAi)) < 0.000001)
{
return(mdistance);
}
T1 = mAi.Matr_x_Vect(ChVector.Vsub(B, A1));
T1p = T1;
T1p.y = 0;
mu = T1.x;
mv = T1.z;
if (mu >= 0 && mv >= 0 && mv <= 1.0 - mu)
{
is_into = true;
mdistance = Mathfx.Abs(T1.y);
Bprojected = ChVector.Vadd(A1, mA.Matr_x_Vect(T1p));
}
return(mdistance);
}
// return false if triangle has almost zero area
public bool IsDegenerated()
{
ChVector u = ChVector.Vsub(p2, p1);
ChVector v = ChVector.Vsub(p3, p1);
ChVector vcr = new ChVector();
vcr = ChVector.Vcross(u, v);
if (Mathfx.Abs(vcr.x) < EPS_TRIDEGENERATE && Math.Abs(vcr.y) < EPS_TRIDEGENERATE && Math.Abs(vcr.z) < EPS_TRIDEGENERATE)
{
return(true);
}
return(false);
}
/// Calculate kinematics quantities based on the current state of the associated
/// wheel body.
public void CalculateKinematics(double time, ///< [in] current time
ChSubsysDefs.WheelState state, ///< [in] current state of associated wheel body
RigidTerrain terrain ///< [in] reference to the terrain system
)
{
// 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 = ChVector.Vcross(wheel_normal, Z_dir);
X_dir.Normalize();
ChVector Y_dir = ChVector.Vcross(Z_dir, X_dir);
// Tire reference coordinate system
// ChMatrix33<double> rot = new ChMatrix33<double>(0); // Needs nesting
rot.Set_A_axis(X_dir, Y_dir, Z_dir);
ChCoordsys tire_csys = new ChCoordsys(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 = Mathfx.Abs(V.x);
double zero_Vx = 1e-4;
m_slip_angle = (abs_Vx > zero_Vx) ? Math.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 = Math.Atan2(n.z, n.y);
}
/// Perform disc-terrain collision detection considering the curvature of the road
/// surface. The surface normal is calculated based on 4 different height values below
/// the wheel center. The effective height is calculated as average value of the four
/// height values.
/// This utility function checks for contact between a disc of specified
/// radius with given position and orientation (specified as the location of
/// its center and a unit vector normal to the disc plane) and the terrain
/// system associated with this tire. It returns true if the disc contacts the
/// terrain and false otherwise. If contact occurs, it returns a coordinate
/// system with the Z axis along the contact normal and the X axis along the
/// "rolling" direction, as well as a positive penetration depth (i.e. the
/// height below the terrain of the lowest point on the disc).
public bool DiscTerrainCollision4pt(
RigidTerrain terrain, ///< [in] reference to terrain system
ChVector disc_center, ///< [in] global location of the disc center
ChVector disc_normal, ///< [in] disc normal, expressed in the global frame
double disc_radius, ///< [in] disc radius
double width, ///< [in] tire width
ref ChCoordsys contact, ///< [out] contact coordinate system (relative to the global frame)
ref double depth, ///< [out] penetration depth (positive if contact occurred)
ref double camber_angle ///< [out] tire camber angle
)
{
double dx = 0.1 * disc_radius;
double dy = 0.3 * width;
// 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.z);
if (disc_center.y <= hc || disc_center.y >= hc + disc_radius)
{
return(false);
}
// Find the lowest point on the disc. There is no contact if the disc is
// (almost) horizontal.
ChVector dir1 = ChVector.Vcross(disc_normal, new ChVector(0, 1, 0));
double sinTilt2 = dir1.Length2();
if (sinTilt2 < 1e-3)
{
return(false);
}
// Contact point (lowest point on disc).
ChVector ptD = disc_center + disc_radius * ChVector.Vcross(disc_normal, dir1 / Math.Sqrt(sinTilt2));
// 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.z);
ChVector longitudinal = ChVector.Vcross(disc_normal, normal);
longitudinal.Normalize();
ChVector lateral = ChVector.Vcross(normal, longitudinal);
// Calculate four contact points in the contact patch
ChVector ptQ1 = ptD + dx * longitudinal;
ptQ1.y = terrain.GetHeight(ptQ1.x, ptQ1.z);
ChVector ptQ2 = ptD - dx * longitudinal;
ptQ2.y = terrain.GetHeight(ptQ2.x, ptQ2.z);
ChVector ptQ3 = ptD + dy * lateral;
ptQ3.y = terrain.GetHeight(ptQ3.x, ptQ3.z);
ChVector ptQ4 = ptD - dy * lateral;
ptQ4.y = terrain.GetHeight(ptQ4.x, ptQ4.z);
// Calculate a smoothed road surface normal
ChVector rQ2Q1 = ptQ1 - ptQ2;
ChVector rQ4Q3 = ptQ3 - ptQ4;
ChVector terrain_normal = ChVector.Vcross(rQ2Q1, rQ4Q3);
terrain_normal.Normalize();
// Find terrain height as average of four points. No contact if lowest point is above
// the terrain.
ptD = 0.25 * (ptQ1 + ptQ2 + ptQ3 + ptQ4);
ChVector d = ptD - disc_center;
double da = d.Length();
if (da >= disc_radius)
{
return(false);
}
// Calculate an improved value for the camber angle
camber_angle = Math.Asin(ChVector.Vdot(disc_normal, terrain_normal));
// ChMatrix33<double> rot = new ChMatrix33<double>(0);
rot.Set_A_axis(longitudinal, lateral, terrain_normal);
contact.pos = ptD;
contact.rot = rot.Get_A_quaternion();
depth = disc_radius - da;
// assert(depth > 0);
return(true);
}