public static int ClipFaceAgainstAnother(ref NativeManifold output, int referenceFaceIndex, RigidTransform transform1, NativeHull hull1, int incidentFaceIndex, RigidTransform transform2, NativeHull hull2)
        {
            Debug.Assert(output.IsCreated);

            var         refPlane       = hull1.GetPlane(referenceFaceIndex);
            NativePlane referencePlane = transform1 * refPlane;

            NativeBuffer <ClipPlane> clippingPlanes = new NativeBuffer <ClipPlane>(hull1.FaceCount, Allocator.Temp);

            // Get every plane on the other polygon
            GetClippingPlanes(ref clippingPlanes, referencePlane, referenceFaceIndex, transform1, hull1);

            // Create face polygon.
            NativeBuffer <ClipVertex> incidentPolygon = new NativeBuffer <ClipVertex>(hull1.VertexCount, Allocator.Temp);

            ComputeFaceClippingPolygon(ref incidentPolygon, incidentFaceIndex, transform2, hull2);

            // Clip face polygon against the clipping planes.
            for (int i = 0; i < clippingPlanes.Length; ++i)
            {
                NativeBuffer <ClipVertex> outputPolygon = new NativeBuffer <ClipVertex>(math.max(hull1.VertexCount, hull2.VertexCount), Allocator.Temp);

                Clip(clippingPlanes[i], ref incidentPolygon, ref outputPolygon);

                if (outputPolygon.Length == 0)
                {
                    return(-1);
                }

                incidentPolygon.Dispose();
                incidentPolygon = outputPolygon;
            }

            for (int i = 0; i < incidentPolygon.Length; ++i)
            {
                ClipVertex vertex   = incidentPolygon[i];
                float      distance = referencePlane.Distance(vertex.position);
                output.Add(vertex.position, distance, new ContactID {
                    FeaturePair = vertex.featurePair
                });
            }

            clippingPlanes.Dispose();
            incidentPolygon.Dispose();

            return(incidentFaceIndex);
        }
        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();
        }
        public static unsafe void CreateEdgeContact(ref NativeManifold output, EdgeQueryResult input, RigidTransform transform1, NativeHull hull1, RigidTransform transform2, NativeHull hull2)
        {
            Debug.Assert(output.IsCreated);

            ContactPoint cp = default;

            if (input.Index1 < 0 || input.Index2 < 0)
            {
                return;
            }

            NativeHalfEdge *edge1 = hull1.GetEdgePtr(input.Index1);
            NativeHalfEdge *twin1 = hull1.GetEdgePtr(edge1->Twin);

            float3 P1 = math.transform(transform1, hull1.GetVertex(edge1->Origin));
            float3 Q1 = math.transform(transform1, hull1.GetVertex(twin1->Origin));
            float3 E1 = Q1 - P1;

            NativeHalfEdge *edge2 = hull2.GetEdgePtr(input.Index2);
            NativeHalfEdge *twin2 = hull2.GetEdgePtr(edge2->Twin);

            float3 P2 = math.transform(transform1, hull2.GetVertex(edge2->Origin));
            float3 Q2 = math.transform(transform1, hull2.GetVertex(twin2->Origin));
            float3 E2 = Q2 - P2;

            float3 normal = math.normalize(math.cross(Q1 - P1, Q2 - P2));
            float3 C2C1   = transform2.pos - transform1.pos;

            if (math.dot(normal, C2C1) < 0)
            {
                // Flip
                output.Normal              = -normal;
                cp.Id.FeaturePair.InEdge1  = input.Index2;
                cp.Id.FeaturePair.OutEdge1 = input.Index2 + 1;

                cp.Id.FeaturePair.InEdge2  = input.Index1 + 1;
                cp.Id.FeaturePair.OutEdge2 = input.Index1;
            }
            else
            {
                output.Normal = normal;

                cp.Id.FeaturePair.InEdge1  = input.Index1;
                cp.Id.FeaturePair.OutEdge1 = input.Index1 + 1;

                cp.Id.FeaturePair.InEdge2  = input.Index2 + 1;
                cp.Id.FeaturePair.OutEdge2 = input.Index2;
            }

            // Compute the closest points between the two edges (center point of penetration)
            ClosestPointsSegmentSegment(P1, Q1, P2, Q2, out float3 C1, out float3 C2);

            float3 position = 0.5f * (C1 + C2);

            //// the closest points on each hull
            //cp.positionOnTarget = Math3d.ProjectPointOnLineSegment(P2, Q2, C2);
            //cp.positionOnSource = Math3d.ProjectPointOnLineSegment(P1, Q1, C1);

            cp.Penetration = C1 - C2;
            cp.Position    = position;
            cp.Distance    = input.Distance;

            output.Add(cp);
        }