/// <summary>
/// Returns this Polygon as convex polygon.
///
/// If this polygon is convex, a clone of it is returned.
/// If this polygon is concave, the Graham's (1972) "points to convex hull" enhanched algorythm is used to generate a convex polygon hull.
/// </summary>
/// <returns></returns>
public Polygon2 ToConvexPolygon()
{
var convexhull = ConvexHullBuilder.Convexhull(this.ToVertices().Distinct());
var clone = this.Clone() as Polygon2;
clone.Clear();
clone.AddRange(convexhull.ToVertices());
return(clone);
}
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;
}
static void DrawFace(MTransform trs, ref ConvexHullBuilder hull, Face face, int[] vertices, float offset, Color color)
{
Gizmos.color = color;
var translation = face.Plane.Normal * offset;
for (int i = face.NumVertices - 1, j = 0; j < face.NumVertices; i = j++)
{
var a = hull.Vertices[vertices[face.FirstVertex + i]].Position;
var b = hull.Vertices[vertices[face.FirstVertex + j]].Position;
a = Mul(trs, a + translation);
b = Mul(trs, b + translation);
Gizmos.DrawLine(a, b);
}
}
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}");
}
}
/// <summary>
/// Creates a convex hull mesh for a set of points.
/// </summary>
/// <param name="points">The points.</param>
/// <param name="vertexLimit">
/// The vertex limit. Must be greater than 0. Common values are 32 or 64.
/// </param>
/// <param name="skinWidth">
/// The skin width. Common values are 0.01 or 0.001.
/// </param>
/// <returns>
/// The mesh of the convex hull or <see langword="null"/> if the point list is
/// <see langword="null"/> or empty.
/// </returns>
/// <remarks>
/// <para>
/// The returned mesh describes the convex hull. All faces are convex polygons.
/// </para>
/// <para>
/// If the created convex hull has more vertices than <paramref name="vertexLimit"/>, the hull
/// will be simplified. The simplified hull is conservative, which means it contains all given
/// <paramref name="points"/> and is less "tight" than the exact hull. It is possible that the
/// simplified hull contains slightly more vertices than <paramref name="vertexLimit"/> (e.g. it
/// is possible that for a vertex limit of 32 a hull with 34 vertices is returned).
/// </para>
/// <para>
/// All planes of the convex hull are extruded by the <paramref name="skinWidth"/>. This can be
/// used to increase or decrease the size of the convex hull.
/// </para>
/// </remarks>
public static DcelMesh CreateConvexHull(IEnumerable <Vector3> points, int vertexLimit, float skinWidth)
{
// Nothing to do for empty input.
if (points == null)
{
return(null);
}
ConvexHullBuilder builder = new ConvexHullBuilder();
builder.Grow(points, vertexLimit, skinWidth);
if (builder.Type == ConvexHullType.Empty)
{
return(null);
}
return(builder.Mesh);
}
public ConvexHullService()
{
builder = new ConvexHullBuilder(this);
}
//
// 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);
}
}
}
static void DrawManifold(bool useExerimentalMethod, ConvexConvexDistanceQueries.Result distance,
MTransform trsA, ref ConvexHullBuilder hullA, ref HullFaceData dataA,
MTransform trsB, ref ConvexHullBuilder hullB, ref HullFaceData dataB)
{
MTransform btoA = Mul(Inverse(trsA), trsB);
MTransform atoB = Mul(Inverse(trsB), trsA);
Face faceA = SelectBestFace(dataA.Faces, distance.ClosestPoints.NormalInA);
Face faceB = SelectBestFace(dataB.Faces, math.mul(atoB.Rotation, -distance.ClosestPoints.NormalInA));
if (useExerimentalMethod)
{
var legacyFaceA = faceA;
var legacyFaceB = faceB;
// Experimental method.
// Extract faces sub-set of A.
{
float bestDot = -2;
var normal = distance.ClosestPoints.NormalInA;
for (int i = 0; i < distance.SimplexDimension; ++i)
{
int vi = distance.SimplexVertexA(i);
for (int j = 0; j < dataA.VertexFaces[vi].NumFaces; ++j)
{
var k = dataA.FaceIndices[dataA.VertexFaces[vi].FirstFace + j];
var face = dataA.Faces[k];
var d = math.dot(face.Plane.Normal, normal);
if (d > bestDot)
{
faceA = face;
bestDot = d;
}
}
}
}
// Extract faces sub-set of B.
{
float bestDot = -2;
var normal = math.mul(atoB.Rotation, -distance.ClosestPoints.NormalInA);
for (int i = 0; i < distance.SimplexDimension; ++i)
{
int vi = distance.SimplexVertexB(i);
for (int j = 0; j < dataB.VertexFaces[vi].NumFaces; ++j)
{
var k = dataB.FaceIndices[dataB.VertexFaces[vi].FirstFace + j];
var face = dataB.Faces[k];
var d = math.dot(face.Plane.Normal, normal);
if (d > bestDot)
{
faceB = face;
bestDot = d;
}
}
}
}
if (legacyFaceA.FirstVertex != faceA.FirstVertex || legacyFaceB.FirstVertex != faceB.FirstVertex)
{
Debug.LogError("Different face set found.");
}
}
var facePlaneAinA = faceA.Plane;
var facePlaneBinA = TransformPlane(btoA, faceB.Plane);
/*drawFace(trsA, ref hullA, faceA, dataA.faceVertices, 0.01f, Color.yellow);
* drawFace(trsB, ref hullB, faceB, dataB.faceVertices, 0.01f, Color.yellow);*/
const float eps = 1e-6f;
var va = new List <float3>();
var vb = new List <float3>();
for (int i = 0; i < faceA.NumVertices; ++i)
{
va.Add(hullA.Vertices[dataA.FaceVertices[faceA.FirstVertex + i]].Position);
}
for (int i = 0; i < faceB.NumVertices; ++i)
{
vb.Add(Mul(btoA, hullB.Vertices[dataB.FaceVertices[faceB.FirstVertex + i]].Position));
}
var vt = new List <float3>();
for (int ai = va.Count - 1, aj = 0; aj < va.Count; ai = aj++)
{
var plane = new float4(math.normalize(math.cross(distance.ClosestPoints.NormalInA, va[ai] - va[aj])), 0);
plane.w = -math.dot(plane.xyz, va[ai]);
if (vb.Count > 1)
{
int bi = vb.Count - 1;
float d2Pi = DistanceToPlaneEps(plane, vb[bi], eps);
for (int bj = 0; bj < vb.Count; bi = bj++)
{
var d2Pj = DistanceToPlaneEps(plane, vb[bj], eps);
if ((d2Pi * d2Pj) < 0)
{
float isec = d2Pi / (d2Pi - d2Pj);
vt.Add(vb[bi] + (vb[bj] - vb[bi]) * isec);
}
if (d2Pj <= 0)
{
vt.Add(vb[bj]);
}
d2Pi = d2Pj;
}
}
var temp = vb;
vb = vt;
vt = temp;
vt.Clear();
}
if (vb.Count == 0)
{
vb.Add(distance.ClosestPoints.PositionOnBinA);
}
{
GUIStyle labelStyle = new GUIStyle();
labelStyle.fontSize = 16;
var projectionInvDen = 1 / math.dot(distance.ClosestPoints.NormalInA, facePlaneAinA.Normal);
int i = vb.Count - 1;
var piOnB = vb[i];
var piD = math.dot(facePlaneAinA, new float4(piOnB, 1)) * projectionInvDen;
var piOnA = piOnB - distance.ClosestPoints.NormalInA * piD;
for (int j = 0; j < vb.Count; i = j++)
{
#if UNITY_EDITOR
var center = Mul(trsA, (piOnA + piOnB) / 2);
Handles.Label(center, $"{piD}", labelStyle);
#endif
var pjOnB = vb[j];
var pjD = math.dot(facePlaneAinA, new float4(pjOnB, 1)) * projectionInvDen;
var pjOnA = pjOnB - distance.ClosestPoints.NormalInA * pjD;
Gizmos.DrawLine(Mul(trsA, piOnB), Mul(trsA, pjOnB));
Gizmos.DrawLine(Mul(trsA, piOnA), Mul(trsA, pjOnA));
Gizmos.DrawLine(Mul(trsA, pjOnB), Mul(trsA, pjOnA));
piOnB = pjOnB;
piOnA = pjOnA;
piD = pjD;
}
}
}
//
// 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);
}
private float GetConcavity(int vertexLimit, bool sampleVertices, bool sampleCenters)
{
// Initially we assume that the new vertices are simply the union of the islands' vertices.
Vertices = IslandA.Vertices.Union(IslandB.Vertices).ToArray();
try
{
// Create hull mesh.
// Incremental hull building.
// Note: Commented out because this building the hull this way is less stable.
//bool rebuild = true;
//try
//{
// if (IslandA.ConvexHullBuilder != null)
// {
// if (IslandB.ConvexHullBuilder == null || IslandA.Vertices.Length > IslandB.Vertices.Length)
// {
// ConvexHullBuilder = IslandA.ConvexHullBuilder.Clone();
// ConvexHullBuilder.Grow(IslandB.Vertices, vertexLimit, 0);
// }
// else
// {
// ConvexHullBuilder = IslandB.ConvexHullBuilder.Clone();
// ConvexHullBuilder.Grow(IslandA.Vertices, vertexLimit, 0);
// }
// rebuild = false;
// }
// else if (IslandB.ConvexHullBuilder != null)
// {
// ConvexHullBuilder = IslandB.ConvexHullBuilder.Clone();
// ConvexHullBuilder.Grow(IslandA.Vertices, vertexLimit, 0);
// rebuild = false;
// }
//}
//catch (GeometryException)
//{
// rebuild = true;
//}
//if (rebuild)
{
try
{
ConvexHullBuilder = new ConvexHullBuilder();
ConvexHullBuilder.Grow(Vertices, vertexLimit, 0);
}
catch (GeometryException)
{
// Hull building failed. Try again with a randomized order.
var random = new Random(1234567);
// Fisher-Yates shuffle:
for (int i = Vertices.Length - 1; i >= 1; i--)
{
var v = Vertices[i];
var j = random.NextInteger(0, i);
Vertices[i] = Vertices[j];
Vertices[j] = v;
}
}
}
var hullMesh = ConvexHullBuilder.Mesh.ToTriangleMesh();
// Now, we have a reduced set of vertices.
Vertices = hullMesh.Vertices.ToArray();
// For larger meshes we create an AabbTree as acceleration structure.
AabbTree <int> partition = null;
if (hullMesh.NumberOfTriangles > 12)
{
partition = new AabbTree <int>
{
GetAabbForItem = i => hullMesh.GetTriangle(i).Aabb,
BottomUpBuildThreshold = 0,
};
for (int i = 0; i < hullMesh.NumberOfTriangles; i++)
{
partition.Add(i);
}
partition.Update(true);
}
Aabb aabb = Aabb;
float aabbExtent = aabb.Extent.Length;
// Note: For a speed-up we could skip some ray tests in the next loop and only sample
// a few vertices if there would be a lot of tests.
// The next loop performs ray casts against the hull mesh to determine the maximum
// concavity. We ensure that we make only one ray cast per vertex even if a vertex
// is shared by many triangles.
float maxConcavity = 0;
foreach (var triangle in IslandA.Triangles.Union(IslandB.Triangles))
{
if (sampleVertices)
{
for (int i = 0; i < triangle.Vertices.Length; i++)
{
// Each vertex can be shared by several triangles of the current islands.
// Therefore, we check the edges that contain this vertex. If an edge neighbor
// is in the same island, we make sure that the vertex concavity is computed only once
// in the triangle with the smallest Id.
var neighbor0 = triangle.Neighbors[(i + 1) % 3];
var neighbor1 = triangle.Neighbors[(i + 2) % 3];
if (neighbor0 != null &&
(neighbor0.Island == IslandA || neighbor0.Island == IslandB) &&
triangle.Id > neighbor0.Id)
{
// No need to test: The neighbor is in the same islands and this triangle Id is larger.
continue;
}
if (neighbor1 != null &&
(neighbor1.Island == IslandA || neighbor1.Island == IslandB) &&
triangle.Id > neighbor1.Id)
{
// No need to test: The neighbor is in the same islands and this triangle Id is larger.
continue;
}
var position = triangle.Vertices[i];
var normal = triangle.VertexNormals[i];
// Degenerate triangles are ignored.
if (normal.IsNumericallyZero)
{
continue;
}
// Shoot a ray from outside the hull mesh to the vertex.
float hitDistance;
Vector3F rayOrigin = position + normal * aabbExtent;
float rayLength = (position - rayOrigin).Length;
var ray = new Ray(rayOrigin, -normal, rayLength);
if (partition != null)
{
// Use AABB tree for better performance.
foreach (var triangleIndex in partition.GetOverlaps(ray))
{
var candidateTriangle = hullMesh.GetTriangle(triangleIndex);
var hit = GeometryHelper.GetContact(ray, candidateTriangle, false, out hitDistance);
if (hit)
{
// The concavity is the distance from the hull to the vertex.
float concavity = rayLength - hitDistance;
maxConcavity = Math.Max(maxConcavity, concavity);
break;
}
}
}
else
{
// No AABB tree.
var hit = GeometryHelper.GetContact(hullMesh, ray, out hitDistance);
if (hit)
{
float concavity = rayLength - hitDistance;
maxConcavity = Math.Max(maxConcavity, concavity);
}
}
}
}
if (sampleCenters)
{
// Test: Also shoot from the triangle centers.
var center = (triangle.Vertices[0] + triangle.Vertices[1] + triangle.Vertices[2]) / 3;
var normal = triangle.Normal;
// Degenerate triangles are ignored.
if (normal.IsNumericallyZero)
{
continue;
}
// Shoot a ray from outside the hull mesh to the vertex.
float hitDistance;
Vector3F rayOrigin = center + normal * aabbExtent;
float rayLength = (center - rayOrigin).Length;
var ray = new Ray(rayOrigin, -normal, rayLength);
if (partition != null)
{
// Use AABBTree for better performance.
foreach (var triangleIndex in partition.GetOverlaps(ray))
{
var candidateTriangle = hullMesh.GetTriangle(triangleIndex);
var hit = GeometryHelper.GetContact(ray, candidateTriangle, false, out hitDistance);
if (hit)
{
// The concavity is the distance from the hull to the vertex.
float concavity = rayLength - hitDistance;
maxConcavity = Math.Max(maxConcavity, concavity);
break;
}
}
}
else
{
// No AABBTree.
var hit = GeometryHelper.GetContact(hullMesh, ray, out hitDistance);
if (hit)
{
float concavity = rayLength - hitDistance;
maxConcavity = Math.Max(maxConcavity, concavity);
}
}
}
}
return(maxConcavity);
}
catch (GeometryException)
{
// Ouch, the convex hull generation failed. This can happen for degenerate inputs
// and numerical problems in the convex hull builder.
ConvexHullBuilder = null;
return(0);
}
}
/// <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);
}
