/// 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);
}
/// Collsion algorithm based on a paper of J. Shane Sui and John A. Hirshey II:
/// "A New Analytical Tire Model for Vehicle Dynamic Analysis" presented at 2001 MSC User Meeting
public static bool DiscTerrainCollisionEnvelope(
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
ChFunction_Recorder areaDep, ///< [in] lookup table to calculate depth from intersection area
ref ChCoordsys contact, ///< [out] contact coordinate system (relative to the global frame)
ref double depth ///< [out] penetration depth (positive if contact occurred)
)
{
return(false);
}
/// Report the tire force and moment. /// This function can be used for reporting purposes or else to calculate tire /// forces in a co-simulation framework. public abstract ChSubsysDefs.TerrainForce ReportTireForce(ChTerrain terrain);
