randygaul / impulseengine Goto Github PK

void SetBox( real hw, real hh )
  {
    m_vertexCount = 4;
    m_vertices[0].Set( -hw, -hh );
    m_vertices[1].Set(  hw, -hh );
    m_vertices[2].Set(  hw,  hh );
    m_vertices[3].Set( -hw,  hh );
    m_normals[0].Set(  0.0f,  -1.0f );
    m_normals[1].Set(  1.0f,   0.0f );
    m_normals[2].Set(  0.0f,   1.0f );
    m_normals[3].Set( -1.0f,   0.0f );
  }

    for (;;)
    {
      hull[outCount] = indexHull;

      // Search for next index that wraps around the hull
      // by computing cross products to find the most counter-clockwise
      // vertex in the set, given the previos hull index
      int32 nextHullIndex = 0;
      for(int32 i = 1; i < (int32)count; ++i)
      {
        // Skip if same coordinate as we need three unique
        // points in the set to perform a cross product
        if(nextHullIndex == indexHull)
        {
          nextHullIndex = i;
          continue;
        }

        // Cross every set of three unique vertices
        // Record each counter clockwise third vertex and add
        // to the output hull
        // See : http://www.oocities.org/pcgpe/math2d.html
        Vec2 e1 = vertices[nextHullIndex] - vertices[hull[outCount]];
        Vec2 e2 = vertices[i] - vertices[hull[outCount]];
        real c = Cross( e1, e2 );
        if(c > 0.0f)
          nextHullIndex = i;

        // Cross product is zero then e vectors are on same line
        // therefor want to record vertex farthest along that line
        if(c == 0.0f && e2.LenSqr( ) > e1.LenSqr( ))
          nextHullIndex = i;
      }
      
      ++outCount;
      indexHull = nextHullIndex;

      // Conclude algorithm upon wrap-around
      if(nextHullIndex == rightMost)
      {
        m_vertexCount = outCount;
        break;
      }
    }

    // Copy vertices into shape's vertices
    for(uint32 i = 0; i < m_vertexCount; ++i)
      m_vertices[i] = vertices[hull[i]];

    // Compute face normals
    for(uint32 i1 = 0; i1 < m_vertexCount; ++i1)
    {
      uint32 i2 = i1 + 1 < m_vertexCount ? i1 + 1 : 0;
      Vec2 face = m_vertices[i2] - m_vertices[i1];

      // Ensure no zero-length edges, because that's bad
      assert( face.LenSqr( ) > EPSILON * EPSILON );

      // Calculate normal with 2D cross product between vector and scalar
      m_normals[i1] = Vec2( face.y, -face.x );
      m_normals[i1].Normalize( );
    }
  }

void CircletoPolygon( Manifold *m, Body *a, Body *b )
{
  Circle *A       = reinterpret_cast<Circle *>      (a->shape);
  PolygonShape *B = reinterpret_cast<PolygonShape *>(b->shape);

  m->contact_count = 0;

  // Transform circle center to Polygon model space
  Vec2 center = a->position;
  center = B->u.Transpose( ) * (center - b->position);

  // Find edge with minimum penetration
  // Exact concept as using support points in Polygon vs Polygon
  real separation = -FLT_MAX;
  uint32 faceNormal = 0;
  for(uint32 i = 0; i < B->m_vertexCount; ++i)
  {
    real s = Dot( B->m_normals[i], center - B->m_vertices[i] );

    if(s > A->radius)
      return;

    if(s > separation)
    {
      separation = s;
      faceNormal = i;
    }
  }

  // Grab face's vertices
  Vec2 v1 = B->m_vertices[faceNormal];
  uint32 i2 = faceNormal + 1 < B->m_vertexCount ? faceNormal + 1 : 0;
  Vec2 v2 = B->m_vertices[i2];

  // Check to see if center is within polygon
  if(separation < EPSILON)
  {
    m->contact_count = 1;
    m->normal = -(B->u * B->m_normals[faceNormal]);
    m->contacts[0] = m->normal * A->radius + a->position;
    m->penetration = A->radius;
    return;
  }

  // Determine which voronoi region of the edge center of circle lies within
  real dot1 = Dot( center - v1, v2 - v1 );
  real dot2 = Dot( center - v2, v1 - v2 );
  m->penetration = A->radius - separation;

  // Closest to v1
  if(dot1 <= 0.0f)
  {
    if(DistSqr( center, v1 ) > A->radius * A->radius)
      return;

    m->contact_count = 1;
    Vec2 n = v1 - center;
    n = B->u * n;
    n.Normalize( );
    m->normal = n;
    v1 = B->u * v1 + b->position;
    m->contacts[0] = v1;
  }

  // Closest to v2
  else if(dot2 <= 0.0f)
  {
    if(DistSqr( center, v2 ) > A->radius * A->radius)
      return;

    m->contact_count = 1;
    Vec2 n = v2 - center;
    v2 = B->u * v2 + b->position;
    m->contacts[0] = v2;
    n = B->u * n;
    n.Normalize( );
    m->normal = n;
  }

  // Closest to face
  else
  {
    Vec2 n = B->m_normals[faceNormal];
    if(Dot( center - v1, n ) > A->radius)
      return;

    n = B->u * n;
    m->normal = -n;
    m->contacts[0] = m->normal * A->radius + a->position;
    m->contact_count = 1;
  }
}

inline bool BiasGreaterThan( real a, real b )
{
  return a >= b;
}

...
const float k_tol = 0.1f * b2_linearSlop; 
if (separationB > separationA + k_tol)
...

PolygonShape poly;
    Vec2 vertices[] = {Vec2(-0.5, -0.5), Vec2(0.5,-0.5), Vec2(0.5, 0.5), Vec2(-0.5,0.5)};
    poly.Set( vertices, 4 );
    Body *b1 = new Body(&poly,-1.6,0);
    b1->SetOrient(0);
    
    PolygonShape poly2;
    Vec2 vertices2[] = {Vec2( -3.740512982560686,  -0.5735584769983424), Vec2( -3.719614114679503,  -1.7226350555144725), Vec2( -2.4932647260279026,  -2.7316117009988794), Vec2( -0.8201915887684166,  -3.339520273224558), Vec2( 0.8485318324075104,  -3.768661315482074), Vec2( 1.4125001862587832,  -3.877552867283213), Vec2( 3.7734708786741002,  -2.3132118400651835), Vec2( 3.9056625143223886,  -2.053720640728945), Vec2( 3.9608941922040817,  -0.21303954202834152), Vec2( 3.4524715186551624,  1.0169073997947553), Vec2( 1.9124525889738742,  2.634607801080719), Vec2( 0.24731544222635193,  3.825379925658064), Vec2( -1.1199503300054676,  4.6035871781070306), Vec2( -3.0722506887204464,  2.6117852157357326)};
    poly2.Set( vertices2, 14);
    Body *b2 = new Body(&poly2,0,0);
    
    b2->SetOrient(0);