// Collide an edge and polygon. This uses the SAT and clipping to produce up to 2 contact points.
// Edge adjacency is handle to produce locally valid contact points and normals. This is intended
// to allow the polygon to slide smoothly over an edge chain.
//
// Algorithm
// 1. Classify front-side or back-side collision with edge.
// 2. Compute separation
// 3. Process adjacent edges
// 4. Classify adjacent edge as convex, flat, null, or concave
// 5. Skip null or concave edges. Concave edges get a separate manifold.
// 6. If the edge is flat, compute contact points as normal. Discard boundary points.
// 7. If the edge is convex, compute it's separation.
// 8. Use the minimum separation of up to three edges. If the minimum separation
// is not the primary edge, return.
// 9. If the minimum separation is the primary edge, compute the contact points and return.
void b2EPCollider::Collide(b2Manifold* manifold)
{
manifold->pointCount = 0;
ComputeAdjacency();
b2EPAxis edgeAxis = ComputeEdgeSeparation();
// If no valid normal can be found than this edge should not collide.
// This can happen on the middle edge of a 3-edge zig-zag chain.
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 float32 k_relativeTol = 0.98f;
const float32 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;
}
b2EPProxy* proxy1;
b2EPProxy* proxy2;
b2ClipVertex incidentEdge[2];
if (primaryAxis.type == b2EPAxis::e_edgeA)
{
proxy1 = &m_proxyA;
proxy2 = &m_proxyB;
manifold->type = b2Manifold::e_faceA;
}
else
{
proxy1 = &m_proxyB;
proxy2 = &m_proxyA;
manifold->type = b2Manifold::e_faceB;
}
int32 edge1 = primaryAxis.index;
FindIncidentEdge(incidentEdge, proxy1, primaryAxis.index, proxy2);
int32 count1 = proxy1->count;
const b2Vec2* vertices1 = proxy1->vertices;
int32 iv1 = edge1;
int32 iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
b2Vec2 v11 = vertices1[iv1];
b2Vec2 v12 = vertices1[iv2];
b2Vec2 tangent = v12 - v11;
tangent.Normalize();
b2Vec2 normal = b2Cross(tangent, 1.0f);
b2Vec2 planePoint = 0.5f * (v11 + v12);
// Face offset.
float32 frontOffset = b2Dot(normal, v11);
// Side offsets, extended by polytope skin thickness.
float32 sideOffset1 = -b2Dot(tangent, v11) + m_radius;
float32 sideOffset2 = b2Dot(tangent, v12) + m_radius;
// Clip incident edge against extruded edge1 side edges.
b2ClipVertex clipPoints1[2];
b2ClipVertex clipPoints2[2];
int np;
// Clip to box side 1
np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1);
if (np < b2_maxManifoldPoints)
{
return;
}
// Clip to negative box side 1
np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2);
if (np < b2_maxManifoldPoints)
{
return;
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.type == b2EPAxis::e_edgeA)
{
manifold->localNormal = normal;
manifold->localPoint = planePoint;
}
else
{
manifold->localNormal = b2MulT(m_xf.R, normal);
manifold->localPoint = b2MulT(m_xf, planePoint);
}
int32 pointCount = 0;
for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
{
float32 separation;
separation = b2Dot(normal, clipPoints2[i].v) - frontOffset;
if (separation <= m_radius)
{
b2ManifoldPoint* cp = manifold->points + pointCount;
if (primaryAxis.type == b2EPAxis::e_edgeA)
{
cp->localPoint = b2MulT(m_xf, clipPoints2[i].v);
cp->id = clipPoints2[i].id;
}
else
{
cp->localPoint = clipPoints2[i].v;
cp->id.cf.typeA = clipPoints2[i].id.cf.typeB;
cp->id.cf.typeB = clipPoints2[i].id.cf.typeA;
cp->id.cf.indexA = clipPoints2[i].id.cf.indexB;
cp->id.cf.indexB = clipPoints2[i].id.cf.indexA;
}
++pointCount;
}
}
manifold->pointCount = pointCount;
}
Mesh CreateMesh(const char* ctmFile, bool computeAdjacency)
{
Mesh mesh = {0};
char qualifiedPath[256] = {0};
strcpy(qualifiedPath, PezResourcePath());
strcat(qualifiedPath, "/\0");
strcat(qualifiedPath, ctmFile);
// Open the CTM file:
CTMcontext ctmContext = ctmNewContext(CTM_IMPORT);
ctmLoad(ctmContext, qualifiedPath);
PezCheckCondition(ctmGetError(ctmContext) == CTM_NONE, "OpenCTM issue with loading %s", qualifiedPath);
CTMuint vertexCount = ctmGetInteger(ctmContext, CTM_VERTEX_COUNT);
CTMuint faceCount = ctmGetInteger(ctmContext, CTM_TRIANGLE_COUNT);
// Create the VBO for positions:
const CTMfloat* positions = ctmGetFloatArray(ctmContext, CTM_VERTICES);
if (positions) {
GLuint handle;
GLsizeiptr size = vertexCount * sizeof(float) * 3;
glGenBuffers(1, &handle);
glBindBuffer(GL_ARRAY_BUFFER, handle);
glBufferData(GL_ARRAY_BUFFER, size, positions, GL_STATIC_DRAW);
mesh.Positions = handle;
}
// Create the VBO for normals:
const CTMfloat* normals = ctmGetFloatArray(ctmContext, CTM_NORMALS);
if (normals) {
GLuint handle;
GLsizeiptr size = vertexCount * sizeof(float) * 3;
glGenBuffers(1, &handle);
glBindBuffer(GL_ARRAY_BUFFER, handle);
glBufferData(GL_ARRAY_BUFFER, size, normals, GL_STATIC_DRAW);
mesh.Normals = handle;
}
// Create the VBO for indices:
const CTMuint* indices = ctmGetIntegerArray(ctmContext, CTM_INDICES);
if (indices) {
GLsizeiptr bufferSize = faceCount * 3 * sizeof(unsigned short);
// Convert indices from 32-bit to 16-bit:
PezCheckCondition(vertexCount < (1 << 16), "Too many indices to fit in 16 bits");
unsigned short* faceBuffer = (unsigned short*) malloc(bufferSize);
unsigned short* pDest = faceBuffer;
const CTMuint* pSrc = indices;
unsigned int remainingFaces = faceCount;
while (remainingFaces--)
{
*pDest++ = (unsigned short) *pSrc++;
*pDest++ = (unsigned short) *pSrc++;
*pDest++ = (unsigned short) *pSrc++;
}
// Compute adjacency if desired:
if (computeAdjacency)
{
bufferSize = faceCount * 6 * sizeof(unsigned short);
unsigned short* destBuffer = (unsigned short*) malloc(bufferSize);
ComputeAdjacency(destBuffer, faceBuffer, faceCount, vertexCount);
free(faceBuffer);
faceBuffer = destBuffer;
}
GLuint handle;
glGenBuffers(1, &handle);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, handle);
glBufferData(GL_ELEMENT_ARRAY_BUFFER, bufferSize, faceBuffer, GL_STATIC_DRAW);
mesh.Faces = handle;
free(faceBuffer);
}
ctmFreeContext(ctmContext);
mesh.FaceCount = faceCount;
mesh.VertexCount = vertexCount;
glBindBuffer(GL_ARRAY_BUFFER, 0);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
return mesh;
}
