Exemplo n.º 1
0
            /// 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);
            }
Exemplo n.º 2
0
            /// 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);
            }