public static void AppendCapsule(CapsuleCollider *capsule, RigidTransform worldFromCollider, ref List <DisplayResult> results) { float r = capsule->Radius; results.Add(new DisplayResult { Mesh = ReferenceSphere, Scale = new Vector4(r * 2.0f, r * 2.0f, r * 2.0f), Position = math.transform(worldFromCollider, capsule->Vertex0), Orientation = worldFromCollider.rot }); results.Add(new DisplayResult { Mesh = ReferenceSphere, Scale = new Vector4(r * 2.0f, r * 2.0f, r * 2.0f), Position = math.transform(worldFromCollider, capsule->Vertex1), Orientation = worldFromCollider.rot }); results.Add(new DisplayResult { Mesh = ReferenceCylinder, Scale = new Vector4(r * 2.0f, math.length(capsule->Vertex1 - capsule->Vertex0) * 0.5f, r * 2.0f), Position = math.transform(worldFromCollider, (capsule->Vertex0 + capsule->Vertex1) * 0.5f), Orientation = math.mul(worldFromCollider.rot, Quaternion.FromToRotation(new float3(0, 1, 0), math.normalizesafe(capsule->Vertex1 - capsule->Vertex0))) }); }
// 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(); } }
// Create a contact point for a pair of capsules in world space. public static unsafe void CapsuleCapsule( CapsuleCollider *capsuleA, CapsuleCollider *capsuleB, MTransform worldFromA, MTransform aFromB, float maxDistance, out Manifold manifold) { // TODO: Should produce a multi-point manifold DistanceQueries.Result convexDistance = DistanceQueries.CapsuleCapsule(capsuleA, capsuleB, aFromB); if (convexDistance.Distance < maxDistance) { manifold = new Manifold(convexDistance, worldFromA); } else { manifold = new Manifold(); } }
// Create a single point manifold between a capsule and a sphere in world space. public static unsafe void CapsuleSphere( CapsuleCollider *capsuleA, SphereCollider *sphereB, MTransform worldFromA, MTransform aFromB, float maxDistance, out Manifold manifold) { DistanceQueries.Result convexDistance = DistanceQueries.CapsuleSphere( capsuleA->Vertex0, capsuleA->Vertex1, capsuleA->Radius, sphereB->Center, sphereB->Radius, aFromB); if (convexDistance.Distance < maxDistance) { manifold = new Manifold(convexDistance, worldFromA); } else { manifold = new Manifold(); } }
public static unsafe Result CapsuleCapsule(CapsuleCollider *capsuleA, CapsuleCollider *capsuleB, MTransform aFromB) { // Transform capsule B into A-space float3 pointB = Mul(aFromB, capsuleB->Vertex0); float3 edgeB = math.mul(aFromB.Rotation, capsuleB->Vertex1 - capsuleB->Vertex0); // Get point and edge of A float3 pointA = capsuleA->Vertex0; float3 edgeA = capsuleA->Vertex1 - capsuleA->Vertex0; // Get the closest points on the capsules SegmentSegment(pointA, edgeA, pointB, edgeB, out float3 closestA, out float3 closestB); float3 diff = closestA - closestB; float coreDistanceSq = math.lengthsq(diff); if (coreDistanceSq < distanceEpsSq) { // Special case for extremely small distances, should be rare float3 normal = math.cross(edgeA, edgeB); if (math.lengthsq(normal) < 1e-5f) { float3 edge = math.normalizesafe(edgeA, math.normalizesafe(edgeB, new float3(1, 0, 0))); // edges are parallel or one of the capsules is a sphere Math.CalculatePerpendicularNormalized(edge, out normal, out float3 unused); // normal is anything perpendicular to edge } else { normal = math.normalize(normal); // normal is cross of edges, sign doesn't matter } return(new Result { NormalInA = normal, PositionOnAinA = pointA - normal * capsuleA->Radius, Distance = -capsuleA->Radius - capsuleB->Radius }); } return(PointPoint(closestA, closestB, capsuleA->Radius, capsuleA->Radius + capsuleB->Radius)); }
// 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 }); }