Collision between a sphere and a mesh(height-map)

Question

I am trying to implement a method that detects the collision between a sphere and a mesh and reflects the vector of speed. I have a method, that detects a collision between a sphere and a triangle and returns the time to this collision using the speed of the sphere.

// Detects whether the triangle contains the point.
// v1, v2, v3 - triangle; n - normal; p - point
template < class T >
bool TrisContainsPoint( const Vertex3D< T > &v1, 
                        const Vertex3D< T > &v2, 
                        const Vertex3D< T > &v3,
                        const Vertex3D< T > &n,
                        const Vertex3D< T > &p )
{
  if( n.MixedProduct( v2 - v1, p - v1 ) <= 0 ) 
    return false; 
  if( n.MixedProduct( v3 - v2, p - v2 ) <= 0 ) 
    return false; 
  if( n.MixedProduct( v1 - v3, p - v3 ) <= 0 ) 
    return false; 
  return true;
};

// Returns the nearist point to p from a-b segment.
template < class T >
Vertex3D< T > ClosestPointToSegment( const Vertex3D< T > &p, const Vertex3D< T > &a, const Vertex3D< T > &b )
{
  Vertex3D< T > c = p - a;
  Vertex3D< T > w = b - a;
  T d = w.Length();
  if ( fabs( d ) < Plane3D< T >::EPS )
    return a;
  w /= d;
  const Vertex3D< T > v = w;
  T t = v & c;
  if ( t < 0.0f )
    return a;
  else if ( t > d )
    return b;
  return a + t * v;
};


// Finds the nearist point to p from the sides of the triangle. Return the smallest distance.
template < class T >
T ClosestPointToTris( const Vertex3D< T > &v1, const Vertex3D< T > &v2, 
                      const Vertex3D< T > &v3, const Vertex3D< T > &p, 
                      Vertex3D< T > *res )
{
  *res = ClosestPointToSegment( p, v1, v2 );
  T bestDist = p.DistanceToSqr( *res );

  Vertex3D< T > pt = ClosestPointToSegment( p, v1, v3 );
  T dist = p.DistanceToSqr( pt );
  if ( dist < bestDist )
  {
    bestDist = dist;
    *res = pt;
  };
  pt = ClosestPointToSegment( p, v2, v3 );
  dist = p.DistanceToSqr( pt );
  if ( dist < bestDist )
  {
    bestDist = dist;
    *res = pt;
  };

  return (T) sqrt( bestDist );
}


// Returns true if the ray intersects the sphere. 
template < class T >
bool IntersectSphereByRay( const Vertex3D< T > &center, T radius, 
                           const Vertex3D< T > &org, const Vertex3D< T > &dir,
                           T *time )
{
  Vertex3D< T > q = center - org;
  T c = q & q;             // squared length
  Vertex3D< T > nDir = dir;
  T l = dir.Length();
  if ( fabs( l ) < Plane3D< T >::EPS )
    return false;
  nDir /= l;
  T v = q & nDir;
  T d = radius * radius - (c - v * v);
  if ( d < 0.0f )
    return false;
  *time = (T)(v - sqrt( d )) / l;
  return true;
};


// Detects the collision of the sphere and the triangle. 
// time - time to collision(can be negative); normal - normal from the triangle to the center of the sphere; ...
template < class T >
bool IntersectTrisBySphere( const Vertex3D< T > &v1, const Vertex3D< T > &v2, const Vertex3D< T > &v3,
                            const Vertex3D< T > &center, T radius, const Vertex3D< T > &speed,
                            T *time, Vertex3D< T > *normal )
{
  Plane3D< T > plane( v1, v2, v3 );
  Vertex3D< T > pt = center - plane.n * radius;
  if ( !plane.IntersectByRay( pt, speed, time ) )
    return false;
  Vertex3D< T > planePt = pt + (*time) * speed;
  *normal = plane.n;
  // Is the point inside the triangle or not?
  if ( !TrisContainsPoint( v1, v2, v3, plane.n, planePt ) )
  {
    // Finds the nearest point in the triangle
    Vertex3D< T > trisPt;
    ClosestPointToTris( v1, v2, v3, planePt, &trisPt );
    if ( !IntersectSphereByRay( center, radius, trisPt, -speed, time ) )
      return false;
    *normal = center - (trisPt - (*time) * speed);
    normal->Normalize();
  };
  return true;
};

For each triangle below the sphere i use the following "algorithm" (every render):

1) Call IntersectTrisBySphere method with the speed of the sphere and the inverse speed of the sphere. Find the time to the collision for each case.

2) If the time with normal speed <= 0 (intersects the triangle) and the time with inverse speed < 0, then I reflect the vector of speed.

Like this:

float k = normal.x*xSpeed+normal.y*ySpeed+normal.z*zSpeed;
xSpeed = koef * (xSpeed + normal.x * k *-2.0f);
ySpeed = koef * (ySpeed + normal.y * k *-2.0f);
zSpeed = koef * (zSpeed + normal.z * k *-2.0f);

This trick with 'inverse speed' is for avoiding the situations when the sphere moves from the triangle (time will be negative), but doesn't intersect it.

So, I am not sure what to do with multiple collision. I mean, my algorithm works fine(not sure:) ) when the sphere intersects only one triangle, but when I use it for several collisions (one by one), it doesn't work.

What should I do? Maybe I have a mistake somewhere else, I don't know. I have spent a lot of time for debuging, but it still doesn't work.

P.S. Sorry for my english :(

Answer 1

You probably should avoid sphere to intersect multiple triangles.

Since you can determine time to collision for a sphere and triangle, therefore, when you detect case of multiplie collision just choose shortest time and advance your simulation for this time instead of fixed timestep. Thus you can be sure you always will deal with single collision.