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