/// <summary>
        /// Populates a list with transformed face vertices.
        /// </summary>
        public static unsafe void ComputeFaceClippingPolygon(ref NativeBuffer <ClipVertex> output, int faceIndex, RigidTransform t, NativeHull hull)
        {
            Debug.Assert(output.IsCreated);

            NativeFace *    face    = hull.GetFacePtr(faceIndex);
            NativePlane     plane   = hull.GetPlane(faceIndex);
            NativeHalfEdge *start   = hull.GetEdgePtr(face->Edge);
            NativeHalfEdge *current = start;

            do
            {
                NativeHalfEdge *twin   = hull.GetEdgePtr(current->Twin);
                float3          vertex = hull.GetVertex(current->Origin);
                float3          P      = math.transform(t, vertex);

                ClipVertex clipVertex;
                clipVertex.featurePair.InEdge1  = -1;
                clipVertex.featurePair.OutEdge1 = -1;
                clipVertex.featurePair.InEdge2  = (sbyte)current->Next;
                clipVertex.featurePair.OutEdge2 = (sbyte)twin->Twin;
                clipVertex.position             = P;
                clipVertex.hull2local           = vertex;
                clipVertex.plane = plane;

                output.Add(clipVertex);

                current = hull.GetEdgePtr(current->Next);
            } while (current != start);
        }
        public static unsafe void QueryEdgeDistance(out EdgeQueryResult 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);

            float3 C1 = transform.pos;

            result.Distance = -float.MaxValue;
            result.Index1   = -1;
            result.Index2   = -1;

            for (int i = 0; i < hull1.EdgeCount; i += 2)
            {
                NativeHalfEdge *edge1 = hull1.GetEdgePtr(i);
                NativeHalfEdge *twin1 = hull1.GetEdgePtr(i + 1);

                Debug.Assert(edge1->Twin == i + 1 && twin1->Twin == i);

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

                float3 U1 = math.rotate(transform, hull1.GetPlane(edge1->Face).Normal);
                float3 V1 = math.rotate(transform, hull1.GetPlane(twin1->Face).Normal);

                for (int j = 0; j < hull2.EdgeCount; j += 2)
                {
                    NativeHalfEdge *edge2 = hull2.GetEdgePtr(j);
                    NativeHalfEdge *twin2 = hull2.GetEdgePtr(j + 1);

                    Debug.Assert(edge2->Twin == j + 1 && twin2->Twin == j);

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

                    float3 U2 = hull2.GetPlane(edge2->Face).Normal;
                    float3 V2 = hull2.GetPlane(twin2->Face).Normal;

                    if (IsMinkowskiFace(U1, V1, -E1, -U2, -V2, -E2))
                    {
                        float distance = Project(P1, E1, P2, E2, C1);
                        if (distance > result.Distance)
                        {
                            result.Index1   = i;
                            result.Index2   = j;
                            result.Distance = distance;
                        }
                    }
                }
            }
        }
        public static unsafe void GetFaceSidePlanes(ref NativeBuffer <ClipPlane> output, NativePlane facePlane, int faceIndex, RigidTransform transform, NativeHull hull)
        {
            NativeHalfEdge *start   = hull.GetEdgePtr(hull.GetFacePtr(faceIndex)->Edge);
            NativeHalfEdge *current = start;

            do
            {
                NativeHalfEdge *twin = hull.GetEdgePtr(current->Twin);
                float3          P    = math.transform(transform, hull.GetVertex(current->Origin));
                float3          Q    = math.transform(transform, hull.GetVertex(twin->Origin));

                ClipPlane clipPlane = default;
                clipPlane.edgeId       = twin->Twin; //edge ID.
                clipPlane.plane.Normal = math.normalize(math.cross(Q - P, facePlane.Normal));
                clipPlane.plane.Offset = math.dot(clipPlane.plane.Normal, P);
                output.Add(clipPlane);

                current = hull.GetEdgePtr(current->Next);
            }while (current != start);
        }
        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);
        }