/// <summary>
/// Finds the point on the surface of a hull closest to a world point.
/// </summary>
public static float3 ClosestPoint(RigidTransform t, NativeHull hull, float3 point)
{
float distance = -float.MaxValue;
int closestFaceIndex = -1;
NativePlane closestPlane = default;
// Find the closest face plane.
for (int i = 0; i < hull.FaceCount; ++i)
{
NativePlane plane = t * hull.GetPlane(i);
float d = plane.Distance(point);
if (d > distance)
{
distance = d;
closestFaceIndex = i;
closestPlane = plane;
}
}
var closestPlanePoint = closestPlane.ClosestPoint(point);
if (distance > 0)
{
// Use a point along the closest edge if the plane point would be outside the face bounds.
ref NativeFace face = ref hull.GetFaceRef(closestFaceIndex);
ref NativeHalfEdge start = ref hull.GetEdgeRef(face.Edge);
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();
}