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