text: /*
   * Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
   *
   * This software is provided 'as-is', without any express or implied
   * warranty.  In no event will the authors be held liable for any damages
   * arising from the use of this software.
   * Permission is granted to anyone to use this software for any purpose,
   * including commercial applications, and to alter it and redistribute it
   * freely, subject to the following restrictions:
   * 1. The origin of this software must not be misrepresented; you must not
   * claim that you wrote the original software. If you use this software
   * in a product, an acknowledgment in the product documentation would be
   * appreciated but is not required.
   * 2. Altered source versions must be plainly marked as such, and must not be
   * misrepresented as being the original software.
   * 3. This notice may not be removed or altered from any source distribution.
   */
  
  #include <Box2D/Collision/b2Collision.h>
  #include <Box2D/Collision/Shapes/b2CircleShape.h>
  #include <Box2D/Collision/Shapes/b2EdgeShape.h>
  #include <Box2D/Collision/Shapes/b2PolygonShape.h>
  
  #include <array>
  
  using namespace box2d;
  
  // Compute contact points for edge versus circle.
  // This accounts for edge connectivity.
  void box2d::b2CollideEdgeAndCircle(b2Manifold* manifold, const b2EdgeShape* edgeA,
                                     const b2Transform& xfA, const b2CircleShape* circleB,
                                     const b2Transform& xfB)
  {
      manifold->pointCount = 0;
  
      // Compute circle in frame of edge
      b2Vec<float, 2> Q = b2MulT(xfA, b2Mul(xfB, circleB->m_p));
  
      b2Vec<float, 2> A = edgeA->m_vertex1, B = edgeA->m_vertex2;
      b2Vec<float, 2> e = B - A;
  
      // Barycentric coordinates
      float u = b2Dot(e, B - Q);
      float v = b2Dot(e, Q - A);
  
      float radius = edgeA->GetRadius() + circleB->GetRadius();
  
      b2ContactFeature cf;
      cf.indexB = 0;
      cf.typeB = b2ContactFeature::e_vertex;
  
      // Region A
      if (v <= 0.0f)
      {
          b2Vec<float, 2> P = A;
          b2Vec<float, 2> d = Q - P;
          float dd = b2Dot(d, d);
          if (dd > radius * radius)
          {
              return;
          }
  
          // Is there an edge connected to A?
          if (edgeA->m_hasVertex0)
          {
              b2Vec<float, 2> A1 = edgeA->m_vertex0;
              b2Vec<float, 2> B1 = A;
              b2Vec<float, 2> e1 = B1 - A1;
              float u1 = b2Dot(e1, B1 - Q);
  
              // Is the circle in Region AB of the previous edge?
              if (u1 > 0.0f)
              {
                  return;
              }
          }
  
          cf.indexA = 0;
          cf.typeA = b2ContactFeature::e_vertex;
          manifold->pointCount = 1;
          manifold->type = b2Manifold::e_circles;
          manifold->localNormal = {{0.0f, 0.0f}};
          manifold->localPoint = P;
          manifold->points[0].id.key = 0;
          manifold->points[0].id.cf = cf;
          manifold->points[0].localPoint = circleB->m_p;
          return;
      }
  
      // Region B
      if (u <= 0.0f)
      {
          b2Vec<float, 2> P = B;
          b2Vec<float, 2> d = Q - P;
          float dd = b2Dot(d, d);
          if (dd > radius * radius)
          {
              return;
          }
  
          // Is there an edge connected to B?
          if (edgeA->m_hasVertex3)
          {
              b2Vec<float, 2> B2 = edgeA->m_vertex3;
              b2Vec<float, 2> A2 = B;
              b2Vec<float, 2> e2 = B2 - A2;
              float v2 = b2Dot(e2, Q - A2);
  
              // Is the circle in Region AB of the next edge?
              if (v2 > 0.0f)
              {
                  return;
              }
          }
  
          cf.indexA = 1;
          cf.typeA = b2ContactFeature::e_vertex;
          manifold->pointCount = 1;
          manifold->type = b2Manifold::e_circles;
          manifold->localNormal = {{0.0f, 0.0f}};
          manifold->localPoint = P;
          manifold->points[0].id.key = 0;
          manifold->points[0].id.cf = cf;
          manifold->points[0].localPoint = circleB->m_p;
          return;
      }
  
      // Region AB
      float den = b2Dot(e, e);
      b2Assert(den > 0.0f);
      b2Vec<float, 2> P = (1.0f / den) * (u * A + v * B);
      b2Vec<float, 2> d = Q - P;
      float dd = b2Dot(d, d);
      if (dd > radius * radius)
      {
          return;
      }
  
      b2Vec<float, 2> n{{-e[b2VecY], e[b2VecX]}};
      if (b2Dot(n, Q - A) < 0.0f)
      {
  //         n.Set(-n[b2VecX], -n[b2VecY]);
          n = -n;
      }
      n.Normalize();
  
      cf.indexA = 0;
      cf.typeA = b2ContactFeature::e_face;
      manifold->pointCount = 1;
      manifold->type = b2Manifold::e_faceA;
      manifold->localNormal = n;
      manifold->localPoint = A;
      manifold->points[0].id.key = 0;
      manifold->points[0].id.cf = cf;
      manifold->points[0].localPoint = circleB->m_p;
  }
  
  namespace 
  {
  
      // This structure is used to keep track of the best separating axis.
      struct b2EPAxis
      {
          enum Type
          {
              e_unknown,
              e_edgeA,
              e_edgeB
          };
  
          Type type;
          int32_t index;
          float separation;
      };
  
      // This holds polygon B expressed in frame A.
      struct b2TempPolygon
      {
          b2Vec<float, 2> vertices[MAX_POLYGON_VERTICES];
          b2Vec<float, 2> normals[MAX_POLYGON_VERTICES];
          int32_t count;
      };
  
      // Reference face used for clipping
      struct b2ReferenceFace
      {
          int32_t i1, i2;
  
          b2Vec<float, 2> v1, v2;
  
          b2Vec<float, 2> normal;
  
          b2Vec<float, 2> sideNormal1;
          float sideOffset1;
  
          b2Vec<float, 2> sideNormal2;
          float sideOffset2;
      };
  
      // This class collides and edge and a polygon, taking into account edge
      // adjacency.
      struct b2EPCollider
      {
          void Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA,
                      const b2PolygonShape* polygonB, const b2Transform& xfB);
          b2EPAxis ComputeEdgeSeparation();
          b2EPAxis ComputePolygonSeparation();
  
          enum VertexType
          {
              e_isolated,
              e_concave,
              e_convex
          };
  
          b2TempPolygon m_polygonB;
  
          b2Transform m_xf;
          b2Vec<float, 2> m_centroidB;
          b2Vec<float, 2> m_v0, m_v1, m_v2, m_v3;
          b2Vec<float, 2> m_normal0, m_normal1, m_normal2;
          b2Vec<float, 2> m_normal;
          VertexType m_type1, m_type2;
          b2Vec<float, 2> m_lowerLimit, m_upperLimit;
          float m_radius;
          bool m_front;
      };
  
  }
  
  // Algorithm:
  // 1. Classify v1 and v2
  // 2. Classify polygon centroid as front or back
  // 3. Flip normal if necessary
  // 4. Initialize normal range to [-pi, pi] about face normal
  // 5. Adjust normal range according to adjacent edges
  // 6. Visit each separating axes, only accept axes within the range
  // 7. Return if _any_ axis indicates separation
  // 8. Clip
  void b2EPCollider::Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA,
                             const b2PolygonShape* polygonB, const b2Transform& xfB)
  {
      m_xf = b2MulT(xfA, xfB);
  
      m_centroidB = b2Mul(m_xf, polygonB->GetCentroid());
  
      m_v0 = edgeA->m_vertex0;
      m_v1 = edgeA->m_vertex1;
      m_v2 = edgeA->m_vertex2;
      m_v3 = edgeA->m_vertex3;
  
      bool hasVertex0 = edgeA->m_hasVertex0;
      bool hasVertex3 = edgeA->m_hasVertex3;
  
      b2Vec<float, 2> edge1 = m_v2 - m_v1;
      edge1.Normalize();
      m_normal1 = {{edge1[b2VecY], -edge1[b2VecX]}};
      float offset1 = b2Dot(m_normal1, m_centroidB - m_v1);
      float offset0 = 0.0f, offset2 = 0.0f;
      bool convex1 = false, convex2 = false;
  
      // Is there a preceding edge?
      if (hasVertex0)
      {
          b2Vec<float, 2> edge0 = m_v1 - m_v0;
          edge0.Normalize();
          m_normal0 = {{edge0[b2VecY], -edge0[b2VecX]}};
          convex1 = b2Cross(edge0, edge1) >= 0.0f;
          offset0 = b2Dot(m_normal0, m_centroidB - m_v0);
      }
  
      // Is there a following edge?
      if (hasVertex3)
      {
          b2Vec<float, 2> edge2 = m_v3 - m_v2;
          edge2.Normalize();
          m_normal2 = {{edge2[b2VecY], -edge2[b2VecX]}};
          convex2 = b2Cross(edge1, edge2) > 0.0f;
          offset2 = b2Dot(m_normal2, m_centroidB - m_v2);
      }
  
      // Determine front or back collision. Determine collision normal limits.
      if (hasVertex0 && hasVertex3)
      {
          if (convex1 && convex2)
          {
              m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
              if (m_front)
              {
                  m_normal = m_normal1;
                  m_lowerLimit = m_normal0;
                  m_upperLimit = m_normal2;
              }
              else
              {
                  m_normal = -m_normal1;
                  m_lowerLimit = -m_normal1;
                  m_upperLimit = -m_normal1;
              }
          }
          else if (convex1)
          {
              m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
              if (m_front)
              {
                  m_normal = m_normal1;
                  m_lowerLimit = m_normal0;
                  m_upperLimit = m_normal1;
              }
              else
              {
                  m_normal = -m_normal1;
                  m_lowerLimit = -m_normal2;
                  m_upperLimit = -m_normal1;
              }
          }
          else if (convex2)
          {
              m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
              if (m_front)
              {
                  m_normal = m_normal1;
                  m_lowerLimit = m_normal1;
                  m_upperLimit = m_normal2;
              }
              else
              {
                  m_normal = -m_normal1;
                  m_lowerLimit = -m_normal1;
                  m_upperLimit = -m_normal0;
              }
          }
          else
          {
              m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
              if (m_front)
              {
                  m_normal = m_normal1;
                  m_lowerLimit = m_normal1;
                  m_upperLimit = m_normal1;
              }
              else
              {
                  m_normal = -m_normal1;
                  m_lowerLimit = -m_normal2;
                  m_upperLimit = -m_normal0;
              }
          }
      }
      else if (hasVertex0)
      {
          if (convex1)
          {
              m_front = offset0 >= 0.0f || offset1 >= 0.0f;
              if (m_front)
              {
                  m_normal = m_normal1;
                  m_lowerLimit = m_normal0;
                  m_upperLimit = -m_normal1;
              }
              else
              {
                  m_normal = -m_normal1;
                  m_lowerLimit = m_normal1;
                  m_upperLimit = -m_normal1;
              }
          }
          else
          {
              m_front = offset0 >= 0.0f && offset1 >= 0.0f;
              if (m_front)
              {
                  m_normal = m_normal1;
                  m_lowerLimit = m_normal1;
                  m_upperLimit = -m_normal1;
              }
              else
              {
                  m_normal = -m_normal1;
                  m_lowerLimit = m_normal1;
                  m_upperLimit = -m_normal0;
              }
          }
      }
      else if (hasVertex3)
      {
          if (convex2)
          {
              m_front = offset1 >= 0.0f || offset2 >= 0.0f;
              if (m_front)
              {
                  m_normal = m_normal1;
                  m_lowerLimit = -m_normal1;
                  m_upperLimit = m_normal2;
              }
              else
              {
                  m_normal = -m_normal1;
                  m_lowerLimit = -m_normal1;
                  m_upperLimit = m_normal1;
              }
          }
          else
          {
              m_front = offset1 >= 0.0f && offset2 >= 0.0f;
              if (m_front)
              {
                  m_normal = m_normal1;
                  m_lowerLimit = -m_normal1;
                  m_upperLimit = m_normal1;
              }
              else
              {
                  m_normal = -m_normal1;
                  m_lowerLimit = -m_normal2;
                  m_upperLimit = m_normal1;
              }
          }
      }
      else
      {
          m_front = offset1 >= 0.0f;
          if (m_front)
          {
              m_normal = m_normal1;
              m_lowerLimit = -m_normal1;
              m_upperLimit = -m_normal1;
          }
          else
          {
              m_normal = -m_normal1;
              m_lowerLimit = m_normal1;
              m_upperLimit = m_normal1;
          }
      }
  
      // Get polygonB in frameA
      auto vBs = polygonB->GetVertices();
      m_polygonB.count = vBs.size();
      for (std::size_t i = 0; i < vBs.size(); ++i)
      {
          m_polygonB.vertices[i] = b2Mul(m_xf, vBs[i]);
          m_polygonB.normals[i] = b2Mul(m_xf.q, vBs[i]);
      }
  
      m_radius = 2.0f * POLYGON_RADIUS;
  
      manifold->pointCount = 0;
  
      b2EPAxis edgeAxis = ComputeEdgeSeparation();
  
      // If no valid normal can be found than this edge should not collide.
      if (edgeAxis.type == b2EPAxis::e_unknown)
      {
          return;
      }
  
      if (edgeAxis.separation > m_radius)
      {
          return;
      }
  
      b2EPAxis polygonAxis = ComputePolygonSeparation();
      if (polygonAxis.type != b2EPAxis::e_unknown && polygonAxis.separation > m_radius)
      {
          return;
      }
  
      // Use hysteresis for jitter reduction.
      const float k_relativeTol = 0.98f;
      const float k_absoluteTol = 0.001f;
  
      b2EPAxis primaryAxis;
      if (polygonAxis.type == b2EPAxis::e_unknown)
      {
          primaryAxis = edgeAxis;
      }
      else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)
      {
          primaryAxis = polygonAxis;
      }
      else
      {
          primaryAxis = edgeAxis;
      }
  
      std::array<b2ClipVertex, 2> ie;
      b2ReferenceFace rf;
      if (primaryAxis.type == b2EPAxis::e_edgeA)
      {
          manifold->type = b2Manifold::e_faceA;
  
          // Search for the polygon normal that is most anti-parallel to the edge
          // normal.
          int32_t bestIndex = 0;
          float bestValue = b2Dot(m_normal, m_polygonB.normals[0]);
          for (int32_t i = 1; i < m_polygonB.count; ++i)
          {
              float value = b2Dot(m_normal, m_polygonB.normals[i]);
              if (value < bestValue)
              {
                  bestValue = value;
                  bestIndex = i;
              }
          }
  
          int32_t i1 = bestIndex;
          int32_t i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0;
  
          ie[0].v = m_polygonB.vertices[i1];
          ie[0].id.cf.indexA = 0;
          ie[0].id.cf.indexB = static_cast<uint8_t>(i1);
          ie[0].id.cf.typeA = b2ContactFeature::e_face;
          ie[0].id.cf.typeB = b2ContactFeature::e_vertex;
  
          ie[1].v = m_polygonB.vertices[i2];
          ie[1].id.cf.indexA = 0;
          ie[1].id.cf.indexB = static_cast<uint8_t>(i2);
          ie[1].id.cf.typeA = b2ContactFeature::e_face;
          ie[1].id.cf.typeB = b2ContactFeature::e_vertex;
  
          if (m_front)
          {
              rf.i1 = 0;
              rf.i2 = 1;
              rf.v1 = m_v1;
              rf.v2 = m_v2;
              rf.normal = m_normal1;
          }
          else
          {
              rf.i1 = 1;
              rf.i2 = 0;
              rf.v1 = m_v2;
              rf.v2 = m_v1;
              rf.normal = -m_normal1;
          }
      }
      else
      {
          manifold->type = b2Manifold::e_faceB;
  
          ie[0].v = m_v1;
          ie[0].id.cf.indexA = 0;
          ie[0].id.cf.indexB = static_cast<uint8_t>(primaryAxis.index);
          ie[0].id.cf.typeA = b2ContactFeature::e_vertex;
          ie[0].id.cf.typeB = b2ContactFeature::e_face;
  
          ie[1].v = m_v2;
          ie[1].id.cf.indexA = 0;
          ie[1].id.cf.indexB = static_cast<uint8_t>(primaryAxis.index);
          ie[1].id.cf.typeA = b2ContactFeature::e_vertex;
          ie[1].id.cf.typeB = b2ContactFeature::e_face;
  
          rf.i1 = primaryAxis.index;
          rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0;
          rf.v1 = m_polygonB.vertices[rf.i1];
          rf.v2 = m_polygonB.vertices[rf.i2];
          rf.normal = m_polygonB.normals[rf.i1];
      }
  
      rf.sideNormal1 = {{rf.normal[b2VecY], -rf.normal[b2VecX]}};
      rf.sideNormal2 = -rf.sideNormal1;
      rf.sideOffset1 = b2Dot(rf.sideNormal1, rf.v1);
      rf.sideOffset2 = b2Dot(rf.sideNormal2, rf.v2);
  
      // Clip incident edge against extruded edge1 side edges.
      std::array<b2ClipVertex, 2> clipPoints1;
      std::array<b2ClipVertex, 2> clipPoints2;
      int32_t np;
  
      // Clip to box side 1
      np = b2ClipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1);
  
      if (np < MAX_MANIFOLD_POINTS)
      {
          return;
      }
  
      // Clip to negative box side 1
      np = b2ClipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2);
  
      if (np < MAX_MANIFOLD_POINTS)
      {
          return;
      }
  
      // Now clipPoints2 contains the clipped points.
      if (primaryAxis.type == b2EPAxis::e_edgeA)
      {
          manifold->localNormal = rf.normal;
          manifold->localPoint = rf.v1;
      }
      else
      {
          manifold->localNormal = polygonB->GetNormals()[rf.i1];
          manifold->localPoint = polygonB->GetNormals()[rf.i1];
      }
  
      int32_t pointCount = 0;
      for (auto& elem : clipPoints2)
      {
          float separation;
  
          separation = b2Dot(rf.normal, elem.v - rf.v1);
  
          if (separation <= m_radius)
          {
              b2ManifoldPoint* cp = manifold->points + pointCount;
  
              if (primaryAxis.type == b2EPAxis::e_edgeA)
              {
                  cp->localPoint = b2MulT(m_xf, elem.v);
                  cp->id = elem.id;
              }
              else
              {
                  cp->localPoint = elem.v;
                  cp->id.cf.typeA = elem.id.cf.typeB;
                  cp->id.cf.typeB = elem.id.cf.typeA;
                  cp->id.cf.indexA = elem.id.cf.indexB;
                  cp->id.cf.indexB = elem.id.cf.indexA;
              }
  
              ++pointCount;
          }
      }
  
      manifold->pointCount = pointCount;
  }
  
  b2EPAxis b2EPCollider::ComputeEdgeSeparation()
  {
      b2EPAxis axis;
      axis.type = b2EPAxis::e_edgeA;
      axis.index = m_front ? 0 : 1;
      axis.separation = FLT_MAX;
  
      for (int32_t i = 0; i < m_polygonB.count; ++i)
      {
          float s = b2Dot(m_normal, m_polygonB.vertices[i] - m_v1);
          if (s < axis.separation)
          {
              axis.separation = s;
          }
      }
  
      return axis;
  }
  
  b2EPAxis b2EPCollider::ComputePolygonSeparation()
  {
      b2EPAxis axis;
      axis.type = b2EPAxis::e_unknown;
      axis.index = -1;
      axis.separation = -FLT_MAX;
  
      b2Vec<float, 2> perp{{-m_normal[b2VecY], m_normal[b2VecX]}};
  
      for (int32_t i = 0; i < m_polygonB.count; ++i)
      {
          b2Vec<float, 2> n = -m_polygonB.normals[i];
  
          float s1 = b2Dot(n, m_polygonB.vertices[i] - m_v1);
          float s2 = b2Dot(n, m_polygonB.vertices[i] - m_v2);
          float s = b2Min(s1, s2);
  
          if (s > m_radius)
          {
              // No collision
              axis.type = b2EPAxis::e_edgeB;
              axis.index = i;
              axis.separation = s;
              return axis;
          }
  
          // Adjacency
          if (b2Dot(n, perp) >= 0.0f)
          {
              if (b2Dot(n - m_upperLimit, m_normal) < -ANGULAR_SLOP)
              {
                  continue;
              }
          }
          else
          {
              if (b2Dot(n - m_lowerLimit, m_normal) < -ANGULAR_SLOP)
              {
                  continue;
              }
          }
  
          if (s > axis.separation)
          {
              axis.type = b2EPAxis::e_edgeB;
              axis.index = i;
              axis.separation = s;
          }
      }
  
      return axis;
  }
  
  void box2d::b2CollideEdgeAndPolygon(b2Manifold* manifold, const b2EdgeShape* edgeA,
                                      const b2Transform& xfA, const b2PolygonShape* polygonB,
                                      const b2Transform& xfB)
  {
      b2EPCollider collider;
      collider.Collide(manifold, edgeA, xfA, polygonB, xfB);
  }
  