/// <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); }