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); }