| public static Mul ( MTransform cFromB, MTransform bFromA ) : MTransform | ||
| cFromB | MTransform | |
| bFromA | MTransform | |
| return | MTransform | |
// 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);
}
public static Aabb TransformAabb(MTransform transform, Aabb aabb)
{
float3 halfExtentsInA = aabb.Extents * 0.5f;
float3 transformedX = math.abs(transform.Rotation.c0 * halfExtentsInA.x);
float3 transformedY = math.abs(transform.Rotation.c1 * halfExtentsInA.y);
float3 transformedZ = math.abs(transform.Rotation.c2 * halfExtentsInA.z);
float3 halfExtentsInB = transformedX + transformedY + transformedZ;
float3 centerInB = Math.Mul(transform, aabb.Center);
return(new Aabb
{
Min = centerInB - halfExtentsInB,
Max = centerInB + halfExtentsInB
});
}
public static Aabb TransformAabb(MTransform transform, Aabb aabb)
{
// Transforming an empty AABB results in NaNs!
if (!aabb.IsValid)
{
return(aabb);
}
float3 halfExtentsInA = aabb.Extents * 0.5f;
float3 transformedX = math.abs(transform.Rotation.c0 * halfExtentsInA.x);
float3 transformedY = math.abs(transform.Rotation.c1 * halfExtentsInA.y);
float3 transformedZ = math.abs(transform.Rotation.c2 * halfExtentsInA.z);
float3 halfExtentsInB = transformedX + transformedY + transformedZ;
float3 centerInB = Math.Mul(transform, aabb.Center);
return(new Aabb
{
Min = centerInB - halfExtentsInB,
Max = centerInB + halfExtentsInB
});
}
public void PushCompositeCollider(ColliderKeyPath compositeKey, MTransform parentFromComposite, out MTransform worldFromParent)
{
m_KeyPath.PushChildKey(compositeKey);
worldFromParent = m_WorldFromA;
m_WorldFromA = Math.Mul(worldFromParent, parentFromComposite);
}