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(); }
/// <summary> /// Perform the Sutherland-Hodgman polygon clipping. Since all side planes are pointing outwards the points that are *behind* the plane are kept. /// </summary> public static void Clip(ClipPlane clipPlane, ref NativeBuffer <ClipVertex> input, ref NativeBuffer <ClipVertex> output) { //Debug.Assert(output.IsCreated && output.Length == 0); Debug.Assert(input.IsCreated && input.Length != 0); ClipVertex vertex1 = input[input.Length - 1]; float distance1 = clipPlane.plane.Distance(vertex1.position); for (int i = 0; i < input.Length; ++i) { ClipVertex vertex2 = input[i]; float distance2 = clipPlane.plane.Distance(vertex2.position); if (distance1 <= 0 && distance2 <= 0) { // Both vertices are behind or lying on the plane -> keep vertex2 output.Add(vertex2); } else if (distance1 <= 0 && distance2 > 0) { // vertex1 is behind the plane, vertex2 is in front -> intersection point float fraction = distance1 / (distance1 - distance2); float3 position = vertex1.position + fraction * (vertex2.position - vertex1.position); // Keep intersection point ClipVertex vertex; vertex.position = position; vertex.featurePair.InEdge1 = -1; vertex.featurePair.InEdge2 = vertex1.featurePair.OutEdge2; vertex.featurePair.OutEdge1 = (sbyte)clipPlane.edgeId; vertex.featurePair.OutEdge2 = -1; vertex.plane = clipPlane.plane; vertex.hull2local = position; output.Add(vertex); } else if (distance2 <= 0 && distance1 > 0) { // vertex2 is behind of the plane, vertex1 is in front -> intersection point float fraction = distance1 / (distance1 - distance2); float3 position = vertex1.position + fraction * (vertex2.position - vertex1.position); // Keep intersection point ClipVertex vertex; vertex.position = position; vertex.featurePair.InEdge1 = (sbyte)clipPlane.edgeId; vertex.featurePair.OutEdge1 = -1; vertex.featurePair.InEdge2 = -1; vertex.featurePair.OutEdge2 = vertex1.featurePair.OutEdge2; vertex.plane = clipPlane.plane; vertex.hull2local = position; output.Add(vertex); // Keep vertex2 as well output.Add(vertex2); } // Keep vertex2 as starting vertex for next edge vertex1 = vertex2; distance1 = distance2; } }