/// <summary>
        /// Populates a list with transformed face vertices.
        /// </summary>
        public static unsafe void ComputeFaceClippingPolygon(ref NativeList <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 int CreateFaceContact2(ref NativeManifold output, int referenceFaceIndex, RigidTransform transform1, NativeHull hull1, RigidTransform transform2, NativeHull hull2)
        {
            // todo, clean this up, i'm reversing the args here so that closest face is the one clipped instead of incident.

            Debug.Assert(output.IsCreated);

            NativePlane referencePlane    = transform1 * hull1.GetPlane(referenceFaceIndex);
            var         incidentFaceIndex = ComputeIncidentFaceIndex(referencePlane, transform2, hull2);

            return(CreateFaceContact(ref output, incidentFaceIndex, transform2, hull2, referenceFaceIndex, transform1, hull1));
        }
        /// <summary>
        /// Finds the index to the least parallel face on the other hull.
        /// </summary>
        /// <param name="facePlane"></param>
        /// <param name="transform"></param>
        /// <param name="hull"></param>
        /// <returns></returns>
        public static unsafe int ComputeIncidentFaceIndex(NativePlane facePlane, RigidTransform transform, NativeHull hull)
        {
            int   faceIndex = 0;
            float min       = math.dot(facePlane.Normal, (transform * hull.GetPlane(faceIndex)).Normal);

            for (int i = 1; i < hull.FaceCount; ++i)
            {
                float dot = math.dot(facePlane.Normal, (transform * hull.GetPlane(i)).Normal);
                if (dot < min)
                {
                    min       = dot;
                    faceIndex = i;
                }
            }
            return(faceIndex);
        }
        public static int CreateFaceContact(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;

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

            GetClippingPlanes(ref clippingPlanes, referencePlane, referenceFaceIndex, transform1, hull1);

            // Create face polygon.
            NativeList <ClipVertex> incidentPolygon = new NativeList <ClipVertex>((int)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)
            {
                NativeList <ClipVertex> outputPolygon = new NativeList <ClipVertex>((int)hull1.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);
        }
Пример #5
0
        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;
                }
            }
        }
 /// <summary>
 /// Populates a list with transformed face planes
 /// </summary>
 public static unsafe void GetClippingPlanes(ref NativeList <ClipPlane> output, NativePlane facePlane, int faceIndex, RigidTransform transform, NativeHull hull)
 {
     Debug.Assert(output.IsCreated);
     for (int i = 0; i < hull.FaceCount; i++)
     {
         var p = hull.GetPlane(i);
         output.Add(new ClipPlane
         {
             plane = transform * p,
         });
     }
 }