public static int ClipFaceAgainstAnother(ref NativeManifold output, int referenceFaceIndex, RigidTransform transform1, NativeHull hull1, int incidentFaceIndex, RigidTransform transform2, NativeHull hull2)
{
Debug.Assert(output.IsCreated);
var refPlane = hull1.GetPlane(referenceFaceIndex);
NativePlane referencePlane = transform1 * refPlane;
NativeBuffer <ClipPlane> clippingPlanes = new NativeBuffer <ClipPlane>(hull1.FaceCount, Allocator.Temp);
// Get every plane on the other polygon
GetClippingPlanes(ref clippingPlanes, referencePlane, referenceFaceIndex, transform1, hull1);
// Create face polygon.
NativeBuffer <ClipVertex> incidentPolygon = new NativeBuffer <ClipVertex>(hull1.VertexCount, Allocator.Temp);
ComputeFaceClippingPolygon(ref incidentPolygon, incidentFaceIndex, transform2, hull2);
// Clip face polygon against the clipping planes.
for (int i = 0; i < clippingPlanes.Length; ++i)
{
NativeBuffer <ClipVertex> outputPolygon = new NativeBuffer <ClipVertex>(math.max(hull1.VertexCount, hull2.VertexCount), Allocator.Temp);
Clip(clippingPlanes[i], ref incidentPolygon, ref outputPolygon);
if (outputPolygon.Length == 0)
{
return(-1);
}
incidentPolygon.Dispose();
incidentPolygon = outputPolygon;
}
for (int i = 0; i < incidentPolygon.Length; ++i)
{
ClipVertex vertex = incidentPolygon[i];
float distance = referencePlane.Distance(vertex.position);
output.Add(vertex.position, distance, new ContactID {
FeaturePair = vertex.featurePair
});
}
clippingPlanes.Dispose();
incidentPolygon.Dispose();
return(incidentFaceIndex);
}
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();
}
/// <summary>
/// Perform the Sutherland-Hodgman polygon clipping. Since all side planes are pointing outwards the points that are *behind* the plane are kept.
/// </summary>
public static void Clip(ClipPlane clipPlane, ref NativeBuffer <ClipVertex> input, ref NativeBuffer <ClipVertex> output)
{
//Debug.Assert(output.IsCreated && output.Length == 0);
Debug.Assert(input.IsCreated && input.Length != 0);
ClipVertex vertex1 = input[input.Length - 1];
float distance1 = clipPlane.plane.Distance(vertex1.position);
for (int i = 0; i < input.Length; ++i)
{
ClipVertex vertex2 = input[i];
float distance2 = clipPlane.plane.Distance(vertex2.position);
if (distance1 <= 0 && distance2 <= 0)
{
// Both vertices are behind or lying on the plane -> keep vertex2
output.Add(vertex2);
}
else if (distance1 <= 0 && distance2 > 0)
{
// vertex1 is behind the plane, vertex2 is in front -> intersection point
float fraction = distance1 / (distance1 - distance2);
float3 position = vertex1.position + fraction * (vertex2.position - vertex1.position);
// Keep intersection point
ClipVertex vertex;
vertex.position = position;
vertex.featurePair.InEdge1 = -1;
vertex.featurePair.InEdge2 = vertex1.featurePair.OutEdge2;
vertex.featurePair.OutEdge1 = (sbyte)clipPlane.edgeId;
vertex.featurePair.OutEdge2 = -1;
vertex.plane = clipPlane.plane;
vertex.hull2local = position;
output.Add(vertex);
}
else if (distance2 <= 0 && distance1 > 0)
{
// vertex2 is behind of the plane, vertex1 is in front -> intersection point
float fraction = distance1 / (distance1 - distance2);
float3 position = vertex1.position + fraction * (vertex2.position - vertex1.position);
// Keep intersection point
ClipVertex vertex;
vertex.position = position;
vertex.featurePair.InEdge1 = (sbyte)clipPlane.edgeId;
vertex.featurePair.OutEdge1 = -1;
vertex.featurePair.InEdge2 = -1;
vertex.featurePair.OutEdge2 = vertex1.featurePair.OutEdge2;
vertex.plane = clipPlane.plane;
vertex.hull2local = position;
output.Add(vertex);
// Keep vertex2 as well
output.Add(vertex2);
}
// Keep vertex2 as starting vertex for next edge
vertex1 = vertex2;
distance1 = distance2;
}
}