public static unsafe void QueryEdgeDistance(out EdgeQueryResult 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);
float3 C1 = transform.pos;
result.Distance = -float.MaxValue;
result.Index1 = -1;
result.Index2 = -1;
for (int i = 0; i < hull1.EdgeCount; i += 2)
{
NativeHalfEdge *edge1 = hull1.GetEdgePtr(i);
NativeHalfEdge *twin1 = hull1.GetEdgePtr(i + 1);
Debug.Assert(edge1->Twin == i + 1 && twin1->Twin == i);
float3 P1 = math.transform(transform, hull1.GetVertex(edge1->Origin));
float3 Q1 = math.transform(transform, hull1.GetVertex(twin1->Origin));
float3 E1 = Q1 - P1;
float3 U1 = math.rotate(transform, hull1.GetPlane(edge1->Face).Normal);
float3 V1 = math.rotate(transform, hull1.GetPlane(twin1->Face).Normal);
for (int j = 0; j < hull2.EdgeCount; j += 2)
{
NativeHalfEdge *edge2 = hull2.GetEdgePtr(j);
NativeHalfEdge *twin2 = hull2.GetEdgePtr(j + 1);
Debug.Assert(edge2->Twin == j + 1 && twin2->Twin == j);
float3 P2 = hull2.GetVertex(edge2->Origin);
float3 Q2 = hull2.GetVertex(twin2->Origin);
float3 E2 = Q2 - P2;
float3 U2 = hull2.GetPlane(edge2->Face).Normal;
float3 V2 = hull2.GetPlane(twin2->Face).Normal;
if (IsMinkowskiFace(U1, V1, -E1, -U2, -V2, -E2))
{
float distance = Project(P1, E1, P2, E2, C1);
if (distance > result.Distance)
{
result.Index1 = i;
result.Index2 = j;
result.Distance = distance;
}
}
}
}
}
/// <summary>
/// Populates a list with transformed face vertices.
/// </summary>
public static unsafe void ComputeFaceClippingPolygon(ref NativeList <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>
/// 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 CreateFaceContact2(ref NativeManifold output, int referenceFaceIndex, RigidTransform transform1, NativeHull hull1, RigidTransform transform2, NativeHull hull2)
{
// todo, clean this up, i'm reversing the args here so that closest face is the one clipped instead of incident.
Debug.Assert(output.IsCreated);
NativePlane referencePlane = transform1 * hull1.GetPlane(referenceFaceIndex);
var incidentFaceIndex = ComputeIncidentFaceIndex(referencePlane, transform2, hull2);
return(CreateFaceContact(ref output, incidentFaceIndex, transform2, hull2, referenceFaceIndex, transform1, hull1));
}
/// <summary>
/// Populates a list with transformed face planes
/// </summary>
public static unsafe void GetClippingPlanes(ref NativeList <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,
});
}
}
public static int CreateFaceContact(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;
NativeList <ClipPlane> clippingPlanes = new NativeList <ClipPlane>((int)hull1.FaceCount, Allocator.Temp);
GetClippingPlanes(ref clippingPlanes, referencePlane, referenceFaceIndex, transform1, hull1);
// Create face polygon.
NativeList <ClipVertex> incidentPolygon = new NativeList <ClipVertex>((int)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)
{
NativeList <ClipVertex> outputPolygon = new NativeList <ClipVertex>((int)hull1.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 static void DrawDebugHull(NativeHull hull, RigidTransform t, DebugHullFlags options = DebugHullFlags.All, Color BaseColor = default)
{
if (options == DebugHullFlags.None)
{
return;
}
if (BaseColor == default)
{
BaseColor = Color.yellow;
}
float faceExplosionDistance = (options & DebugHullFlags.ExplodeFaces) != 0 ? 0.3f : 0;
// Iterate each twin pair at the same time.
for (int j = 0; j < hull.edgeCount; j = j + 2)
{
var edge = hull.GetEdge(j);
var twin = hull.GetEdge(j + 1);
var edgePlane = edge.face != -1 ? hull.GetPlane(edge.face) : new NativePlane();
var twinPlane = twin.face != -1 ? hull.GetPlane(twin.face) : new NativePlane();
var rotatedEdgeNormal = math.rotate(t, edgePlane.Normal);
var rotatedTwinNormal = math.rotate(t, twinPlane.Normal);
var edgeVertex1 = math.transform(t, hull.GetVertex(edge.origin));
var twinVertex1 = math.transform(t, hull.GetVertex(twin.origin));
var edgeVertex2 = math.transform(t, hull.GetVertex(edge.origin));
var twinVertex2 = math.transform(t, hull.GetVertex(twin.origin));
if ((options & DebugHullFlags.Outline) != 0)
{
Debug.DrawLine(edgeVertex1 + rotatedEdgeNormal * faceExplosionDistance, twinVertex1 + rotatedEdgeNormal * faceExplosionDistance, BaseColor);
Debug.DrawLine(edgeVertex2 + rotatedTwinNormal * faceExplosionDistance, twinVertex2 + rotatedTwinNormal * faceExplosionDistance, BaseColor);
}
if ((options & DebugHullFlags.EdgeLinks) != 0)
{
Debug.DrawLine((edgeVertex1 + twinVertex1) / 2 + rotatedEdgeNormal * faceExplosionDistance, (edgeVertex2 + twinVertex2) / 2 + rotatedTwinNormal * faceExplosionDistance, Color.gray);
}
}
if ((options & DebugHullFlags.PlaneNormals) != 0)
{
hull.IterateFaces((int index, ref NativePlane plane, ref NativeHalfEdge firstEdge) =>
{
var tPlane = plane.Transform(t);
DebugDrawer.DebugArrow(tPlane.Position, tPlane.Rotation * 0.2f, BaseColor);
});
}
if ((options & DebugHullFlags.Indices) != 0)
{
var dupeCheck = new HashSet <Vector3>();
for (int i = 0; i < hull.vertexCount; i++)
{
// Offset the label if multiple verts are on the same position.
var v = math.transform(t, hull.GetVertex(i));
var offset = dupeCheck.Contains(v) ? (float3)Vector3.forward * 0.05f : 0;
DebugDrawer.DrawLabel(v + offset, i.ToString());
dupeCheck.Add(v);
}
}
if ((options & DebugHullFlags.FaceWinding) != 0)
{
for (int i = 0; i < hull.faceCount; i++)
{
var face = hull.GetFace(i);
var plane = hull.GetPlane(i);
var tPlane = t * plane;
var edge = hull.GetEdge(face.edge);
var startOrigin = edge.origin;
do
{
var nextEdge = hull.GetEdge(edge.next);
var startVert = math.transform(t, hull.GetVertex(edge.origin));
var endVert = math.transform(t, hull.GetVertex(nextEdge.origin));
var center = (endVert + startVert) / 2;
var dir = math.normalize(endVert - startVert);
var insetDir = math.normalize(math.cross(tPlane.Normal, dir));
if ((options & DebugHullFlags.ExplodeFaces) != 0)
{
DebugDrawer.DebugArrow(center + tPlane.Normal * faceExplosionDistance, dir * 0.2f, Color.black);
}
else
{
DebugDrawer.DebugArrow(center + tPlane.Normal * faceExplosionDistance + insetDir * 0.1f, dir * 0.2f, Color.black);
}
edge = nextEdge;
} while (edge.origin != startOrigin);
}
}
}
