/// <summary> /// Finds the point on the surface of a hull closest to a world point. /// </summary> public static float3 ClosestPoint(RigidTransform t, NativeHull hull, float3 point) { float distance = -float.MaxValue; int closestFaceIndex = -1; NativePlane closestPlane = default; // Find the closest face plane. for (int i = 0; i < hull.FaceCount; ++i) { NativePlane plane = t * hull.GetPlane(i); float d = plane.Distance(point); if (d > distance) { distance = d; closestFaceIndex = i; closestPlane = plane; } } var closestPlanePoint = closestPlane.ClosestPoint(point); if (distance > 0) { // Use a point along the closest edge if the plane point would be outside the face bounds. ref NativeFace face = ref hull.GetFaceRef(closestFaceIndex); ref NativeHalfEdge start = ref hull.GetEdgeRef(face.Edge);
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(); }