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); }
public void Add(ContactPoint cp) { Points.Add(cp); }