internal unsafe void SetAsTriangle(float3 v0, float3 v1, float3 v2)
{
m_Header.Type = ColliderType.Triangle;
m_Header.Version += 1;
ConvexHull.VerticesBlob.Length = 3;
ConvexHull.FaceVertexIndicesBlob.Length = 6;
fixed(PolygonCollider *collider = &this)
{
float3 *vertices = (float3 *)(&collider->m_Vertices[0]);
vertices[0] = v0;
vertices[1] = v1;
vertices[2] = v2;
ConvexHull.Face *faces = (ConvexHull.Face *)(&collider->m_Faces[0]);
faces[0] = new ConvexHull.Face {
FirstIndex = 0, NumVertices = 3, MinHalfAngleCompressed = 0xff
};
faces[1] = new ConvexHull.Face {
FirstIndex = 3, NumVertices = 3, MinHalfAngleCompressed = 0xff
};
byte *index = &collider->m_FaceVertexIndices[0];
*index++ = 0; *index++ = 1; *index++ = 2;
*index++ = 2; *index++ = 1; *index++ = 0;
}
SetPlanes();
}
// Tries to generate a manifold between a face and an edge. It can fail for the same reasons as FaceFace().
// In those cases, FaceEdge() returns false and the caller should generate a contact from the closest points on the shapes.
private static unsafe bool FaceEdge(
ref ConvexHull faceConvexA, ref ConvexHull edgeConvexB, int faceIndexA, MTransform worldFromA, MTransform aFromB,
float3 normal, float distance, ref Manifold manifold)
{
// Check if the face is nearly perpendicular to the normal
const float cosMaxAngle = 0.05f;
Plane planeA = faceConvexA.Planes[faceIndexA];
float dotA = math.dot(planeA.Normal, normal);
if (math.abs(dotA) < cosMaxAngle)
{
return(false);
}
// Check if the manifold gets a point roughly as close as the closest
distance += closestDistanceTolerance;
bool foundClosestPoint = false;
// Get the supporting face on A
ConvexHull.Face faceA = faceConvexA.Faces[faceIndexA];
byte * indicesA = faceConvexA.FaceVertexIndicesPtr + faceA.FirstIndex;
// Get edge in B
float3 vertexB0 = Math.Mul(aFromB, edgeConvexB.Vertices[0]);
float3 edgeB = math.mul(aFromB.Rotation, edgeConvexB.Vertices[1] - edgeConvexB.Vertices[0]);
// For each edge of A
float3 *verticesA = faceConvexA.VerticesPtr;
float fracEnterB = 0.0f;
float fracExitB = 1.0f;
for (EdgeIterator edgeA = EdgeIterator.Begin(verticesA, indicesA, -normal, faceA.NumVertices); edgeA.Valid(); edgeA.Advance())
{
// Cast edge B against plane A
castRayPlane(vertexB0, edgeB, edgeA.Perp, edgeA.Offset, ref fracEnterB, ref fracExitB);
}
// If edge B hits A, add a contact points
if (fracEnterB < fracExitB)
{
float invDotA = math.rcp(dotA);
float sumRadii = faceConvexA.ConvexRadius + edgeConvexB.ConvexRadius;
float distance0 = (math.dot(vertexB0, planeA.Normal) + planeA.Distance) * -invDotA;
float deltaDistance = math.dot(edgeB, planeA.Normal) * -invDotA;
foundClosestPoint |= AddEdgeContact(vertexB0, edgeB, distance0, deltaDistance, fracEnterB, normal, edgeConvexB.ConvexRadius, sumRadii, worldFromA, distance, ref manifold);
foundClosestPoint |= AddEdgeContact(vertexB0, edgeB, distance0, deltaDistance, fracExitB, normal, edgeConvexB.ConvexRadius, sumRadii, worldFromA, distance, ref manifold);
}
return(foundClosestPoint);
}
// Tries to generate a manifold between a pair of faces. It can fail in some cases due to numerical accuracy:
// 1) both faces are nearly perpendicular to the normal
// 2) the closest features on the shapes are vertices, so that the intersection of the projection of the faces to the plane perpendicular to the normal contains only one point
// In those cases, FaceFace() returns false and the caller should generate a contact from the closest points on the shapes.
private static unsafe bool FaceFace(
ref ConvexHull convexA, ref ConvexHull convexB, int faceIndexA, int faceIndexB, MTransform worldFromA, MTransform aFromB,
float3 normal, float distance, ref Manifold manifold)
{
// Get the plane of each face
Plane planeA = convexA.Planes[faceIndexA];
Plane planeB = TransformPlane(aFromB, convexB.Planes[faceIndexB]);
// Handle cases where one of the faces is nearly perpendicular to the contact normal
// This gets around divide by zero / numerical problems from dividing collider planes which often contain some error by a very small number, amplifying that error
const float cosMaxAngle = 0.05f;
float dotA = math.dot(planeA.Normal, normal);
float dotB = math.dot(planeB.Normal, normal);
bool acceptB = true; // true if vertices of B projected onto the face of A are accepted
if (dotA > -cosMaxAngle)
{
// Handle cases where both faces are nearly perpendicular to the contact normal.
if (dotB < cosMaxAngle)
{
// Both faces are nearly perpendicular to the contact normal, let the caller generate a single contact
return(false);
}
// Face of A is nearly perpendicular to the contact normal, don't try to project vertices onto it
acceptB = false;
}
else if (dotB < cosMaxAngle)
{
// Face of B is nearly perpendicular to the normal, so we need to clip the edges of B against face A instead
MTransform bFromA = Inverse(aFromB);
float3 normalInB = math.mul(bFromA.Rotation, -normal);
MTransform worldFromB = Mul(worldFromA, aFromB);
bool result = FaceFace(ref convexB, ref convexA, faceIndexB, faceIndexA, worldFromB, bFromA, normalInB, distance, ref manifold);
manifold.Normal = -manifold.Normal;
manifold.Flip();
return(result);
}
// Check if the manifold gets a point roughly as close as the closest
distance += closestDistanceTolerance;
bool foundClosestPoint = false;
// Transform vertices of B into A-space
// Initialize validB, which is true for each vertex of B that is inside face A
ConvexHull.Face faceA = convexA.Faces[faceIndexA];
ConvexHull.Face faceB = convexB.Faces[faceIndexB];
bool * validB = stackalloc bool[faceB.NumVertices];
float3 * verticesBinA = stackalloc float3[faceB.NumVertices];
{
byte * indicesB = convexB.FaceVertexIndicesPtr + faceB.FirstIndex;
float3 *verticesB = convexB.VerticesPtr;
for (int i = 0; i < faceB.NumVertices; i++)
{
validB[i] = acceptB;
verticesBinA[i] = Mul(aFromB, verticesB[indicesB[i]]);
}
}
// For each edge of A
float invDotB = math.rcp(dotB);
float sumRadii = convexA.ConvexRadius + convexB.ConvexRadius;
byte * indicesA = convexA.FaceVertexIndicesPtr + faceA.FirstIndex;
float3 *verticesA = convexA.VerticesPtr;
for (EdgeIterator edgeA = EdgeIterator.Begin(verticesA, indicesA, -normal, faceA.NumVertices); edgeA.Valid(); edgeA.Advance())
{
float fracEnterA = 0.0f;
float fracExitA = 1.0f;
// For each edge of B
for (EdgeIterator edgeB = EdgeIterator.Begin(verticesBinA, null, normal, faceB.NumVertices); edgeB.Valid(); edgeB.Advance())
{
// Cast edge A against plane B and test if vertex B is inside plane A
castRayPlane(edgeA.Vertex0, edgeA.Edge, edgeB.Perp, edgeB.Offset, ref fracEnterA, ref fracExitA);
validB[edgeB.Index] &= (math.dot(edgeB.Vertex1, edgeA.Perp) < edgeA.Offset);
}
// If edge A hits B, add a contact points
if (fracEnterA < fracExitA)
{
float distance0 = (math.dot(edgeA.Vertex0, planeB.Normal) + planeB.Distance) * invDotB;
float deltaDistance = math.dot(edgeA.Edge, planeB.Normal) * invDotB;
float3 vertexAOnB = edgeA.Vertex0 - normal * distance0;
float3 edgeAOnB = edgeA.Edge - normal * deltaDistance;
foundClosestPoint |= AddEdgeContact(vertexAOnB, edgeAOnB, distance0, deltaDistance, fracEnterA, normal, convexB.ConvexRadius, sumRadii, worldFromA, distance, ref manifold);
if (fracExitA < 1.0f) // If the exit fraction is 1, then the next edge has the same contact point with enter fraction 0
{
foundClosestPoint |= AddEdgeContact(vertexAOnB, edgeAOnB, distance0, deltaDistance, fracExitA, normal, convexB.ConvexRadius, sumRadii, worldFromA, distance, ref manifold);
}
}
}
// For each vertex of B
float invDotA = math.rcp(dotA);
for (int i = 0; i < faceB.NumVertices; i++)
{
if (validB[i] && manifold.NumContacts < Manifold.k_MaxNumContacts)
{
float3 vertexB = verticesBinA[i];
float distanceB = (math.dot(vertexB, planeA.Normal) + planeA.Distance) * -invDotA;
manifold[manifold.NumContacts++] = new ContactPoint
{
Position = Mul(worldFromA, vertexB) + manifold.Normal * convexB.ConvexRadius,
Distance = distanceB - sumRadii
};
foundClosestPoint |= distanceB <= distance;
}
}
return(foundClosestPoint);
}