示例#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);
            }
示例#2
0
            /// 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);
            }