Box2D Forums

b2Assert(0.0f <= lower && lower <= maxLambda);

bool b2PolygonShape::TestSegment(
   const b2XForm& xf,
   float32* lambda,
   b2Vec2* normal,
   const b2Segment& segment,
   float32 maxLambda) const
{
   float32 upper = maxLambda;

   b2Vec2 p1 = b2MulT(xf.R, segment.p1 - xf.position);
   b2Vec2 p2 = b2MulT(xf.R, segment.p2 - xf.position);
   b2Vec2 p_delta = p2 - p1;
   int32 index = -1;

   for( int32 i = 0; i < m_vertexCount; ++i )
   {
      // Test the segment from vertex i to vertex i+1
      b2Vec2 s1 = m_vertices[i];
      b2Vec2 s2 = m_vertices[(i+1)%m_vertexCount];
      b2Vec2 s_delta = s2 - s1;

      float32 denom = ( s_delta.y * p_delta.x ) - ( s_delta.x * p_delta.y );

      // If the segments are parallel, there can be no intersection.
      if( 0.0f == denom )
         continue;

      float32 lambda_s = ( p_delta.x * ( p1.y - s1.y ) ) - ( p_delta.y * ( p1.x - s1.x ) );
      lambda_s /= denom;

      // Make sure the collision occurs along the part of s that actually makes up a side of the polygon.
      if( ( 0.0f > lambda_s ) || ( 1.0f < lambda_s ) )
         continue;

      float32 lambda_p = ( s_delta.x * ( p1.y - s1.y ) ) - ( s_delta.y * ( p1.x - s1.x ) );
      lambda_p /= denom;

      // See if this is closer along p (the segment we are testing against this polygon) than any previous results.
      if( ( lambda_p < upper ) && ( lambda_p >= 0.0f ) )
      {
         upper = lambda_p;
         index = i;
      }
   }

   if (index >= 0)
   {
      *lambda = upper;
      *normal = b2Mul(xf.R, m_normals[index]);
      return true;
   }

   return false;
}

Entity*
raycast( float32* lambda, b2Vec2* normal, const b2Segment& segment, float32 maxLambda )
{
   // TODO: this is exceptionally naive, and should be replaced by proper raycasting at some point
   float32 min_lambda = maxLambda, this_lambda;
   b2Vec2 min_normal, this_normal;
   Entity* min_entity = NULL;

   for( std::list<Entity*>::iterator it = entities.begin( ); it != entities.end( ); ++it )
   {
      b2Body* body = (*it)->getBody( );
      b2Shape* shape = body->m_shapeList;

      while( NULL != shape )
      {
         if( true == shape->TestSegment( body->GetXForm( ), &this_lambda, &this_normal, segment, maxLambda ) )
         {
            if( this_lambda < min_lambda )
            {
               min_lambda = this_lambda;
               min_normal = this_normal;
               min_entity = (*it);
            }
         }

         shape = shape->m_next;
      }
   }

   if( NULL != min_entity )
   {
      *lambda = min_lambda;
      *normal = min_normal;
   }

   return min_entity;
}