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 b3CreateEdgeContact(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 = (sbyte)input.Index2; cp.Id.FeaturePair.OutEdge1 = (sbyte)(input.Index2 + 1); cp.Id.FeaturePair.InEdge2 = (sbyte)(input.Index1 + 1); cp.Id.FeaturePair.OutEdge2 = (sbyte)input.Index1; } else { output.Normal = normal; cp.Id.FeaturePair.InEdge1 = (sbyte)input.Index1; cp.Id.FeaturePair.OutEdge1 = (sbyte)(input.Index1 + 1); cp.Id.FeaturePair.InEdge2 = (sbyte)(input.Index2 + 1); cp.Id.FeaturePair.OutEdge2 = (sbyte)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); }