private static void ClipFace(ref NativeManifold tmp, int i, RigidTransform transform1, NativeHull hull1, RigidTransform transform2, NativeHull hull2)
{
NativePlane plane = transform1 * hull1.GetPlane(i);
var incidentFaceIndex = ComputeIncidentFaceIndex(plane, transform2, hull2);
ClipFaceAgainstAnother(ref tmp, incidentFaceIndex, transform2, hull2, i, transform1, hull1);
}
/// <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);
/// <summary>
/// Populates a list with transformed face vertices.
/// </summary>
public static unsafe void ComputeFaceClippingPolygon(ref NativeBuffer <ClipVertex> output, int faceIndex, RigidTransform t, NativeHull hull)
{
Debug.Assert(output.IsCreated);
NativeFace * face = hull.GetFacePtr(faceIndex);
NativePlane plane = hull.GetPlane(faceIndex);
NativeHalfEdge *start = hull.GetEdgePtr(face->Edge);
NativeHalfEdge *current = start;
do
{
NativeHalfEdge *twin = hull.GetEdgePtr(current->Twin);
float3 vertex = hull.GetVertex(current->Origin);
float3 P = math.transform(t, vertex);
ClipVertex clipVertex;
clipVertex.featurePair.InEdge1 = -1;
clipVertex.featurePair.OutEdge1 = -1;
clipVertex.featurePair.InEdge2 = (sbyte)current->Next;
clipVertex.featurePair.OutEdge2 = (sbyte)twin->Twin;
clipVertex.position = P;
clipVertex.hull2local = vertex;
clipVertex.plane = plane;
output.Add(clipVertex);
current = hull.GetEdgePtr(current->Next);
} while (current != start);
}
/// <summary>
/// Determines if a world point is contained within a hull
/// </summary>
public static bool Contains(RigidTransform t, NativeHull hull, float3 point)
{
float maxDistance = -float.MaxValue;
for (int i = 0; i < hull.FaceCount; ++i)
{
NativePlane plane = t * hull.GetPlane(i);
float d = plane.Distance(point);
if (d > maxDistance)
{
maxDistance = d;
}
}
return(maxDistance < 0);
}
/// <summary>
/// Finds the index to the least parallel face on the other hull.
/// </summary>
/// <param name="facePlane"></param>
/// <param name="transform"></param>
/// <param name="hull"></param>
/// <returns></returns>
public static unsafe int ComputeIncidentFaceIndex(NativePlane facePlane, RigidTransform transform, NativeHull hull)
{
int faceIndex = 0;
float min = math.dot(facePlane.Normal, (transform * hull.GetPlane(faceIndex)).Normal);
for (int i = 1; i < hull.FaceCount; ++i)
{
float dot = math.dot(facePlane.Normal, (transform * hull.GetPlane(i)).Normal);
if (dot < min)
{
min = dot;
faceIndex = i;
}
}
return(faceIndex);
}
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 static unsafe void QueryFaceDistance(out FaceQueryResult result, RigidTransform transform1, NativeHull hull1, RigidTransform transform2, NativeHull hull2)
{
// Perform computations in the local space of the second hull.
RigidTransform transform = math.mul(math.inverse(transform2), transform1);
result.Distance = -float.MaxValue;
result.Index = -1;
for (int i = 0; i < hull1.FaceCount; ++i)
{
NativePlane plane = transform * hull1.GetPlane(i);
float3 support = hull2.GetSupport(-plane.Normal);
float distance = plane.Distance(support);
if (distance > result.Distance)
{
result.Distance = distance;
result.Index = i;
}
}
}
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 GetFaceSidePlanes(ref NativeBuffer <ClipPlane> output, NativePlane facePlane, int faceIndex, RigidTransform transform, NativeHull hull)
{
NativeHalfEdge *start = hull.GetEdgePtr(hull.GetFacePtr(faceIndex)->Edge);
NativeHalfEdge *current = start;
do
{
NativeHalfEdge *twin = hull.GetEdgePtr(current->Twin);
float3 P = math.transform(transform, hull.GetVertex(current->Origin));
float3 Q = math.transform(transform, hull.GetVertex(twin->Origin));
ClipPlane clipPlane = default;
clipPlane.edgeId = twin->Twin; //edge ID.
clipPlane.plane.Normal = math.normalize(math.cross(Q - P, facePlane.Normal));
clipPlane.plane.Offset = math.dot(clipPlane.plane.Normal, P);
output.Add(clipPlane);
current = hull.GetEdgePtr(current->Next);
}while (current != start);
}
/// <summary>
/// Populates a list with transformed face planes
/// </summary>
public static unsafe void GetClippingPlanes(ref NativeBuffer <ClipPlane> output, NativePlane facePlane, int faceIndex, RigidTransform transform, NativeHull hull)
{
Debug.Assert(output.IsCreated);
for (int i = 0; i < hull.FaceCount; i++)
{
var p = hull.GetPlane(i);
output.Add(new ClipPlane
{
plane = transform * p,
});
}
}
