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