/// <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 static CollisionInfo GetDebugCollisionInfo(RigidTransform transform1, NativeHull hull1, RigidTransform transform2, NativeHull hull2)
{
CollisionInfo result = default;
QueryFaceDistance(out result.Face1, transform1, hull1, transform2, hull2);
QueryFaceDistance(out result.Face2, transform2, hull2, transform1, hull1);
QueryEdgeDistance(out result.Edge, transform1, hull1, transform2, hull2);
result.IsColliding = result.Face1.Distance < 0 && result.Face2.Distance < 0 && result.Edge.Distance < 0;
return(result);
}
/// <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);
}
public static void DrawBasicHull(NativeHull hull1, RigidTransform t, Color?color = null, int duration = 1)
{
if (!hull1.IsValid)
{
throw new ArgumentException("Hull is not valid", nameof(hull1));
}
if (!color.HasValue)
{
color = UnityColors.Blue;
}
foreach (var edge in hull1.GetEdges())
{
var a = math.transform(t, hull1.GetVertex(edge.Origin));
var b = math.transform(t, hull1.GetVertex(hull1.GetEdge(edge.Twin).Origin));
Debug.DrawLine(a, b, color.Value);
}
}
public static bool DrawNativeHullHullIntersection(RigidTransform transform1, NativeHull hull1, RigidTransform transform2, NativeHull hull2)
{
for (int i = 0; i < hull2.FaceCount; i++)
{
var tmp = new NativeManifold(Allocator.Temp);
ClipFace(ref tmp, i, transform2, hull2, transform1, hull1);
HullDrawingUtility.DebugDrawManifold(tmp);
tmp.Dispose();
}
for (int i = 0; i < hull1.FaceCount; i++)
{
var tmp = new NativeManifold(Allocator.Temp);
ClipFace(ref tmp, i, transform1, hull1, transform2, hull2);
HullDrawingUtility.DebugDrawManifold(tmp);
tmp.Dispose();
}
return(true);
}
public static bool IsColliding(RigidTransform transform1, NativeHull hull1, RigidTransform transform2, NativeHull hull2)
{
FaceQueryResult faceQuery;
QueryFaceDistance(out faceQuery, transform1, hull1, transform2, hull2);
if (faceQuery.Distance > 0)
{
return(false);
}
QueryFaceDistance(out faceQuery, transform2, hull2, transform1, hull1);
if (faceQuery.Distance > 0)
{
return(false);
}
QueryEdgeDistance(out EdgeQueryResult edgeQuery, transform1, hull1, transform2, hull2);
if (edgeQuery.Distance > 0)
{
return(false);
}
return(true);
}
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;
}
}
}
}
}
public static void DrawDebugHull(NativeHull hull, RigidTransform t, DebugHullFlags options = DebugHullFlags.All, Color BaseColor = default)
{
if (!hull.IsValid)
{
throw new ArgumentException("Hull is not valid", nameof(hull));
}
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)
{
for (int i = 0; i < hull.FaceCount; i++)
{
var centroid = math.transform(t, hull.CalculateFaceCentroid(hull.GetFace(i)));
var normal = math.rotate(t, hull.GetPlane(i).Normal);
DebugDrawer.DrawArrow(centroid, normal * 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.DrawArrow(center + tPlane.Normal * faceExplosionDistance, dir * 0.2f, Color.black);
}
else
{
DebugDrawer.DrawArrow(center + tPlane.Normal * faceExplosionDistance + insetDir * 0.1f, dir * 0.2f, Color.black);
}
edge = nextEdge;
} while (edge.Origin != startOrigin);
}
}
}
public EdgeEnumerator(NativeHull hull, int faceIndex) : this()
{
_hull = hull;
_offset = hull.GetFace(faceIndex).Edge;
_currentIndex = -1;
}
public unsafe static void SetFromFaces(ref NativeHull hull, NativeHullDef def)
{
Debug.Assert(def.FaceCount > 0);
Debug.Assert(def.VertexCount > 0);
hull.VertexCount = def.VertexCount;
var arr = def.VerticesNative.ToArray();
hull.VerticesNative = new NativeArrayNoLeakDetection <float3>(arr, Allocator.Persistent);
hull.Vertices = (float3 *)hull.VerticesNative.GetUnsafePtr();
hull.FaceCount = def.FaceCount;
hull.FacesNative = new NativeArrayNoLeakDetection <NativeFace>(hull.FaceCount, Allocator.Persistent);
hull.Faces = (NativeFace *)hull.FacesNative.GetUnsafePtr();
// Initialize all faces by assigning -1 to each edge reference.
for (int k = 0; k < def.FaceCount; ++k)
{
NativeFace *f = hull.Faces + k;
f->Edge = -1;
}
CreateFacesPlanes(ref hull, ref def);
var edgeMap = new Dictionary <(int v1, int v2), int>();
var edgesList = new NativeHalfEdge[10000]; // todo lol
// Loop through all faces.
for (int i = 0; i < def.FaceCount; ++i)
{
NativeFaceDef face = def.FacesNative[i];
int vertCount = face.VertexCount;
Debug.Assert(vertCount >= 3);
int *vertices = face.Vertices;
var faceHalfEdges = new List <int>();
// Loop through all face edges.
for (int j = 0; j < vertCount; ++j)
{
int v1 = vertices[j];
int v2 = j + 1 < vertCount ? vertices[j + 1] : vertices[0];
bool edgeFound12 = edgeMap.TryGetValue((v1, v2), out int iter12);
bool edgeFound21 = edgeMap.ContainsKey((v2, v1));
Debug.Assert(edgeFound12 == edgeFound21);
if (edgeFound12)
{
// The edge is shared by two faces.
int e12 = iter12;
// Link adjacent face to edge.
if (edgesList[e12].Face == -1)
{
edgesList[e12].Face = i;
}
else
{
throw new Exception("Two shared edges can't have the same vertices in the same order");
}
if (hull.Faces[i].Edge == -1)
{
hull.Faces[i].Edge = e12;
}
faceHalfEdges.Add(e12);
}
else
{
// The next edge of the current half edge in the array is the twin edge.
int e12 = hull.EdgeCount++;
int e21 = hull.EdgeCount++;
if (hull.Faces[i].Edge == -1)
{
hull.Faces[i].Edge = e12;
}
faceHalfEdges.Add(e12);
edgesList[e12].Prev = -1;
edgesList[e12].Next = -1;
edgesList[e12].Twin = e21;
edgesList[e12].Face = i;
edgesList[e12].Origin = v1;
edgesList[e21].Prev = -1;
edgesList[e21].Next = -1;
edgesList[e21].Twin = e12;
edgesList[e21].Face = -1;
edgesList[e21].Origin = v2;
// Add edges to map.
edgeMap[(v1, v2)] = e12;
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 bool NativeHullHullContact(ref NativeManifold result, RigidTransform transform1, NativeHull hull1, RigidTransform transform2, NativeHull hull2)
{
FaceQueryResult faceQuery1;
HullCollision.QueryFaceDistance(out faceQuery1, transform1, hull1, transform2, hull2);
if (faceQuery1.Distance > 0)
{
return(false);
}
FaceQueryResult faceQuery2;
HullCollision.QueryFaceDistance(out faceQuery2, transform2, hull2, transform1, hull1);
if (faceQuery2.Distance > 0)
{
return(false);
}
HullCollision.QueryEdgeDistance(out EdgeQueryResult edgeQuery, transform1, hull1, transform2, hull2);
if (edgeQuery.Distance > 0)
{
return(false);
}
float kRelEdgeTolerance = 0.90f; //90%
float kRelFaceTolerance = 0.95f; //95%
float kAbsTolerance = 0.5f * 0.005f;
// Favor face contacts over edge contacts.
float maxFaceSeparation = math.max(faceQuery1.Distance, faceQuery2.Distance);
if (edgeQuery.Distance > kRelEdgeTolerance * maxFaceSeparation + kAbsTolerance)
{
CreateEdgeContact(ref result, edgeQuery, transform1, hull1, transform2, hull2);
}
else
{
// Favor first hull face to avoid face flip-flops.
if (faceQuery2.Distance > kRelFaceTolerance * faceQuery1.Distance + kAbsTolerance)
{
// 2 = reference, 1 = incident.
CreateFaceContact(ref result, faceQuery2, transform2, hull2, transform1, hull1, true);
}
else
{
// 1 = reference, 2 = incident.
CreateFaceContact(ref result, faceQuery1, transform1, hull1, transform2, hull2, false);
}
}
return(true);
}
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);
}
/// <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);
}
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,
});
}
}
/// <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);
}
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);
}
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 NativeHull CreateFromMesh(Mesh mesh)
{
var faces = new List <DetailedFaceDef>();
var verts = mesh.vertices.Select(RoundVertex).ToArray();
var uniqueVerts = verts.Distinct().ToList();
var indices = mesh.triangles;
// Create faces from Triangles and collapse multiple vertices with same position into shared vertices.
for (int i = 0; i < mesh.triangles.Length; i = i + 3)
{
var idx1 = i;
var idx2 = i + 1;
var idx3 = i + 2;
Vector3 p1 = verts[indices[idx1]];
Vector3 p2 = verts[indices[idx2]];
Vector3 p3 = verts[indices[idx3]];
var normal = math.normalize(math.cross(p3 - p2, p1 - p2));
// Round normal so that faces with only slight variances can be grouped properly together.
var roundedNormal = RoundVertex(normal);
faces.Add(new DetailedFaceDef
{
Center = ((p1 + p2 + p3) / 3),
Normal = roundedNormal,
Verts = new List <float3> {
p1, p2, p3
},
Indices = new List <int>
{
uniqueVerts.IndexOf(p1),
uniqueVerts.IndexOf(p2),
uniqueVerts.IndexOf(p3)
}
});
}
var faceDefs = new List <NativeFaceDef>();
var orphanIndices = new HashSet <int>();
// Merge all faces with the same normal and shared vertex
var mergedFaces = GroupBySharedVertex(GroupByNormal(faces));
foreach (var faceGroup in mergedFaces)
{
var indicesFromMergedFaces = faceGroup.SelectMany(face => face.Indices).ToArray();
// Collapse points inside the new combined face by using only the border vertices.
var border = PolygonPerimeter.CalculatePerimeter(indicesFromMergedFaces, ref uniqueVerts);
var borderIndices = border.Select(b => b.EndIndex).ToArray();
foreach (var idx in indicesFromMergedFaces.Except(borderIndices))
{
orphanIndices.Add(idx);
}
var v = stackalloc int[borderIndices.Length];
int max = 0;
for (int i = 0; i < borderIndices.Length; i++)
{
var idx = borderIndices[i];
if (idx > max)
{
max = idx;
}
v[i] = idx;
}
faceDefs.Add(new NativeFaceDef
{
HighestIndex = max,
VertexCount = borderIndices.Length,
Vertices = v,
});
}
// Remove vertices with no edges connected to them and fix all impacted face vertex references.
foreach (var orphanIdx in orphanIndices.OrderByDescending(i => i))
{
uniqueVerts.RemoveAt(orphanIdx);
foreach (var face in faceDefs.Where(f => f.HighestIndex >= orphanIdx))
{
for (int i = 0; i < face.VertexCount; i++)
{
var faceVertIdx = face.Vertices[i];
if (faceVertIdx >= orphanIdx)
{
face.Vertices[i] = --faceVertIdx;
}
}
}
}
var result = new NativeHull();
using (var faceNative = new NativeArray <NativeFaceDef>(faceDefs.ToArray(), Allocator.Temp))
using (var vertsNative = new NativeArray <float3>(uniqueVerts.ToArray(), Allocator.Temp))
{
NativeHullDef hullDef;
hullDef.VertexCount = vertsNative.Length;
hullDef.VerticesNative = vertsNative;
hullDef.FaceCount = faceNative.Length;
hullDef.FacesNative = faceNative;
SetFromFaces(ref result, hullDef);
}
result.IsCreated = true;
HullValidation.ValidateHull(result);
return(result);
}
public static void DrawEdge(int i, RigidTransform t1, NativeHull hull1, Color?color = null)
{
if (i > 0 && i < hull1.EdgeCount - 1)
{
ref var localEdge = ref hull1.GetEdgeRef(i);
ref var twinEdge = ref hull1.GetEdgeRef(localEdge.Twin);
public static unsafe NativeHull CreateBox(float3 scale)
{
float3[] cubeVertices =
{
new float3(0.5f, 0.5f, -0.5f),
new float3(-0.5f, 0.5f, -0.5f),
new float3(-0.5f, -0.5f, -0.5f),
new float3(0.5f, -0.5f, -0.5f),
new float3(0.5f, 0.5f, 0.5f),
new float3(-0.5f, 0.5f, 0.5f),
new float3(-0.5f, -0.5f, 0.5f),
new float3(0.5f, -0.5f, 0.5f),
};
for (int i = 0; i < 8; ++i)
{
cubeVertices[i].x *= scale.x;
cubeVertices[i].y *= scale.y;
cubeVertices[i].z *= scale.z;
}
int *left = stackalloc int[] { 1, 2, 6, 5 };
int *right = stackalloc int[] { 4, 7, 3, 0 };
int *down = stackalloc int[] { 3, 7, 6, 2 };
int *up = stackalloc int[] { 0, 1, 5, 4 };
int *back = stackalloc int[] { 4, 5, 6, 7 };
int *front = stackalloc int[] { 0, 3, 2, 1 };
NativeFaceDef[] boxFaces =
{
new NativeFaceDef {
VertexCount = 4, Vertices = left
},
new NativeFaceDef {
VertexCount = 4, Vertices = right
},
new NativeFaceDef {
VertexCount = 4, Vertices = down
},
new NativeFaceDef {
VertexCount = 4, Vertices = up
},
new NativeFaceDef {
VertexCount = 4, Vertices = back
},
new NativeFaceDef {
VertexCount = 4, Vertices = front
},
};
var result = new NativeHull();
using (var boxFacesNative = new NativeArray <NativeFaceDef>(boxFaces, Allocator.Temp))
using (var cubeVertsNative = new NativeArray <float3>(cubeVertices, Allocator.Temp))
{
NativeHullDef hullDef;
hullDef.VertexCount = 8;
hullDef.VerticesNative = cubeVertsNative;
hullDef.FaceCount = 6;
hullDef.FacesNative = boxFacesNative;
SetFromFaces(ref result, hullDef);
}
result.IsCreated = true;
return(result);
}
public EdgeEnumerator(NativeHull hull) : this()
{
_hull = hull;
_offset = -1;
_currentIndex = -1;
}
