public bool Contains(Aabb aabb) => math.all((Min <= aabb.Min) & (Max >= aabb.Max));
public bool Overlaps(Aabb other)
{
return(math.all(Max >= other.Min & Min <= other.Max));
}
public static Aabb Union(Aabb a, Aabb b)
{
a.Include(b);
return(a);
}
public void Include(Aabb aabb)
{
Min = math.min(Min, aabb.Min);
Max = math.max(Max, aabb.Max);
}
public static unsafe BlobAssetReference <Collider> Create(NativeArray <float3> vertices, NativeArray <int3> triangles, CollisionFilter filter, Material material)
{
// Copy vertices
var tempVertices = new NativeArray <float3>(vertices, Allocator.Temp);
// Triangle indices - needed for WeldVertices
var tempIndices = new NativeArray <int>(triangles.Length * 3, Allocator.Temp);
for (int iTriangle = 0; iTriangle < triangles.Length; iTriangle++)
{
if (triangles[iTriangle][0] >= 0 && triangles[iTriangle][0] < vertices.Length &&
triangles[iTriangle][1] >= 0 && triangles[iTriangle][1] < vertices.Length &&
triangles[iTriangle][2] >= 0 && triangles[iTriangle][2] < vertices.Length)
{
tempIndices[iTriangle * 3] = triangles[iTriangle][0];
tempIndices[iTriangle * 3 + 1] = triangles[iTriangle][1];
tempIndices[iTriangle * 3 + 2] = triangles[iTriangle][2];
}
else
{
throw new ArgumentException("Tried to create a MeshCollider with indices referencing outside vertex array");
}
}
// Build connectivity and primitives
NativeList <float3> uniqueVertices = MeshConnectivityBuilder.WeldVertices(tempIndices, tempVertices);
var tempTriangleIndices = new NativeArray <int3>(triangles.Length, Allocator.Temp);
UnsafeUtility.MemCpy(tempTriangleIndices.GetUnsafePtr(), tempIndices.GetUnsafePtr(), tempIndices.Length * UnsafeUtility.SizeOf <int>());
var connectivity = new MeshConnectivityBuilder(tempTriangleIndices, uniqueVertices);
NativeList <MeshConnectivityBuilder.Primitive> primitives = connectivity.EnumerateQuadDominantGeometry(tempTriangleIndices, uniqueVertices);
// Build bounding volume hierarchy
int nodeCount = math.max(primitives.Length * 2 + 1, 2); // We need at least two nodes - an "invalid" node and a root node.
var nodes = new NativeArray <BoundingVolumeHierarchy.Node>(nodeCount, Allocator.Temp);
int numNodes = 0;
{
// Prepare data for BVH
var points = new NativeList <BoundingVolumeHierarchy.PointAndIndex>(primitives.Length, Allocator.Temp);
var aabbs = new NativeArray <Aabb>(primitives.Length, Allocator.Temp);
for (int i = 0; i < primitives.Length; i++)
{
MeshConnectivityBuilder.Primitive p = primitives[i];
// Skip degenerate triangles
if (MeshConnectivityBuilder.IsTriangleDegenerate(p.Vertices[0], p.Vertices[1], p.Vertices[2]))
{
continue;
}
aabbs[i] = Aabb.CreateFromPoints(p.Vertices);
points.Add(new BoundingVolumeHierarchy.PointAndIndex
{
Position = aabbs[i].Center,
Index = i
});
}
var bvh = new BoundingVolumeHierarchy(nodes);
bvh.Build(points.AsArray(), aabbs, out numNodes, useSah: true);
}
// Build mesh sections
BoundingVolumeHierarchy.Node *nodesPtr = (BoundingVolumeHierarchy.Node *)nodes.GetUnsafePtr();
MeshBuilder.TempSection sections = MeshBuilder.BuildSections(nodesPtr, numNodes, primitives);
// Allocate collider
int meshDataSize = Mesh.CalculateMeshDataSize(numNodes, sections.Ranges);
int totalColliderSize = Math.NextMultipleOf(sizeof(MeshCollider), 16) + meshDataSize;
MeshCollider *meshCollider = (MeshCollider *)UnsafeUtility.Malloc(totalColliderSize, 16, Allocator.Temp);
// Initialize it
{
UnsafeUtility.MemClear(meshCollider, totalColliderSize);
meshCollider->MemorySize = totalColliderSize;
meshCollider->m_Header.Type = ColliderType.Mesh;
meshCollider->m_Header.CollisionType = CollisionType.Composite;
meshCollider->m_Header.Version += 1;
meshCollider->m_Header.Magic = 0xff;
ref var mesh = ref meshCollider->Mesh;
mesh.Init(nodesPtr, numNodes, sections, filter, material);
// Calculate combined filter
meshCollider->m_Header.Filter = mesh.Sections.Length > 0 ? mesh.Sections[0].Filters[0] : CollisionFilter.Default;
for (int i = 0; i < mesh.Sections.Length; ++i)
{
for (var j = 0; j < mesh.Sections[i].Filters.Length; ++j)
{
var f = mesh.Sections[i].Filters[j];
meshCollider->m_Header.Filter = CollisionFilter.CreateUnion(meshCollider->m_Header.Filter, f);
}
}
meshCollider->m_Aabb = meshCollider->Mesh.BoundingVolumeHierarchy.Domain;
meshCollider->NumColliderKeyBits = meshCollider->Mesh.NumColliderKeyBits;
}
public Aabb ExpandAabb(Aabb aabb) => new Aabb
{
Max = math.max(aabb.Max, aabb.Max + Linear) + Uniform,
Min = math.min(aabb.Min, aabb.Min + Linear) - Uniform
};
public void Intersect(Aabb aabb)
{
Min = math.max(Min, aabb.Min);
Max = math.min(Max, aabb.Max);
}
// followed by variable sized mesh data
#region Construction
// Create a mesh collider asset from a set of triangles
public static unsafe BlobAssetReference <Collider> Create(float3[] vertices, int[] indices, CollisionFilter?filter = null, Material?material = null)
{
int numVertices = vertices.Length;
int numIndices = indices.Length;
int numTriangles = numIndices / 3;
// Copy vertices
float3[] tempVertices = new float3[numVertices];
Array.Copy(vertices, tempVertices, numVertices);
// Copy indices
int[] tempIndices = new int[numIndices];
for (int iTriangle = 0; iTriangle < numTriangles; iTriangle++)
{
int iIndex0 = iTriangle * 3;
int iIndex1 = iIndex0 + 1;
int iIndex2 = iIndex0 + 2;
tempIndices[iIndex0] = indices[iIndex0];
tempIndices[iIndex1] = indices[iIndex1];
tempIndices[iIndex2] = indices[iIndex2];
}
// Build connectivity and primitives
List <MeshConnectivityBuilder.Primitive> primitives = null;
{
MeshConnectivityBuilder.WeldVertices(tempIndices, ref tempVertices);
var connectivity = new MeshConnectivityBuilder(tempIndices, tempVertices);
primitives = connectivity.EnumerateQuadDominantGeometry(tempIndices, tempVertices);
}
// Build bounding volume hierarchy
var nodes = new NativeArray <BoundingVolumeHierarchy.Node>(primitives.Count * 2 + 1, Allocator.Temp);
int numNodes = 0;
{
// Prepare data for BVH
var points = new NativeArray <BoundingVolumeHierarchy.PointAndIndex>(primitives.Count, Allocator.Temp);
var aabbs = new NativeArray <Aabb>(primitives.Count, Allocator.Temp);
for (int i = 0; i < primitives.Count; i++)
{
MeshConnectivityBuilder.Primitive p = primitives[i];
// Skip degenerate triangles
if (MeshConnectivityBuilder.IsTriangleDegenerate(p.Vertices[0], p.Vertices[1], p.Vertices[2]))
{
continue;
}
aabbs[i] = Aabb.CreateFromPoints(p.Vertices);
points[i] = new BoundingVolumeHierarchy.PointAndIndex
{
Position = aabbs[i].Center,
Index = i
};
}
var bvh = new BoundingVolumeHierarchy(nodes);
bvh.Build(points, aabbs, out numNodes, useSah: true);
points.Dispose();
aabbs.Dispose();
}
// Build mesh sections
BoundingVolumeHierarchy.Node * nodesPtr = (BoundingVolumeHierarchy.Node *)nodes.GetUnsafePtr();
List <MeshBuilder.TempSection> sections = MeshBuilder.BuildSections(nodesPtr, numNodes, primitives);
// Allocate collider
int meshDataSize = Mesh.CalculateMeshDataSize(numNodes, sections);
int totalColliderSize = Math.NextMultipleOf(sizeof(MeshCollider), 16) + meshDataSize;
MeshCollider *meshCollider = (MeshCollider *)UnsafeUtility.Malloc(totalColliderSize, 16, Allocator.Temp);
// Initialize it
{
UnsafeUtility.MemClear(meshCollider, totalColliderSize);
meshCollider->MemorySize = totalColliderSize;
meshCollider->m_Header.Type = ColliderType.Mesh;
meshCollider->m_Header.CollisionType = CollisionType.Composite;
meshCollider->m_Header.Version += 1;
meshCollider->m_Header.Magic = 0xff;
ref var mesh = ref meshCollider->Mesh;
mesh.Init(nodesPtr, numNodes, sections, filter ?? CollisionFilter.Default, material ?? Material.Default);
// Calculate combined filter
meshCollider->m_Header.Filter = mesh.Sections[0].Filters[0];
for (int i = 0; i < mesh.Sections.Length; ++i)
{
foreach (CollisionFilter f in mesh.Sections[i].Filters)
{
meshCollider->m_Header.Filter = CollisionFilter.CreateUnion(meshCollider->m_Header.Filter, f);
}
}
meshCollider->m_Aabb = meshCollider->Mesh.BoundingVolumeHierarchy.Domain;
meshCollider->NumColliderKeyBits = meshCollider->Mesh.NumColliderKeyBits;
}
/// <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];
Aabb domain = GetSupportingAabb(verticesA, numVerticesA, verticesB, numVerticesB, aFromB);
const float simplificationTolerance = 0.0f;
var hull = new ConvexHullBuilder(verticesCapacity, vertices, triangles, null,
domain, simplificationTolerance, ConvexHullBuilder.IntResolution.Low);
// 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 cross = math.cross(simplex.B.Xyz - simplex.A.Xyz, simplex.C.Xyz - simplex.A.Xyz);
float crossLengthSq = math.lengthsq(cross);
if (crossLengthSq < 1e-8f) // hull builder can accept extremely thin triangles for which we cannot compute an accurate normal
{
simplex.NumVertices = 2;
goto case 2;
}
float3 normal = cross * math.rsqrt(crossLengthSq);
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);
// There could be multiple triangles in the closest plane, pick the one that has the closest point to the origin on its face
foreach (int triangleIndex in hull.Triangles.Indices)
{
if (distancesCache[triangleIndex] >= closestPlane.Distance - 1e-4f)
{
ConvexHullBuilder.Triangle triangle = hull.Triangles[triangleIndex];
float3 a = hull.Vertices[triangle.Vertex0].Position;
float3 b = hull.Vertices[triangle.Vertex1].Position;
float3 c = hull.Vertices[triangle.Vertex2].Position;
float3 cross = math.cross(b - a, c - a);
float3 dets = new float3(
math.dot(math.cross(a - c, cross), a),
math.dot(math.cross(b - a, cross), b),
math.dot(math.cross(c - b, cross), c));
if (math.all(dets >= 0))
{
Plane plane = hull.ComputePlane(triangleIndex);
if (math.dot(plane.Normal, closestPlane.Normal) > (1 - 1e-4f))
{
closestTriangleIndex = triangleIndex;
closestPlane = hull.ComputePlane(triangleIndex);
}
break;
}
}
}
// 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 == 0;
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);
}
