public static unsafe void QueryFaceDistance(out FaceQueryResult result, RigidTransform transform1, NativeHull hull1, RigidTransform transform2, NativeHull hull2) { // Perform computations in the local space of the second hull. RigidTransform transform = math.mul(math.inverse(transform2), transform1); result.Distance = -float.MaxValue; result.Index = -1; for (int i = 0; i < hull1.FaceCount; ++i) { NativePlane plane = transform * hull1.GetPlane(i); float3 support = hull2.GetSupport(-plane.Normal); float distance = plane.Distance(support); if (distance > result.Distance) { result.Distance = distance; result.Index = i; } } }
public unsafe static void CreateFaceContact(ref NativeManifold output, FaceQueryResult input, RigidTransform transform1, NativeHull hull1, RigidTransform transform2, NativeHull hull2, bool flipNormal) { var refPlane = hull1.GetPlane(input.Index); NativePlane referencePlane = transform1 * refPlane; var clippingPlanesStackPtr = stackalloc ClipPlane[hull1.FaceCount]; var clippingPlanes = new NativeBuffer <ClipPlane>(clippingPlanesStackPtr, hull1.FaceCount); //NativeList<ClipPlane> clippingPlanes = new NativeList<ClipPlane>((int)hull1.FaceCount, Allocator.Temp); // Find only the side planes of the reference face GetFaceSidePlanes(ref clippingPlanes, referencePlane, input.Index, transform1, hull1); var incidentPolygonStackPtr = stackalloc ClipVertex[hull1.FaceCount]; var incidentPolygon = new NativeBuffer <ClipVertex>(incidentPolygonStackPtr, hull1.VertexCount); var incidentFaceIndex = ComputeIncidentFaceIndex(referencePlane, transform2, hull2); ComputeFaceClippingPolygon(ref incidentPolygon, incidentFaceIndex, transform2, hull2); //HullDrawingUtility.DrawFaceWithOutline(incidentFaceIndex, transform2, hull2, Color.yellow.ToOpacity(0.3f)); var outputPolygonStackPtr = stackalloc ClipVertex[hull1.FaceCount]; // Clip face polygon against the clipping planes. for (int i = 0; i < clippingPlanes.Length; ++i) { var outputPolygon = new NativeBuffer <ClipVertex>(outputPolygonStackPtr, hull1.FaceCount); Clip(clippingPlanes[i], ref incidentPolygon, ref outputPolygon); if (outputPolygon.Length == 0) { return; } incidentPolygon = outputPolygon; } // Get all contact points below reference face. for (int i = 0; i < incidentPolygon.Length; ++i) { ClipVertex vertex = incidentPolygon[i]; float distance = referencePlane.Distance(vertex.position); if (distance <= 0) { // Below reference plane -> position constraint violated. ContactID id = default; id.FeaturePair = vertex.featurePair; if (flipNormal) { output.Normal = -referencePlane.Normal; Swap(id.FeaturePair.InEdge1, id.FeaturePair.InEdge2); Swap(id.FeaturePair.OutEdge1, id.FeaturePair.OutEdge2); } else { output.Normal = referencePlane.Normal; } // Project clipped point onto reference plane. float3 position = referencePlane.ClosestPoint(vertex.position); // Add point and distance to the plane to the manifold. output.Add(position, distance, id); } } //clippingPlanes.Dispose(); //incidentPolygon.Dispose(); }