// Create contact points for a capsule and triangle in world space. public static unsafe void CapsuleTriangle( CapsuleCollider *capsuleA, PolygonCollider *triangleB, MTransform worldFromA, MTransform aFromB, float maxDistance, out Manifold manifold) { Assert.IsTrue(triangleB->Vertices.Length == 3); DistanceQueries.Result convexDistance = DistanceQueries.CapsuleTriangle(capsuleA, triangleB, aFromB); if (convexDistance.Distance < maxDistance) { // Build manifold manifold = new Manifold { Normal = math.mul(worldFromA.Rotation, -convexDistance.NormalInA) // negate the normal because we are temporarily flipping to triangle A capsule B }; MTransform worldFromB = Mul(worldFromA, aFromB); MTransform bFromA = Inverse(aFromB); float3 normalInB = math.mul(bFromA.Rotation, convexDistance.NormalInA); int faceIndexB = triangleB->ConvexHull.GetSupportingFace(normalInB); if (FaceEdge(ref triangleB->ConvexHull, ref capsuleA->ConvexHull, faceIndexB, worldFromB, bFromA, -normalInB, convexDistance.Distance + capsuleA->Radius, ref manifold)) { manifold.Flip(); } else { manifold = new Manifold(convexDistance, worldFromA); } } else { manifold = new Manifold(); } }
public void Execute(ref ModifiableContactHeader contactHeader, ref ModifiableContactPoint contactPoint) { bool isBodyA = (contactHeader.EntityA == SurfaceEntity); bool isBodyB = (contactHeader.EntityB == SurfaceEntity); if (isBodyA || isBodyB) { if (contactPoint.Index == 0) { // if we have a mesh surface we can get the surface normal from the plane of the polygon var rbIdx = CollisionWorld.GetRigidBodyIndex(SurfaceEntity); var body = CollisionWorld.Bodies[rbIdx]; if (body.Collider.Value.CollisionType == CollisionType.Composite) { unsafe { body.Collider.Value.GetLeaf(isBodyA ? contactHeader.ColliderKeyA : contactHeader.ColliderKeyB, out ChildCollider leafCollider); if (leafCollider.Collider->Type == ColliderType.Triangle || leafCollider.Collider->Type == ColliderType.Quad) { PolygonCollider *polygonCollider = (PolygonCollider *)leafCollider.Collider; // Potential optimization: If TransformFromChild has no rotation just use body.WorldFromBody.rot // This is likely if you only have a MeshCollider with no hierarchy. quaternion rotation = math.mul(body.WorldFromBody.rot, leafCollider.TransformFromChild.rot); float3 surfaceNormal = math.rotate(rotation, polygonCollider->Planes[0].Normal); distanceScale = math.dot(surfaceNormal, contactHeader.Normal); contactHeader.Normal = surfaceNormal; } } } } contactPoint.Distance *= distanceScale; } }
// Create a single point manifold between a triangle and sphere in world space. public static unsafe void TriangleSphere( PolygonCollider *triangleA, SphereCollider *sphereB, MTransform worldFromA, MTransform aFromB, float maxDistance, out Manifold manifold) { Assert.IsTrue(triangleA->Vertices.Length == 3); DistanceQueries.Result convexDistance = DistanceQueries.TriangleSphere( triangleA->Vertices[0], triangleA->Vertices[1], triangleA->Vertices[2], triangleA->Planes[0].Normal, sphereB->Center, sphereB->Radius, aFromB); if (convexDistance.Distance < maxDistance) { manifold = new Manifold(convexDistance, worldFromA); } else { manifold = new Manifold(); } }
// Create contact points for a box and triangle in world space. public static unsafe void BoxTriangle( BoxCollider *boxA, PolygonCollider *triangleB, MTransform worldFromA, MTransform aFromB, float maxDistance, out Manifold manifold) { Assert.IsTrue(triangleB->Vertices.Length == 3); // Get triangle in box space MTransform aFromBoxA = new MTransform(boxA->Orientation, boxA->Center); MTransform boxAFromB = Mul(Inverse(aFromBoxA), aFromB); float3 t0 = Mul(boxAFromB, triangleB->ConvexHull.Vertices[0]); float3 t1 = Mul(boxAFromB, triangleB->ConvexHull.Vertices[1]); float3 t2 = Mul(boxAFromB, triangleB->ConvexHull.Vertices[2]); Plane triPlane = triangleB->ConvexHull.Planes[0]; float3 triangleNormal = math.mul(boxAFromB.Rotation, triPlane.Normal); FourTransposedPoints vertsB; FourTransposedPoints edgesB; FourTransposedPoints perpsB; CalcTrianglePlanes(t0, t1, t2, triangleNormal, out vertsB, out edgesB, out perpsB); float3 halfExtents = boxA->Size * 0.5f + maxDistance; // find the closest minkowski plane float4 plane; { // Box face vs triangle vertex float4 planeFaceVertex; { // get aabb of minkowski diff float3 tMin = math.min(math.min(t0, t1), t2) - halfExtents; float3 tMax = math.max(math.max(t0, t1), t2) + halfExtents; // find the aabb face closest to the origin float3 axis0 = new float3(1, 0, 0); float3 axis1 = axis0.zxy; // 010 float3 axis2 = axis0.yzx; // 001 float4 planeX = SelectMaxW(new float4(axis0, -tMax.x), new float4(-axis0, tMin.x)); float4 planeY = SelectMaxW(new float4(axis1, -tMax.y), new float4(-axis1, tMin.y)); float4 planeZ = SelectMaxW(new float4(axis2, -tMax.z), new float4(-axis2, tMin.z)); planeFaceVertex = SelectMaxW(planeX, planeY); planeFaceVertex = SelectMaxW(planeFaceVertex, planeZ); } // Box vertex vs triangle face float4 planeVertexFace; { // Calculate the triangle normal float triangleOffset = math.dot(triangleNormal, t0); float expansionOffset = math.dot(math.abs(triangleNormal), halfExtents); planeVertexFace = SelectMaxW( new float4(triangleNormal, -triangleOffset - expansionOffset), new float4(-triangleNormal, triangleOffset - expansionOffset)); } // Edge planes float4 planeEdgeEdge = new float4(0, 0, 0, -float.MaxValue); { // Test the planes from crossing axis i with each edge of the triangle, for example if i = 1 then n0 is from (0, 1, 0) x (t1 - t0). for (int i = 0, j = 1, k = 2; i < 3; j = k, k = i, i++) { // Normalize the cross product and flip it to point outward from the edge float4 lengthsSq = edgesB.GetComponent(j) * edgesB.GetComponent(j) + edgesB.GetComponent(k) * edgesB.GetComponent(k); float4 invLengths = math.rsqrt(lengthsSq); float4 dots = edgesB.GetComponent(j) * perpsB.GetComponent(k) - edgesB.GetComponent(k) * perpsB.GetComponent(j); float4 factors = invLengths * math.sign(dots); float4 nj = -edgesB.GetComponent(k) * factors; float4 nk = edgesB.GetComponent(j) * factors; float4 distances = -nj *vertsB.GetComponent(j) - nk * vertsB.GetComponent(k) - math.abs(nj) * halfExtents[j] - math.abs(nk) * halfExtents[k]; // If the box edge is parallel to the triangle face then skip it, the plane is redundant with a vertex-face plane bool4 valid = dots != float4.zero; distances = math.select(Constants.Min4F, distances, valid); float3 n0 = new float3(); n0[i] = 0.0f; n0[j] = nj[0]; n0[k] = nk[0]; float3 n1 = new float3(); n1[i] = 0.0f; n1[j] = nj[1]; n1[k] = nk[1]; float3 n2 = new float3(); n2[i] = 0.0f; n2[j] = nj[2]; n2[k] = nk[2]; float4 temp = SelectMaxW(SelectMaxW(new float4(n0, distances.x), new float4(n1, distances.y)), new float4(n2, distances.z)); planeEdgeEdge = SelectMaxW(planeEdgeEdge, temp); } } plane = SelectMaxW(SelectMaxW(planeFaceVertex, planeVertexFace), planeEdgeEdge); } manifold = new Manifold(); // Check for a separating plane TODO.ma could early out as soon as any plane with w>0 is found if (plane.w <= 0.0f) { // Get the normal and supporting faces float3 normalInA = math.mul(boxA->Orientation, plane.xyz); manifold.Normal = math.mul(worldFromA.Rotation, normalInA); int faceIndexA = boxA->ConvexHull.GetSupportingFace(-normalInA); int faceIndexB = triangleB->ConvexHull.GetSupportingFace(math.mul(math.transpose(aFromB.Rotation), normalInA)); // Build manifold if (!FaceFace(ref boxA->ConvexHull, ref triangleB->ConvexHull, faceIndexA, faceIndexB, worldFromA, aFromB, normalInA, float.MaxValue, ref manifold)) { // The closest points are vertices, we need GJK to find them ConvexConvex( ref ((ConvexCollider *)boxA)->ConvexHull, ref ((ConvexCollider *)triangleB)->ConvexHull, worldFromA, aFromB, maxDistance, out manifold); } } }
// Dispatch any pair of convex colliders public static unsafe Result ConvexConvex(Collider *convexA, Collider *convexB, MTransform aFromB) { Result result; bool flip = false; switch (convexA->Type) { case ColliderType.Sphere: SphereCollider *sphereA = (SphereCollider *)convexA; switch (convexB->Type) { case ColliderType.Sphere: result = SphereSphere(sphereA, (SphereCollider *)convexB, aFromB); break; case ColliderType.Capsule: CapsuleCollider *capsuleB = (CapsuleCollider *)convexB; result = CapsuleSphere(capsuleB->Vertex0, capsuleB->Vertex1, capsuleB->Radius, sphereA->Center, sphereA->Radius, Inverse(aFromB)); flip = true; break; case ColliderType.Triangle: PolygonCollider *triangleB = (PolygonCollider *)convexB; result = TriangleSphere( triangleB->Vertices[0], triangleB->Vertices[1], triangleB->Vertices[2], triangleB->Planes[0].Normal, sphereA->Center, sphereA->Radius, Inverse(aFromB)); flip = true; break; case ColliderType.Quad: PolygonCollider *quadB = (PolygonCollider *)convexB; result = QuadSphere( quadB->Vertices[0], quadB->Vertices[1], quadB->Vertices[2], quadB->Vertices[3], quadB->Planes[0].Normal, sphereA->Center, sphereA->Radius, Inverse(aFromB)); flip = true; break; case ColliderType.Box: result = BoxSphere((BoxCollider *)convexB, sphereA, Inverse(aFromB)); flip = true; break; case ColliderType.Cylinder: case ColliderType.Convex: result = ConvexConvex(ref sphereA->ConvexHull, ref ((ConvexCollider *)convexB)->ConvexHull, aFromB); break; default: throw new NotImplementedException(); } break; case ColliderType.Capsule: CapsuleCollider *capsuleA = (CapsuleCollider *)convexA; switch (convexB->Type) { case ColliderType.Sphere: SphereCollider *sphereB = (SphereCollider *)convexB; result = CapsuleSphere(capsuleA->Vertex0, capsuleA->Vertex1, capsuleA->Radius, sphereB->Center, sphereB->Radius, aFromB); break; case ColliderType.Capsule: result = CapsuleCapsule(capsuleA, (CapsuleCollider *)convexB, aFromB); break; case ColliderType.Triangle: result = CapsuleTriangle(capsuleA, (PolygonCollider *)convexB, aFromB); break; case ColliderType.Box: case ColliderType.Quad: case ColliderType.Cylinder: case ColliderType.Convex: result = ConvexConvex(ref capsuleA->ConvexHull, ref ((ConvexCollider *)convexB)->ConvexHull, aFromB); break; default: throw new NotImplementedException(); } break; case ColliderType.Triangle: PolygonCollider *triangleA = (PolygonCollider *)convexA; switch (convexB->Type) { case ColliderType.Sphere: SphereCollider *sphereB = (SphereCollider *)convexB; result = TriangleSphere( triangleA->Vertices[0], triangleA->Vertices[1], triangleA->Vertices[2], triangleA->Planes[0].Normal, sphereB->Center, sphereB->Radius, aFromB); break; case ColliderType.Capsule: result = CapsuleTriangle((CapsuleCollider *)convexB, triangleA, Inverse(aFromB)); flip = true; break; case ColliderType.Box: case ColliderType.Triangle: case ColliderType.Quad: case ColliderType.Cylinder: case ColliderType.Convex: result = ConvexConvex(ref triangleA->ConvexHull, ref ((ConvexCollider *)convexB)->ConvexHull, aFromB); break; default: throw new NotImplementedException(); } break; case ColliderType.Box: BoxCollider *boxA = (BoxCollider *)convexA; switch (convexB->Type) { case ColliderType.Sphere: result = BoxSphere(boxA, (SphereCollider *)convexB, aFromB); break; case ColliderType.Capsule: case ColliderType.Box: case ColliderType.Triangle: case ColliderType.Quad: case ColliderType.Cylinder: case ColliderType.Convex: result = ConvexConvex(ref boxA->ConvexHull, ref ((ConvexCollider *)convexB)->ConvexHull, aFromB); break; default: throw new NotImplementedException(); } break; case ColliderType.Quad: case ColliderType.Cylinder: case ColliderType.Convex: result = ConvexConvex(ref ((ConvexCollider *)convexA)->ConvexHull, ref ((ConvexCollider *)convexB)->ConvexHull, aFromB); break; default: throw new NotImplementedException(); } if (flip) { result.PositionOnAinA = Mul(aFromB, result.PositionOnBinA); result.NormalInA = math.mul(aFromB.Rotation, -result.NormalInA); } return(result); }
public static unsafe Result CapsuleTriangle(CapsuleCollider *capsuleA, PolygonCollider *triangleB, MTransform aFromB) { // Get vertices float3 c0 = capsuleA->Vertex0; float3 c1 = capsuleA->Vertex1; float3 t0 = Mul(aFromB, triangleB->ConvexHull.Vertices[0]); float3 t1 = Mul(aFromB, triangleB->ConvexHull.Vertices[1]); float3 t2 = Mul(aFromB, triangleB->ConvexHull.Vertices[2]); float3 direction; float distanceSq; float3 pointCapsule; float sign = 1.0f; // negated if penetrating { // Calculate triangle edges and edge planes float3 faceNormal = math.mul(aFromB.Rotation, triangleB->ConvexHull.Planes[0].Normal); FourTransposedPoints vertsB; FourTransposedPoints edgesB; FourTransposedPoints perpsB; CalcTrianglePlanes(t0, t1, t2, faceNormal, out vertsB, out edgesB, out perpsB); // c0 against triangle face { FourTransposedPoints rels = new FourTransposedPoints(c0) - vertsB; PointTriangleFace(c0, t0, faceNormal, vertsB, edgesB, perpsB, rels, out float signedDistance); distanceSq = signedDistance * signedDistance; if (distanceSq > distanceEpsSq) { direction = -faceNormal * signedDistance; } else { direction = math.select(faceNormal, -faceNormal, math.dot(c1 - c0, faceNormal) < 0); // rare case, capsule point is exactly on the triangle face } pointCapsule = c0; } // c1 against triangle face { FourTransposedPoints rels = new FourTransposedPoints(c1) - vertsB; PointTriangleFace(c1, t0, faceNormal, vertsB, edgesB, perpsB, rels, out float signedDistance); float distanceSq1 = signedDistance * signedDistance; float3 direction1; if (distanceSq1 > distanceEpsSq) { direction1 = -faceNormal * signedDistance; } else { direction1 = math.select(faceNormal, -faceNormal, math.dot(c0 - c1, faceNormal) < 0); // rare case, capsule point is exactly on the triangle face } SelectMin(ref direction, ref distanceSq, ref pointCapsule, direction1, distanceSq1, c1); } // axis against triangle edges float3 axis = c1 - c0; for (int i = 0; i < 3; i++) { float3 closestOnCapsule, closestOnTriangle; SegmentSegment(c0, axis, vertsB.GetPoint(i), edgesB.GetPoint(i), out closestOnCapsule, out closestOnTriangle); float3 edgeDiff = closestOnCapsule - closestOnTriangle; float edgeDistanceSq = math.lengthsq(edgeDiff); edgeDiff = math.select(edgeDiff, perpsB.GetPoint(i), edgeDistanceSq < distanceEpsSq); // use edge plane if the capsule axis intersects the edge SelectMin(ref direction, ref distanceSq, ref pointCapsule, edgeDiff, edgeDistanceSq, closestOnCapsule); } // axis against triangle face { // Find the intersection of the axis with the triangle plane float axisDot = math.dot(axis, faceNormal); float dist0 = math.dot(t0 - c0, faceNormal); // distance from c0 to the plane along the normal float t = dist0 * math.select(math.rcp(axisDot), 0.0f, axisDot == 0.0f); if (t > 0.0f && t < 1.0f) { // If they intersect, check if the intersection is inside the triangle FourTransposedPoints rels = new FourTransposedPoints(c0 + axis * t) - vertsB; float4 dots = perpsB.Dot(rels); if (math.all(dots <= float4.zero)) { // Axis intersects the triangle, choose the separating direction float dist1 = axisDot - dist0; bool use1 = math.abs(dist1) < math.abs(dist0); float dist = math.select(-dist0, dist1, use1); float3 closestOnCapsule = math.select(c0, c1, use1); SelectMin(ref direction, ref distanceSq, ref pointCapsule, dist * faceNormal, dist * dist, closestOnCapsule); // Even if the edge is closer than the face, we now know that the edge hit was penetrating sign = -1.0f; } } } } float invDistance = math.rsqrt(distanceSq); float distance; float3 normal; if (distanceSq < distanceEpsSq) { normal = math.normalize(direction); // rare case, capsule axis almost exactly touches the triangle distance = 0.0f; } else { normal = direction * invDistance * sign; // common case, distanceSq = lengthsq(direction) distance = distanceSq * invDistance * sign; } return(new Result { NormalInA = normal, PositionOnAinA = pointCapsule - normal * capsuleA->Radius, Distance = distance - capsuleA->Radius }); }