public void AddRandomPoints(int count)
{
for (int i = 0; i < count; ++i)
{
var point = Prng.NextFloat3(0, 1);
if (Distribution == DistributionType.Sphere)
{
point = math.normalize(point);
}
Hull.AddPoint(point);
}
UpdateMesh = true;
}
public void Execute()
{
// Add points to the builder
ConvexHullBuilder builder = Builder[0];
for (int i = 0; i < Points.Length; i++)
{
builder.AddPoint(Points[i]);
}
builder.BuildFaceIndices();
// Write back the builder
Builder[0] = builder;
}
public void BuildConvexHull2D()
{
// Build circle.
using (var hull = new ConvexHullBuilder(8192, 8192 * 2))
{
var expectedCom = new float3(4, 5, 3);
for (int n = 1024, i = 0; i < n; ++i)
{
var angle = (float)(i / n * 2 * System.Math.PI);
hull.AddPoint(new float3(math.cos(angle), math.sin(angle), 0));
}
var massProperties = hull.ComputeMassProperties();
Debug.Log($"COM: {massProperties.CenterOfMass}");
Debug.Log($"Area: {massProperties.SurfaceArea}");
}
}
//
// Reference implementations of queries using simple brute-force methods
//
static unsafe float RefConvexConvexDistance(ref ConvexHull a, ref ConvexHull b, MTransform aFromB)
{
bool success = false;
if (a.NumVertices + b.NumVertices < 64) // too slow without burst
{
// Build the minkowski difference in a-space
int maxNumVertices = a.NumVertices * b.NumVertices;
Aabb aabb = Aabb.Empty;
for (int iB = 0; iB < b.NumVertices; iB++)
{
float3 vertexB = Math.Mul(aFromB, b.Vertices[iB]);
for (int iA = 0; iA < a.NumVertices; iA++)
{
float3 vertexA = a.Vertices[iA];
aabb.Include(vertexA - vertexB);
}
}
ConvexHullBuilderStorage diffStorage = new ConvexHullBuilderStorage(maxNumVertices, Allocator.Temp, aabb, 0.0f, ConvexHullBuilder.IntResolution.Low);
ref ConvexHullBuilder diff = ref diffStorage.Builder;
success = true;
for (int iB = 0; iB < b.NumVertices; iB++)
{
float3 vertexB = Math.Mul(aFromB, b.Vertices[iB]);
for (int iA = 0; iA < a.NumVertices; iA++)
{
float3 vertexA = a.Vertices[iA];
diff.AddPoint(vertexA - vertexB, (uint)(iA | iB << 16));
}
}
float distance = 0.0f;
if (success && diff.Dimension == 3)
{
// Find the closest triangle to the origin
distance = float.MaxValue;
bool penetrating = true;
foreach (int t in diff.Triangles.Indices)
{
ConvexHullBuilder.Triangle triangle = diff.Triangles[t];
float3 v0 = diff.Vertices[triangle.GetVertex(0)].Position;
float3 v1 = diff.Vertices[triangle.GetVertex(1)].Position;
float3 v2 = diff.Vertices[triangle.GetVertex(2)].Position;
float3 n = diff.ComputePlane(t).Normal;
DistanceQueries.Result result = DistanceQueries.TriangleSphere(v0, v1, v2, n, float3.zero, 0.0f, MTransform.Identity);
if (result.Distance < distance)
{
distance = result.Distance;
}
penetrating = penetrating & (math.dot(n, -result.NormalInA) < 0.0f); // only penetrating if inside of all planes
}
if (penetrating)
{
distance = -distance;
}
distance -= a.ConvexRadius + b.ConvexRadius;
}
else
{
success = false;
}
diffStorage.Dispose();
if (success)
{
return(distance);
}
}
// followed by variable sized convex hull data
#region Construction
// Create a convex collider from the given point cloud.
public static unsafe BlobAssetReference <Collider> Create(
NativeArray <float3> points, float convexRadius,
float3?scale = null, CollisionFilter?filter = null, Material?material = null)
{
if (convexRadius < 0.0f || !math.isfinite(convexRadius))
{
throw new ArgumentException("Tried to create ConvexCollider with invalid convex radius");
}
// Build convex hull
int verticesCapacity = points.Length;
int triangleCapacity = 2 * verticesCapacity;
var vertices = (ConvexHullBuilder.Vertex *)UnsafeUtility.Malloc(verticesCapacity * sizeof(ConvexHullBuilder.Vertex), 16, Allocator.Temp);
var triangles = (ConvexHullBuilder.Triangle *)UnsafeUtility.Malloc(triangleCapacity * sizeof(ConvexHullBuilder.Triangle), 16, Allocator.Temp);
var builder = new ConvexHullBuilder(vertices, verticesCapacity, triangles, triangleCapacity);
float3 s = scale ?? new float3(1);
// Build the points' AABB and validate them
var domain = new Aabb();
foreach (var point in points)
{
if (math.any(!math.isfinite(point)))
{
throw new ArgumentException("Tried to create ConvexCollider with invalid points");
}
domain.Include(point * s);
}
// Add points to the hull
builder.IntegerSpaceAabb = domain;
foreach (float3 point in points)
{
builder.AddPoint(point * s);
}
// TODO: shrink by convex radius
// Build face information
float maxAngle = 0.1f * (float)math.PI / 180.0f;
builder.BuildFaceIndices(maxAngle);
// Simplify the hull until it's under the max vertices requirement
// TODO.ma this is just a failsafe. We need to think about user-controlled simplification settings & how to warn the user if their shape is too complex.
{
const int maxVertices = 252; // as per Havok
float maxSimplificationError = 1e-3f;
int iterations = 0;
while (builder.Vertices.PeakCount > maxVertices)
{
if (iterations++ > 10) // don't loop forever
{
Assert.IsTrue(false);
return(new BlobAssetReference <Collider>());
}
builder.SimplifyVertices(maxSimplificationError);
builder.BuildFaceIndices();
maxSimplificationError *= 2.0f;
}
}
// Convert hull to compact format
var tempHull = new TempHull(ref builder);
// Allocate collider
int totalSize = UnsafeUtility.SizeOf <ConvexCollider>();
totalSize += tempHull.Vertices.Count * sizeof(float3);
totalSize = Math.NextMultipleOf16(totalSize); // planes currently must be aligned for Havok
totalSize += tempHull.Planes.Count * sizeof(Plane);
totalSize += tempHull.Faces.Count * sizeof(ConvexHull.Face);
totalSize += tempHull.FaceVertexIndices.Count * sizeof(short);
totalSize += tempHull.VertexEdges.Count * sizeof(ConvexHull.Edge);
totalSize += tempHull.FaceLinks.Count * sizeof(ConvexHull.Edge);
ConvexCollider *collider = (ConvexCollider *)UnsafeUtility.Malloc(totalSize, 16, Allocator.Temp);
// Initialize it
{
UnsafeUtility.MemClear(collider, totalSize);
collider->MemorySize = totalSize;
collider->m_Header.Type = ColliderType.Convex;
collider->m_Header.CollisionType = CollisionType.Convex;
collider->m_Header.Version = 0;
collider->m_Header.Magic = 0xff;
collider->m_Header.Filter = filter ?? CollisionFilter.Default;
collider->m_Header.Material = material ?? Material.Default;
ref var hull = ref collider->ConvexHull;
hull.ConvexRadius = convexRadius;
// Initialize blob arrays
{
byte *end = (byte *)collider + UnsafeUtility.SizeOf <ConvexCollider>();
hull.VerticesBlob.Offset = (int)(end - (byte *)UnsafeUtility.AddressOf(ref hull.VerticesBlob.Offset));
hull.VerticesBlob.Length = tempHull.Vertices.Count;
end += sizeof(float3) * tempHull.Vertices.Count;
end = (byte *)Math.NextMultipleOf16((ulong)end); // planes currently must be aligned for Havok
hull.FacePlanesBlob.Offset = (int)(end - (byte *)UnsafeUtility.AddressOf(ref hull.FacePlanesBlob.Offset));
hull.FacePlanesBlob.Length = tempHull.Planes.Count;
end += sizeof(Plane) * tempHull.Planes.Count;
hull.FacesBlob.Offset = (int)(end - (byte *)UnsafeUtility.AddressOf(ref hull.FacesBlob.Offset));
hull.FacesBlob.Length = tempHull.Faces.Count;
end += sizeof(ConvexHull.Face) * tempHull.Faces.Count;
hull.FaceVertexIndicesBlob.Offset = (int)(end - (byte *)UnsafeUtility.AddressOf(ref hull.FaceVertexIndicesBlob.Offset));
hull.FaceVertexIndicesBlob.Length = tempHull.FaceVertexIndices.Count;
end += sizeof(byte) * tempHull.FaceVertexIndices.Count;
hull.VertexEdgesBlob.Offset = (int)(end - (byte *)UnsafeUtility.AddressOf(ref hull.VertexEdgesBlob.Offset));
hull.VertexEdgesBlob.Length = tempHull.VertexEdges.Count;
end += sizeof(ConvexHull.Edge) * tempHull.VertexEdges.Count;
hull.FaceLinksBlob.Offset = (int)(end - (byte *)UnsafeUtility.AddressOf(ref hull.FaceLinksBlob.Offset));
hull.FaceLinksBlob.Length = tempHull.FaceLinks.Count;
end += sizeof(ConvexHull.Edge) * tempHull.FaceLinks.Count;
}
// Fill blob arrays
{
for (int i = 0; i < tempHull.Vertices.Count; i++)
{
hull.Vertices[i] = tempHull.Vertices[i];
hull.VertexEdges[i] = tempHull.VertexEdges[i];
}
for (int i = 0; i < tempHull.Faces.Count; i++)
{
hull.Planes[i] = tempHull.Planes[i];
hull.Faces[i] = tempHull.Faces[i];
}
for (int i = 0; i < tempHull.FaceVertexIndices.Count; i++)
{
hull.FaceVertexIndices[i] = tempHull.FaceVertexIndices[i];
hull.FaceLinks[i] = tempHull.FaceLinks[i];
}
}
// Fill mass properties
{
var massProperties = builder.ComputeMassProperties();
Math.DiagonalizeSymmetricApproximation(massProperties.InertiaTensor, out float3x3 orientation, out float3 inertia);
float maxLengthSquared = 0.0f;
foreach (float3 vertex in hull.Vertices)
{
maxLengthSquared = math.max(maxLengthSquared, math.lengthsq(vertex - massProperties.CenterOfMass));
}
collider->MassProperties = new MassProperties
{
MassDistribution = new MassDistribution
{
Transform = new RigidTransform(orientation, massProperties.CenterOfMass),
InertiaTensor = inertia
},
Volume = massProperties.Volume,
AngularExpansionFactor = math.sqrt(maxLengthSquared)
};
}
}
void OnDrawGizmos()
{
{
var s = 0.25f;
var com = MassProperties.CenterOfMass;
Gizmos.color = Color.white;
Gizmos.DrawLine(transform.TransformPoint(com + new float3(-s, 0, 0)), transform.TransformPoint(com + new float3(+s, 0, 0)));
Gizmos.DrawLine(transform.TransformPoint(com + new float3(0, -s, 0)), transform.TransformPoint(com + new float3(0, +s, 0)));
Gizmos.DrawLine(transform.TransformPoint(com + new float3(0, 0, -s)), transform.TransformPoint(com + new float3(0, 0, +s)));
}
if (UpdateMesh)
{
UpdateMesh = false;
UpdateMeshNow();
}
// Display faces.
if (ShowFaces && HullData.Faces != null)
{
MTransform trs = new MTransform(transform.rotation, transform.position);
Gizmos.color = Color.white;
foreach (Face face in HullData.Faces)
{
var offset = face.Plane.Normal * 0.0001f;
for (int i = face.NumVertices - 1, j = 0; j < face.NumVertices; i = j++)
{
var a = Hull.Vertices[HullData.FaceVertices[face.FirstVertex + i]].Position;
var b = Hull.Vertices[HullData.FaceVertices[face.FirstVertex + j]].Position;
a = Mul(trs, a + offset);
b = Mul(trs, b + offset);
Gizmos.DrawLine(a, b);
}
}
}
// Display triangles.
if (ShowTriangles)
{
MTransform trs = new MTransform(transform.rotation, transform.position);
Gizmos.color = Color.white;
for (int i = Hull.Triangles.GetFirstIndex(); i != -1; i = Hull.Triangles.GetNextIndex(i))
{
var a = Mul(trs, Hull.Vertices[Hull.Triangles[i].Vertex0].Position);
var b = Mul(trs, Hull.Vertices[Hull.Triangles[i].Vertex1].Position);
var c = Mul(trs, Hull.Vertices[Hull.Triangles[i].Vertex2].Position);
Gizmos.DrawLine(a, b);
Gizmos.DrawLine(b, c);
Gizmos.DrawLine(c, a);
}
}
// Display vertex normals.
if (ShowVertexNormals)
{
MTransform trs = new MTransform(transform.rotation, transform.position);
Gizmos.color = Color.white;
for (var vertex = Hull.Triangles.GetFirstIndex(); vertex != -1; vertex = Hull.Triangles.GetNextIndex(vertex))
{
var normal = math.mul(trs.Rotation, Hull.ComputeVertexNormal(vertex)) * 0.25f;
var start = Mul(trs, Hull.Vertices[vertex].Position);
Gizmos.DrawRay(start, normal);
}
}
// Display labels.
if (ShowLabels)
{
}
// Compute distance to every other convex hulls.
if (CollideOthers)
{
var thisId = GetInstanceID();
var cvxs = FindObjectsOfType <ConvexConvexDistanceTest>();
MTransform transformA = new MTransform(transform.rotation, transform.position);
float3[] verticesA = GetVertexArray();
foreach (var cvx in cvxs)
{
if (cvx.GetInstanceID() == thisId)
{
continue;
}
MTransform transformB = new MTransform(cvx.transform.rotation, cvx.transform.position);
float3[] verticesB = cvx.GetVertexArray();
MTransform btoA = Mul(Inverse(transformA), transformB);
ConvexConvexDistanceQueries.Result result;
fixed(float3 *va = verticesA)
{
fixed(float3 *vb = verticesB)
{
result = ConvexConvexDistanceQueries.ConvexConvex(va, verticesA.Length, vb, verticesB.Length, btoA, PenetrationHandling);
}
}
var from = Mul(transformA, result.ClosestPoints.PositionOnAinA);
var to = Mul(transformA, result.ClosestPoints.PositionOnBinA);
if (TraceQueryResults)
{
Debug.Log($"Iterations={result.Iterations}, plane={result.ClosestPoints.NormalInA}, distance={result.ClosestPoints.Distance}");
Debug.Log($"Features A = [{result.Simplex[0] >> 16}, {result.Simplex[1] >> 16}, {result.Simplex[2] >> 16}]");
Debug.Log($"Features B = [{result.Simplex[0] & 0xffff}, {result.Simplex[1] & 0xffff}, {result.Simplex[2] & 0xffff}]");
}
if (ShowManifold && Hull.Dimension == 3 && cvx.Hull.Dimension == 3)
{
DrawManifold(Experimental, result,
transformA, ref Hull, ref HullData,
transformB, ref cvx.Hull, ref cvx.HullData);
}
if (ShowProjection)
{
Gizmos.color = Color.white;
var trs = Mul(transformA, new MTransform(float3x3.identity, result.ClosestPoints.NormalInA * result.ClosestPoints.Distance));
{
var tv = stackalloc float3[Hull.Vertices.PeakCount];
for (var vertex = Hull.Vertices.GetFirstIndex(); vertex != -1; vertex = Hull.Vertices.GetNextIndex(vertex))
{
tv[vertex] = Mul(trs, Hull.Vertices[vertex].Position);
}
if (Hull.Dimension == 3)
{
for (var edge = Hull.GetFirstPrimaryEdge(); edge.IsValid; edge = Hull.GetNextPrimaryEdge(edge))
{
Gizmos.DrawLine(tv[Hull.StartVertex(edge)], tv[Hull.EndVertex(edge)]);
}
}
else if (Hull.Dimension >= 1)
{
for (int i = Hull.Vertices.PeakCount - 1, j = 0; j < Hull.Vertices.PeakCount; i = j++)
{
Gizmos.DrawLine(tv[i], tv[j]);
}
}
}
}
Gizmos.color = Color.red;
Gizmos.DrawSphere(from, 0.05f);
Gizmos.color = Color.green;
Gizmos.DrawSphere(to, 0.05f);
Gizmos.color = Color.white;
Gizmos.DrawLine(from, to);
if (ShowCso)
{
Gizmos.color = Color.yellow;
using (var cso = new ConvexHullBuilder(8192, 8192 * 2))
{
for (int i = 0; i < verticesA.Length; ++i)
{
for (int j = 0; j < verticesB.Length; ++j)
{
cso.AddPoint(verticesA[i] - Mul(btoA, verticesB[j]));
}
}
if (cso.Dimension == 2)
{
for (int n = cso.Vertices.PeakCount, i = n - 1, j = 0; j < n; i = j++)
{
Gizmos.DrawLine(cso.Vertices[i].Position, cso.Vertices[j].Position);
}
}
else if (cso.Dimension == 3)
{
foreach (var triangle in cso.Triangles.Elements)
{
Gizmos.DrawLine(cso.Vertices[triangle.Vertex0].Position, cso.Vertices[triangle.Vertex1].Position);
Gizmos.DrawLine(cso.Vertices[triangle.Vertex1].Position, cso.Vertices[triangle.Vertex2].Position);
Gizmos.DrawLine(cso.Vertices[triangle.Vertex2].Position, cso.Vertices[triangle.Vertex0].Position);
}
}
Gizmos.DrawLine(new float3(-0.1f, 0, 0), new float3(+0.1f, 0, 0));
Gizmos.DrawLine(new float3(0, -0.1f, 0), new float3(0, +0.1f, 0));
Gizmos.DrawLine(new float3(0, 0, -0.1f), new float3(0, 0, +0.1f));
}
}
}
}
// Draw vertices.
#if UNITY_EDITOR
GUIStyle labelStyle = new GUIStyle();
labelStyle.fontSize = 24;
#endif
Gizmos.color = Color.yellow;
for (int i = Hull.Vertices.GetFirstIndex(); i != -1; i = Hull.Vertices.GetNextIndex(i))
{
var w = transform.TransformPoint(Hull.Vertices[i].Position);
#if UNITY_EDITOR
if (ShowLabels)
{
Handles.color = Color.white;
Handles.Label(w, $"{i}:{Hull.Vertices[i].Cardinality}", labelStyle);
}
else
#endif
{
Gizmos.DrawSphere(w, 0.01f);
}
}
}
//
// Reference implementations of queries using simple brute-force methods
//
static unsafe float RefConvexConvexDistance(ref ConvexHull a, ref ConvexHull b, MTransform aFromB)
{
// Build the minkowski difference in a-space
int maxNumVertices = a.NumVertices * b.NumVertices;
ConvexHullBuilder diff = new ConvexHullBuilder(maxNumVertices, 2 * maxNumVertices, Allocator.Temp);
bool success = true;
Aabb aabb = Aabb.Empty;
for (int iB = 0; iB < b.NumVertices; iB++)
{
float3 vertexB = Math.Mul(aFromB, b.Vertices[iB]);
for (int iA = 0; iA < a.NumVertices; iA++)
{
float3 vertexA = a.Vertices[iA];
aabb.Include(vertexA - vertexB);
}
}
diff.IntegerSpaceAabb = aabb;
for (int iB = 0; iB < b.NumVertices; iB++)
{
float3 vertexB = Math.Mul(aFromB, b.Vertices[iB]);
for (int iA = 0; iA < a.NumVertices; iA++)
{
float3 vertexA = a.Vertices[iA];
if (!diff.AddPoint(vertexA - vertexB, (uint)(iA | iB << 16)))
{
// TODO - coplanar vertices are tripping up ConvexHullBuilder, we should fix it but for now fall back to DistanceQueries.ConvexConvex()
success = false;
}
}
}
float distance;
if (!success || diff.Triangles.GetFirstIndex() == -1)
{
// No triangles unless the difference is 3D, fall back to GJK
// Most of the time this happens for cases like sphere-sphere, capsule-capsule, etc. which have special implementations,
// so comparing those to GJK still validates the results of different API queries against each other.
distance = DistanceQueries.ConvexConvex(ref a, ref b, aFromB).Distance;
}
else
{
// Find the closest triangle to the origin
distance = float.MaxValue;
bool penetrating = true;
for (int t = diff.Triangles.GetFirstIndex(); t != -1; t = diff.Triangles.GetNextIndex(t))
{
ConvexHullBuilder.Triangle triangle = diff.Triangles[t];
float3 v0 = diff.Vertices[triangle.GetVertex(0)].Position;
float3 v1 = diff.Vertices[triangle.GetVertex(1)].Position;
float3 v2 = diff.Vertices[triangle.GetVertex(2)].Position;
float3 n = diff.ComputePlane(t).Normal;
DistanceQueries.Result result = DistanceQueries.TriangleSphere(v0, v1, v2, n, float3.zero, 0.0f, MTransform.Identity);
if (result.Distance < distance)
{
distance = result.Distance;
}
penetrating = penetrating & (math.dot(n, -result.NormalInA) < 0.0f); // only penetrating if inside of all planes
}
if (penetrating)
{
distance = -distance;
}
distance -= a.ConvexRadius + b.ConvexRadius;
}
diff.Dispose();
return(distance);
}
/// <summary>
/// Generalized convex-convex distance.
/// </summary>
/// <param name="verticesA">Vertices of the first collider in local space</param>
/// <param name="verticesB">Vertices of the second collider in local space</param>
/// <param name="aFromB">Transform from the local space of B to the local space of A</param>
/// <param name="penetrationHandling">How to compute penetration.</param>
/// <returns></returns>
public static unsafe Result ConvexConvex(
float3 *verticesA, int numVerticesA, float3 *verticesB, int numVerticesB,
MTransform aFromB, PenetrationHandling penetrationHandling)
{
const float epsTerminationSq = 1e-8f; // Main loop quits when it cannot find a point that improves the simplex by at least this much
const float epsPenetrationSq = 1e-9f; // Epsilon used to check for penetration. Should be smaller than shape cast ConvexConvex keepDistance^2.
// Initialize simplex.
Simplex simplex = new Simplex();
simplex.NumVertices = 1;
simplex.A = GetSupportingVertex(new float3(1, 0, 0), verticesA, numVerticesA, verticesB, numVerticesB, aFromB);
simplex.Direction = simplex.A.Xyz;
simplex.ScaledDistance = math.lengthsq(simplex.A.Xyz);
float scaleSq = simplex.ScaledDistance;
// Iterate.
int iteration = 0;
bool penetration = false;
const int maxIterations = 64;
for (; iteration < maxIterations; ++iteration)
{
// Find a new support vertex
SupportVertex newSv = GetSupportingVertex(-simplex.Direction, verticesA, numVerticesA, verticesB, numVerticesB, aFromB);
// If the new vertex is not significantly closer to the origin, quit
float scaledImprovement = math.dot(simplex.A.Xyz - newSv.Xyz, simplex.Direction);
if (scaledImprovement * math.abs(scaledImprovement) < epsTerminationSq * scaleSq)
{
break;
}
// Add the new vertex and reduce the simplex
switch (simplex.NumVertices++)
{
case 1: simplex.B = newSv; break;
case 2: simplex.C = newSv; break;
default: simplex.D = newSv; break;
}
simplex.SolveDistance();
// Check for penetration
scaleSq = math.lengthsq(simplex.Direction);
float scaledDistanceSq = simplex.ScaledDistance * simplex.ScaledDistance;
if (simplex.NumVertices == 4 || scaledDistanceSq <= epsPenetrationSq * scaleSq)
{
penetration = true;
break;
}
}
// Finalize result.
var ret = new Result {
Iterations = iteration + 1
};
// Handle penetration.
if (penetration && penetrationHandling != PenetrationHandling.DotNotCompute)
{
// Allocate a hull for EPA
int verticesCapacity = 64;
int triangleCapacity = 2 * verticesCapacity;
ConvexHullBuilder.Vertex * vertices = stackalloc ConvexHullBuilder.Vertex[verticesCapacity];
ConvexHullBuilder.Triangle *triangles = stackalloc ConvexHullBuilder.Triangle[triangleCapacity];
var hull = new ConvexHullBuilder(vertices, verticesCapacity, triangles, triangleCapacity);
// Initialize int space
// TODO - either the hull should be robust when int space changes after points are already added, or the ability to do so should be removed, probably the latter
// Currently for example a valid triangle can collapse to a line segment when the bounds grow
hull.IntegerSpaceAabb = GetSupportingAabb(verticesA, numVerticesA, verticesB, numVerticesB, aFromB);
// Add simplex vertices to the hull, remove any vertices from the simplex that do not increase the hull dimension
hull.AddPoint(simplex.A.Xyz, simplex.A.Id);
if (simplex.NumVertices > 1)
{
hull.AddPoint(simplex.B.Xyz, simplex.B.Id);
if (simplex.NumVertices > 2)
{
int dimension = hull.Dimension;
hull.AddPoint(simplex.C.Xyz, simplex.C.Id);
if (dimension == 0 && hull.Dimension == 1)
{
simplex.B = simplex.C;
}
if (simplex.NumVertices > 3)
{
dimension = hull.Dimension;
hull.AddPoint(simplex.D.Xyz, simplex.D.Id);
if (dimension > hull.Dimension)
{
if (dimension == 0)
{
simplex.B = simplex.D;
}
else if (dimension == 1)
{
simplex.C = simplex.D;
}
}
}
}
}
simplex.NumVertices = (hull.Dimension + 1);
// If the simplex is not 3D, try expanding the hull in all directions
while (hull.Dimension < 3)
{
// Choose expansion directions
float3 support0, support1, support2;
switch (simplex.NumVertices)
{
case 1:
support0 = new float3(1, 0, 0);
support1 = new float3(0, 1, 0);
support2 = new float3(0, 0, 1);
break;
case 2:
Math.CalculatePerpendicularNormalized(math.normalize(simplex.B.Xyz - simplex.A.Xyz), out support0, out support1);
support2 = float3.zero;
break;
default:
UnityEngine.Assertions.Assert.IsTrue(simplex.NumVertices == 3);
support0 = math.cross(simplex.B.Xyz - simplex.A.Xyz, simplex.C.Xyz - simplex.A.Xyz);
support1 = float3.zero;
support2 = float3.zero;
break;
}
// Try each one
int numSupports = 4 - simplex.NumVertices;
bool success = false;
for (int i = 0; i < numSupports; i++)
{
for (int j = 0; j < 2; j++) // +/- each direction
{
SupportVertex vertex = GetSupportingVertex(support0, verticesA, numVerticesA, verticesB, numVerticesB, aFromB);
hull.AddPoint(vertex.Xyz, vertex.Id);
if (hull.Dimension == simplex.NumVertices)
{
switch (simplex.NumVertices)
{
case 1: simplex.B = vertex; break;
case 2: simplex.C = vertex; break;
default: simplex.D = vertex; break;
}
// Next dimension
success = true;
simplex.NumVertices++;
i = numSupports;
break;
}
support0 = -support0;
}
support0 = support1;
support1 = support2;
}
if (!success)
{
break;
}
}
// We can still fail to build a tetrahedron if the minkowski difference is really flat.
// In those cases just find the closest point to the origin on the infinite extension of the simplex (point / line / plane)
if (hull.Dimension != 3)
{
switch (simplex.NumVertices)
{
case 1:
{
ret.ClosestPoints.Distance = math.length(simplex.A.Xyz);
ret.ClosestPoints.NormalInA = -math.normalizesafe(simplex.A.Xyz, new float3(1, 0, 0));
break;
}
case 2:
{
float3 edge = math.normalize(simplex.B.Xyz - simplex.A.Xyz);
float3 direction = math.cross(math.cross(edge, simplex.A.Xyz), edge);
Math.CalculatePerpendicularNormalized(edge, out float3 safeNormal, out float3 unused); // backup, take any direction perpendicular to the edge
float3 normal = math.normalizesafe(direction, safeNormal);
ret.ClosestPoints.Distance = math.dot(normal, simplex.A.Xyz);
ret.ClosestPoints.NormalInA = -normal;
break;
}
default:
{
UnityEngine.Assertions.Assert.IsTrue(simplex.NumVertices == 3);
float3 normal = math.normalize(math.cross(simplex.B.Xyz - simplex.A.Xyz, simplex.C.Xyz - simplex.A.Xyz));
float dot = math.dot(normal, simplex.A.Xyz);
ret.ClosestPoints.Distance = math.abs(dot);
ret.ClosestPoints.NormalInA = math.select(-normal, normal, dot < 0);
break;
}
}
}
else
{
int closestTriangleIndex;
Plane closestPlane = new Plane();
float stopThreshold = 1e-4f;
uint *uidsCache = stackalloc uint[triangleCapacity];
for (int i = 0; i < triangleCapacity; i++)
{
uidsCache[i] = 0;
}
float *distancesCache = stackalloc float[triangleCapacity];
do
{
// Select closest triangle.
closestTriangleIndex = -1;
foreach (int triangleIndex in hull.Triangles.Indices)
{
if (hull.Triangles[triangleIndex].Uid != uidsCache[triangleIndex])
{
uidsCache[triangleIndex] = hull.Triangles[triangleIndex].Uid;
distancesCache[triangleIndex] = hull.ComputePlane(triangleIndex).Distance;
}
if (closestTriangleIndex == -1 || distancesCache[closestTriangleIndex] < distancesCache[triangleIndex])
{
closestTriangleIndex = triangleIndex;
}
}
closestPlane = hull.ComputePlane(closestTriangleIndex);
// Add supporting vertex or exit.
SupportVertex sv = GetSupportingVertex(closestPlane.Normal, verticesA, numVerticesA, verticesB, numVerticesB, aFromB);
float d2P = math.dot(closestPlane.Normal, sv.Xyz) + closestPlane.Distance;
if (math.abs(d2P) > stopThreshold && hull.AddPoint(sv.Xyz, sv.Id))
{
stopThreshold *= 1.3f;
}
else
{
break;
}
} while (++iteration < maxIterations);
// Generate simplex.
ConvexHullBuilder.Triangle triangle = hull.Triangles[closestTriangleIndex];
simplex.NumVertices = 3;
simplex.A.Xyz = hull.Vertices[triangle.Vertex0].Position; simplex.A.Id = hull.Vertices[triangle.Vertex0].UserData;
simplex.B.Xyz = hull.Vertices[triangle.Vertex1].Position; simplex.B.Id = hull.Vertices[triangle.Vertex1].UserData;
simplex.C.Xyz = hull.Vertices[triangle.Vertex2].Position; simplex.C.Id = hull.Vertices[triangle.Vertex2].UserData;
simplex.Direction = -closestPlane.Normal;
simplex.ScaledDistance = closestPlane.Distance;
// Set normal and distance.
ret.ClosestPoints.NormalInA = -closestPlane.Normal;
ret.ClosestPoints.Distance = closestPlane.Distance;
}
}
else
{
// Compute distance and normal.
float lengthSq = math.lengthsq(simplex.Direction);
float invLength = math.rsqrt(lengthSq);
bool smallLength = lengthSq < 1e-10f;
ret.ClosestPoints.Distance = math.select(simplex.ScaledDistance * invLength, 0.0f, smallLength);
ret.ClosestPoints.NormalInA = math.select(simplex.Direction * invLength, new float3(1, 0, 0), smallLength);
// Make sure the normal is always valid.
if (!math.all(math.isfinite(ret.ClosestPoints.NormalInA)))
{
ret.ClosestPoints.NormalInA = new float3(1, 0, 0);
}
}
// Compute position.
float3 closestPoint = ret.ClosestPoints.NormalInA * ret.ClosestPoints.Distance;
float4 coordinates = simplex.ComputeBarycentricCoordinates(closestPoint);
ret.ClosestPoints.PositionOnAinA =
verticesA[simplex.A.IdA] * coordinates.x +
verticesA[simplex.B.IdA] * coordinates.y +
verticesA[simplex.C.IdA] * coordinates.z +
verticesA[simplex.D.IdA] * coordinates.w;
// Encode simplex.
ret.Simplex.x = simplex.A.Id;
ret.Simplex.y = simplex.NumVertices >= 2 ? simplex.B.Id : Result.InvalidSimplexVertex;
ret.Simplex.z = simplex.NumVertices >= 3 ? simplex.C.Id : Result.InvalidSimplexVertex;
// Done.
UnityEngine.Assertions.Assert.IsTrue(math.isfinite(ret.ClosestPoints.Distance));
UnityEngine.Assertions.Assert.IsTrue(math.abs(math.lengthsq(ret.ClosestPoints.NormalInA) - 1.0f) < 1e-5f);
return(ret);
}