public static bool OverlapSphereCustom <T, C>(ref T target, float3 position, float radius, ref C collector, CollisionFilter filter, QueryInteraction queryInteraction = QueryInteraction.Default)
where T : struct, ICollidable
where C : struct, ICollector <DistanceHit>
{
PointDistanceInput input = new PointDistanceInput
{
Filter = filter,
MaxDistance = radius,
Position = position
};
if (queryInteraction == QueryInteraction.Default)
{
return(target.CalculateDistance(input, ref collector));
}
else
{
unsafe
{
var interactionCollector = new QueryInteractionCollector <DistanceHit, C>
{
Collector = collector
};
bool returnValue = target.CalculateDistance(input, ref interactionCollector);
collector = interactionCollector.Collector;
return(returnValue);
}
}
}
public static unsafe BlobAssetReference <Collider> CreateQuad(float3 vertex0, float3 vertex1, float3 vertex2, float3 vertex3, CollisionFilter filter, Material material)
{
SafetyChecks.CheckFiniteAndThrow(vertex0, nameof(vertex0));
SafetyChecks.CheckFiniteAndThrow(vertex1, nameof(vertex1));
SafetyChecks.CheckFiniteAndThrow(vertex2, nameof(vertex2));
SafetyChecks.CheckFiniteAndThrow(vertex3, nameof(vertex3));
SafetyChecks.CheckCoplanarAndThrow(vertex0, vertex1, vertex2, vertex3, nameof(vertex3));
PolygonCollider collider = default;
collider.InitAsQuad(vertex0, vertex1, vertex2, vertex3, filter, material);
return(BlobAssetReference <Collider> .Create(&collider, UnsafeUtility.SizeOf <PolygonCollider>()));
}
internal void InitAsTriangle(float3 vertex0, float3 vertex1, float3 vertex2, CollisionFilter filter, Material material)
{
Init(filter, material);
SetAsTriangle(vertex0, vertex1, vertex2);
}
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 static unsafe BlobAssetReference <Collider> CreateTriangle(float3 vertex0, float3 vertex1, float3 vertex2, CollisionFilter filter, Material material)
{
SafetyChecks.CheckFiniteAndThrow(vertex0, nameof(vertex0));
SafetyChecks.CheckFiniteAndThrow(vertex1, nameof(vertex1));
SafetyChecks.CheckFiniteAndThrow(vertex2, nameof(vertex2));
var collider = new PolygonCollider();
collider.InitAsTriangle(vertex0, vertex1, vertex2, filter, material);
return(BlobAssetReference <Collider> .Create(&collider, UnsafeUtility.SizeOf <PolygonCollider>()));
}
// Create a compound collider containing an array of other colliders.
// The source colliders are copied into the compound, so that it becomes one blob.
public static unsafe BlobAssetReference <Collider> Create(NativeArray <ColliderBlobInstance> children)
{
if (children.Length == 0)
{
throw new ArgumentException();
}
// Get the total required memory size for the compound plus all its children,
// and the combined filter of all children
// TODO: Verify that the size is enough
int totalSize = Math.NextMultipleOf16(UnsafeUtility.SizeOf <CompoundCollider>());
CollisionFilter filter = children[0].Collider.Value.Filter;
foreach (var child in children)
{
totalSize += Math.NextMultipleOf16(child.Collider.Value.MemorySize);
filter = CollisionFilter.CreateUnion(filter, child.Collider.Value.Filter);
}
totalSize += (children.Length + BoundingVolumeHierarchy.Constants.MaxNumTreeBranches) * UnsafeUtility.SizeOf <BoundingVolumeHierarchy.Node>();
// Allocate the collider
var compoundCollider = (CompoundCollider *)UnsafeUtility.Malloc(totalSize, 16, Allocator.Temp);
UnsafeUtility.MemClear(compoundCollider, totalSize);
compoundCollider->m_Header.Type = ColliderType.Compound;
compoundCollider->m_Header.CollisionType = CollisionType.Composite;
compoundCollider->m_Header.Version = 0;
compoundCollider->m_Header.Magic = 0xff;
compoundCollider->m_Header.Filter = filter;
// Initialize children array
Child *childrenPtr = (Child *)((byte *)compoundCollider + UnsafeUtility.SizeOf <CompoundCollider>());
compoundCollider->m_ChildrenBlob.Offset = (int)((byte *)childrenPtr - (byte *)(&compoundCollider->m_ChildrenBlob.Offset));
compoundCollider->m_ChildrenBlob.Length = children.Length;
byte *end = (byte *)childrenPtr + UnsafeUtility.SizeOf <Child>() * children.Length;
end = (byte *)Math.NextMultipleOf16((ulong)end);
// Copy children
for (int i = 0; i < children.Length; i++)
{
Collider *collider = (Collider *)children[i].Collider.GetUnsafePtr();
UnsafeUtility.MemCpy(end, collider, collider->MemorySize);
childrenPtr[i].m_ColliderOffset = (int)(end - (byte *)(&childrenPtr[i].m_ColliderOffset));
childrenPtr[i].CompoundFromChild = children[i].CompoundFromChild;
end += Math.NextMultipleOf16(collider->MemorySize);
}
// Build mass properties
compoundCollider->MassProperties = compoundCollider->BuildMassProperties();
// Build bounding volume
int numNodes = compoundCollider->BuildBoundingVolume(out NativeArray <BoundingVolumeHierarchy.Node> nodes);
int bvhSize = numNodes * UnsafeUtility.SizeOf <BoundingVolumeHierarchy.Node>();
compoundCollider->m_BvhNodesBlob.Offset = (int)(end - (byte *)(&compoundCollider->m_BvhNodesBlob.Offset));
compoundCollider->m_BvhNodesBlob.Length = numNodes;
UnsafeUtility.MemCpy(end, nodes.GetUnsafeReadOnlyPtr(), bvhSize);
end += bvhSize;
nodes.Dispose();
// Copy to blob asset
int usedSize = (int)(end - (byte *)compoundCollider);
UnityEngine.Assertions.Assert.IsTrue(usedSize < totalSize);
compoundCollider->MemorySize = usedSize;
var blob = BlobAssetReference <Collider> .Create(compoundCollider, usedSize);
UnsafeUtility.Free(compoundCollider, Allocator.Temp);
return(blob);
}
public void AddColliderKeys(ColliderKey *keys, int count)
{
var colliderKeys = new ColliderKeyPair {
ColliderKeyA = m_ConvexColliderKey, ColliderKeyB = m_ConvexColliderKey
};
CollisionFilter filter = m_ConvexColliderA->Filter;
// Collide the convex A with all overlapping leaves of B
switch (m_CompositeColliderB->Type)
{
// Special case meshes (since we know all polygons will be built on the fly)
case ColliderType.Mesh:
{
Mesh *mesh = &((MeshCollider *)m_CompositeColliderB)->Mesh;
uint numMeshKeyBits = mesh->NumColliderKeyBits;
var polygon = new PolygonCollider();
polygon.InitEmpty();
for (int i = 0; i < count; i++)
{
ColliderKey compositeKey = m_CompositeColliderKeyPath.GetLeafKey(keys[i]);
uint meshKey = compositeKey.Value >> (32 - (int)numMeshKeyBits);
if (mesh->GetPolygon(meshKey, filter, ref polygon))
{
if (m_Flipped)
{
colliderKeys.ColliderKeyA = compositeKey;
}
else
{
colliderKeys.ColliderKeyB = compositeKey;
}
switch (m_ConvexColliderA->CollisionType)
{
case CollisionType.Convex:
ConvexConvex(
m_Context, colliderKeys, m_ConvexColliderA, (Collider *)&polygon,
m_WorldFromA, m_WorldFromB, m_CollisionTolerance, m_Flipped);
break;
case CollisionType.Terrain:
TerrainConvex(
m_Context, colliderKeys, m_ConvexColliderA, (Collider *)&polygon,
m_WorldFromA, m_WorldFromB, m_CollisionTolerance, m_Flipped);
break;
default: // GetLeaf() may not return a composite collider
throw new NotImplementedException();
}
}
}
}
break;
// General case for all other composites (compounds, compounds of meshes, etc)
default:
{
for (int i = 0; i < count; i++)
{
ColliderKey compositeKey = m_CompositeColliderKeyPath.GetLeafKey(keys[i]);
m_CompositeColliderB->GetLeaf(compositeKey, out ChildCollider leaf);
if (CollisionFilter.IsCollisionEnabled(filter, leaf.Collider->Filter)) // TODO: shouldn't be needed if/when filtering is done fully by the BVH query
{
if (m_Flipped)
{
colliderKeys.ColliderKeyA = compositeKey;
}
else
{
colliderKeys.ColliderKeyB = compositeKey;
}
MTransform worldFromLeafB = Mul(m_WorldFromB, new MTransform(leaf.TransformFromChild));
switch (leaf.Collider->CollisionType)
{
case CollisionType.Convex:
ConvexConvex(
m_Context, colliderKeys, m_ConvexColliderA, leaf.Collider,
m_WorldFromA, worldFromLeafB, m_CollisionTolerance, m_Flipped);
break;
case CollisionType.Terrain:
ConvexTerrain(
m_Context, colliderKeys, m_ConvexColliderA, leaf.Collider,
m_WorldFromA, worldFromLeafB, m_CollisionTolerance, m_Flipped);
break;
default: // GetLeaf() may not return a composite collider
throw new NotImplementedException();
}
}
}
}
break;
}
}
public static bool SphereCastAll <T>(ref T target, float3 origin, float radius, float3 direction, float maxDistance, ref NativeList <ColliderCastHit> outHits, CollisionFilter filter, QueryInteraction queryInteraction = QueryInteraction.Default)
where T : struct, ICollidable
{
var collector = new AllHitsCollector <ColliderCastHit>(1.0f, ref outHits);
return(target.SphereCastCustom(origin, radius, direction, maxDistance, ref collector, filter, queryInteraction));
}
public static bool SphereCast <T>(ref T target, float3 origin, float radius, float3 direction, float maxDistance, out ColliderCastHit hitInfo, CollisionFilter filter, QueryInteraction queryInteraction = QueryInteraction.Default)
where T : struct, ICollidable
{
var collector = new ClosestHitCollector <ColliderCastHit>(1.0f);
bool hasHit = target.SphereCastCustom(origin, radius, direction, maxDistance, ref collector, filter, queryInteraction);
hitInfo = collector.ClosestHit;
return(hasHit);
}
public static bool OverlapBoxCustom <T, C>(ref T target, float3 center, quaternion orientation, float3 halfExtents, ref C collector, CollisionFilter filter, QueryInteraction queryInteraction = QueryInteraction.Default)
where T : struct, ICollidable
where C : struct, ICollector <DistanceHit>
{
Assert.IsTrue(collector.MaxFraction == 0);
BoxCollider collider = default;
BoxGeometry geometry = new BoxGeometry
{
BevelRadius = 0.0f,
Center = float3.zero,
Size = halfExtents * 2,
Orientation = quaternion.identity
};
collider.Initialize(geometry, filter, Material.Default);
ColliderDistanceInput input;
unsafe
{
input = new ColliderDistanceInput
{
Collider = (Collider *)UnsafeUtility.AddressOf(ref collider),
MaxDistance = 0.0f,
Transform = new RigidTransform
{
pos = center,
rot = orientation
}
};
}
if (queryInteraction == QueryInteraction.Default)
{
return(target.CalculateDistance(input, ref collector));
}
else
{
unsafe
{
var interactionCollector = new QueryInteractionCollector <DistanceHit, C>
{
Collector = collector,
};
bool returnValue = target.CalculateDistance(input, ref interactionCollector);
collector = interactionCollector.Collector;
return(returnValue);
}
}
}
public static bool CheckBox <T>(ref T target, float3 center, quaternion orientation, float3 halfExtents, CollisionFilter filter, QueryInteraction queryInteraction = QueryInteraction.Default)
where T : struct, ICollidable
{
var collector = new AnyHitCollector <DistanceHit>(0.0f);
return(target.OverlapBoxCustom(center, orientation, halfExtents, ref collector, filter, queryInteraction));
}
public static bool CheckCapsule <T>(ref T target, float3 point1, float3 point2, float radius, CollisionFilter filter, QueryInteraction queryInteraction = QueryInteraction.Default)
where T : struct, ICollidable
{
var collector = new AnyHitCollector <DistanceHit>(0.0f);
return(target.OverlapCapsuleCustom(point1, point2, radius, ref collector, filter, queryInteraction));
}
public static bool OverlapCapsuleCustom <T, C>(ref T target, float3 point1, float3 point2, float radius, ref C collector, CollisionFilter filter, QueryInteraction queryInteraction = QueryInteraction.Default)
where T : struct, ICollidable
where C : struct, ICollector <DistanceHit>
{
Assert.IsTrue(collector.MaxFraction == 0);
CapsuleCollider collider = default;
float3 center = (point1 + point2) / 2;
CapsuleGeometry geometry = new CapsuleGeometry
{
Radius = radius,
Vertex0 = point1 - center,
Vertex1 = point2 - center
};
collider.Initialize(geometry, filter, Material.Default);
ColliderDistanceInput input;
unsafe
{
input = new ColliderDistanceInput
{
Collider = (Collider *)UnsafeUtility.AddressOf(ref collider),
MaxDistance = 0.0f,
Transform = new RigidTransform
{
pos = center,
rot = quaternion.identity
}
};
}
if (queryInteraction == QueryInteraction.Default)
{
return(target.CalculateDistance(input, ref collector));
}
else
{
unsafe
{
var interactionCollector = new QueryInteractionCollector <DistanceHit, C>
{
Collector = collector,
};
bool returnValue = target.CalculateDistance(input, ref interactionCollector);
collector = interactionCollector.Collector;
return(returnValue);
}
}
}
public static bool OverlapSphere <T>(ref T target, float3 position, float radius, ref NativeList <DistanceHit> outHits, CollisionFilter filter, QueryInteraction queryInteraction = QueryInteraction.Default)
where T : struct, ICollidable
{
var collector = new AllHitsCollector <DistanceHit>(radius, ref outHits);
return(target.OverlapSphereCustom(position, radius, ref collector, filter, queryInteraction));
}
public static unsafe BlobAssetReference <Collider> Create(
NativeArray <float> heights, int2 size, float3 scale, CollisionMethod collisionMethod, CollisionFilter filter, Material material
)
{
if (math.any(size < 2))
{
throw new ArgumentOutOfRangeException("Tried to create TerrainCollider with size < 2");
}
if (math.any(scale <= float3.zero))
{
throw new ArgumentOutOfRangeException("Tried to create TerrainCollider with scale <= 0");
}
// Allocate memory for the collider
int totalSize = sizeof(TerrainCollider) + Terrain.CalculateDataSize(size);
var collider = (TerrainCollider *)UnsafeUtility.Malloc(totalSize, 16, Allocator.Temp);
// BlobAssetReference<T> does memcpy, so allocate memory first and then assign properly aligned values to result
var blob = BlobAssetReference <Collider> .Create(collider, totalSize);
UnsafeUtility.Free(collider, Allocator.Temp);
collider = (TerrainCollider *)blob.GetUnsafePtr();
UnsafeUtility.MemClear(collider, totalSize);
// Initialize the collider
collider->m_Header.Type = ColliderType.Terrain;
collider->m_Header.CollisionType = (collisionMethod == CollisionMethod.Triangles) ? CollisionType.Composite : CollisionType.Terrain;
collider->m_Header.Version = 1;
collider->m_Header.Magic = 0xff;
collider->m_Header.Filter = filter;
collider->Material = material;
collider->MemorySize = totalSize;
collider->Terrain.Init(size, scale, (float *)heights.GetUnsafePtr());
return(blob);
}
public static bool BoxCastCustom <T, C>(ref T target, float3 center, quaternion orientation, float3 halfExtents, float3 direction, float maxDistance, ref C collector, CollisionFilter filter, QueryInteraction queryInteraction = QueryInteraction.Default)
where T : struct, ICollidable
where C : struct, ICollector <ColliderCastHit>
{
BoxCollider collider = default;
BoxGeometry boxGeometry = new BoxGeometry
{
BevelRadius = 0,
Center = 0,
Orientation = quaternion.identity,
Size = halfExtents * 2
};
collider.Initialize(boxGeometry, filter, Material.Default);
ColliderCastInput input;
unsafe
{
input = new ColliderCastInput
{
Collider = (Collider *)UnsafeUtility.AddressOf(ref collider),
Orientation = orientation,
Start = center,
End = center + direction * maxDistance
};
}
if (queryInteraction == QueryInteraction.Default)
{
return(target.CastCollider(input, ref collector));
}
else
{
unsafe
{
var interactionCollector = new QueryInteractionCollector <ColliderCastHit, C>
{
Collector = collector
};
bool returnValue = target.CastCollider(input, ref interactionCollector);
collector = interactionCollector.Collector;
return(returnValue);
}
}
}
public static unsafe bool PointCollider <T>(PointDistanceInput input, Collider *target, ref T collector) where T : struct, ICollector <DistanceHit>
{
if (!CollisionFilter.IsCollisionEnabled(input.Filter, target->Filter))
{
return(false);
}
Result result;
switch (target->Type)
{
case ColliderType.Sphere:
var sphere = (SphereCollider *)target;
result = PointPoint(sphere->Center, input.Position, sphere->Radius, sphere->Radius);
break;
case ColliderType.Capsule:
var capsule = (CapsuleCollider *)target;
result = CapsuleSphere(capsule->Vertex0, capsule->Vertex1, capsule->Radius, input.Position, 0.0f, MTransform.Identity);
break;
case ColliderType.Triangle:
var triangle = (PolygonCollider *)target;
result = TriangleSphere(
triangle->Vertices[0], triangle->Vertices[1], triangle->Vertices[2], triangle->Planes[0].Normal,
input.Position, 0.0f, MTransform.Identity);
break;
case ColliderType.Quad:
var quad = (PolygonCollider *)target;
result = QuadSphere(
quad->Vertices[0], quad->Vertices[1], quad->Vertices[2], quad->Vertices[3], quad->Planes[0].Normal,
input.Position, 0.0f, MTransform.Identity);
break;
case ColliderType.Convex:
case ColliderType.Box:
case ColliderType.Cylinder:
ref ConvexHull hull = ref ((ConvexCollider *)target)->ConvexHull;
result = ConvexConvex(hull.VerticesPtr, hull.NumVertices, hull.ConvexRadius, &input.Position, 1, 0.0f, MTransform.Identity);
break;
case ColliderType.Mesh:
return(PointMesh(input, (MeshCollider *)target, ref collector));
case ColliderType.Compound:
return(PointCompound(input, (CompoundCollider *)target, ref collector));
default:
throw new NotImplementedException();
}
if (result.Distance < collector.MaxFraction)
{
collector.AddHit(new DistanceHit
{
Fraction = result.Distance,
SurfaceNormal = -result.NormalInA,
Position = result.PositionOnAinA,
ColliderKey = ColliderKey.Empty
});
return(true);
}
return(false);
}
public static bool BoxCast <T>(ref T target, float3 center, quaternion orientation, float3 halfExtents, float3 direction, float maxDistance, CollisionFilter filter, QueryInteraction queryInteraction = QueryInteraction.Default)
where T : struct, ICollidable
{
var collector = new AnyHitCollector <ColliderCastHit>(1.0f);
return(target.BoxCastCustom(center, orientation, halfExtents, direction, maxDistance, ref collector, filter, queryInteraction));
}
// Write a set of contact manifolds for a pair of bodies to the given stream.
public static unsafe void BodyBody(RigidBody rigidBodyA, RigidBody rigidBodyB, MotionVelocity motionVelocityA, MotionVelocity motionVelocityB,
float collisionTolerance, float timeStep, BodyIndexPair pair, ref NativeStream.Writer contactWriter)
{
Collider *colliderA = rigidBodyA.Collider;
Collider *colliderB = rigidBodyB.Collider;
if (colliderA == null || colliderB == null || !CollisionFilter.IsCollisionEnabled(colliderA->Filter, colliderB->Filter))
{
return;
}
// Build combined motion expansion
MotionExpansion expansion;
{
MotionExpansion expansionA = motionVelocityA.CalculateExpansion(timeStep);
MotionExpansion expansionB = motionVelocityB.CalculateExpansion(timeStep);
expansion = new MotionExpansion
{
Linear = expansionA.Linear - expansionB.Linear,
Uniform = expansionA.Uniform + expansionB.Uniform + collisionTolerance
};
}
var context = new Context
{
BodyIndices = pair,
BodyCustomTags = new CustomTagsPair {
CustomTagsA = rigidBodyA.CustomTags, CustomTagsB = rigidBodyB.CustomTags
},
BodiesHaveInfiniteMass =
!math.any(motionVelocityA.InverseInertiaAndMass) &&
!math.any(motionVelocityB.InverseInertiaAndMass),
ContactWriter = (NativeStream.Writer *)UnsafeUtility.AddressOf(ref contactWriter)
};
var worldFromA = new MTransform(rigidBodyA.WorldFromBody);
var worldFromB = new MTransform(rigidBodyB.WorldFromBody);
// Dispatch to appropriate manifold generator
switch (colliderA->CollisionType)
{
case CollisionType.Convex:
switch (colliderB->CollisionType)
{
case CollisionType.Convex:
ConvexConvex(context, ColliderKeyPair.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false);
break;
case CollisionType.Composite:
ConvexComposite(context, ColliderKey.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion, false);
break;
case CollisionType.Terrain:
ConvexTerrain(context, ColliderKeyPair.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false);
break;
}
break;
case CollisionType.Composite:
switch (colliderB->CollisionType)
{
case CollisionType.Convex:
CompositeConvex(context, colliderA, colliderB, worldFromA, worldFromB, expansion, false);
break;
case CollisionType.Composite:
CompositeComposite(context, colliderA, colliderB, worldFromA, worldFromB, expansion, false);
break;
case CollisionType.Terrain:
CompositeTerrain(context, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false);
break;
}
break;
case CollisionType.Terrain:
switch (colliderB->CollisionType)
{
case CollisionType.Convex:
TerrainConvex(context, ColliderKeyPair.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false);
break;
case CollisionType.Composite:
TerrainComposite(context, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false);
break;
case CollisionType.Terrain:
UnityEngine.Assertions.Assert.IsTrue(false);
break;
}
break;
}
}
public static bool CapsuleCastCustom <T, C>(ref T target, float3 point1, float3 point2, float radius, float3 direction, float maxDistance, ref C collector, CollisionFilter filter, QueryInteraction queryInteraction = QueryInteraction.Default)
where T : struct, ICollidable
where C : struct, ICollector <ColliderCastHit>
{
CapsuleCollider collider = default;
float3 center = (point1 + point2) / 2;
CapsuleGeometry geometry = new CapsuleGeometry
{
Radius = radius,
Vertex0 = point1 - center,
Vertex1 = point2 - center
};
collider.Initialize(geometry, filter, Material.Default);
ColliderCastInput input;
unsafe
{
input = new ColliderCastInput
{
Collider = (Collider *)UnsafeUtility.AddressOf(ref collider),
Orientation = quaternion.identity,
Start = center,
End = center + direction * maxDistance
};
}
if (queryInteraction == QueryInteraction.Default)
{
return(target.CastCollider(input, ref collector));
}
else
{
unsafe
{
var interactionCollector = new QueryInteractionCollector <ColliderCastHit, C>
{
Collector = collector
};
bool returnValue = target.CastCollider(input, ref interactionCollector);
collector = interactionCollector.Collector;
return(returnValue);
}
}
}
public static BlobAssetReference <Collider> Create(NativeArray <float3> vertices, NativeArray <int3> triangles, CollisionFilter filter) => Create(vertices, triangles, filter, Material.Default);
public static bool CapsuleCast <T>(ref T target, float3 point1, float3 point2, float radius, float3 direction, float maxDistance, CollisionFilter filter, QueryInteraction queryInteraction = QueryInteraction.Default)
where T : struct, ICollidable
{
var collector = new AnyHitCollector <ColliderCastHit>(1.0f);
return(target.CapsuleCastCustom(point1, point2, radius, direction, maxDistance, ref collector, filter, queryInteraction));
}
// Create a compound collider containing an array of other colliders.
// The source colliders are copied into the compound, so that it becomes one blob.
public static unsafe BlobAssetReference <Collider> Create(NativeArray <ColliderBlobInstance> children)
{
SafetyChecks.CheckNotEmptyAndThrow(children, nameof(children));
// Get the total required memory size for the compound plus all its children,
// and the combined filter of all children
// TODO: Verify that the size is enough
int totalSize = Math.NextMultipleOf16(UnsafeUtility.SizeOf <CompoundCollider>());
CollisionFilter filter = children[0].Collider.Value.Filter;
var srcToDestInstanceAddrs = new NativeHashMap <long, long>(children.Length, Allocator.Temp);
for (var childIndex = 0; childIndex < children.Length; childIndex++)
{
var child = children[childIndex];
var instanceKey = (long)child.Collider.GetUnsafePtr();
if (srcToDestInstanceAddrs.ContainsKey(instanceKey))
{
continue;
}
totalSize += Math.NextMultipleOf16(child.Collider.Value.MemorySize);
filter = CollisionFilter.CreateUnion(filter, child.Collider.Value.Filter);
srcToDestInstanceAddrs.Add(instanceKey, 0L);
}
totalSize += (children.Length + BoundingVolumeHierarchy.Constants.MaxNumTreeBranches) * UnsafeUtility.SizeOf <BoundingVolumeHierarchy.Node>();
// Allocate the collider
var compoundCollider = (CompoundCollider *)UnsafeUtility.Malloc(totalSize, 16, Allocator.Temp);
UnsafeUtility.MemClear(compoundCollider, totalSize);
compoundCollider->m_Header.Type = ColliderType.Compound;
compoundCollider->m_Header.CollisionType = CollisionType.Composite;
compoundCollider->m_Header.Version = 0;
compoundCollider->m_Header.Magic = 0xff;
compoundCollider->m_Header.Filter = filter;
// Initialize children array
Child *childrenPtr = (Child *)((byte *)compoundCollider + UnsafeUtility.SizeOf <CompoundCollider>());
compoundCollider->m_ChildrenBlob.Offset = (int)((byte *)childrenPtr - (byte *)(&compoundCollider->m_ChildrenBlob.Offset));
compoundCollider->m_ChildrenBlob.Length = children.Length;
byte *end = (byte *)childrenPtr + UnsafeUtility.SizeOf <Child>() * children.Length;
end = (byte *)Math.NextMultipleOf16((ulong)end);
uint maxTotalNumColliderKeyBits = 0;
// Copy children
for (int i = 0; i < children.Length; i++)
{
Collider *collider = (Collider *)children[i].Collider.GetUnsafePtr();
var srcInstanceKey = (long)collider;
var dstAddr = srcToDestInstanceAddrs[srcInstanceKey];
if (dstAddr == 0L)
{
dstAddr = (long)end;
srcToDestInstanceAddrs[srcInstanceKey] = dstAddr;
UnsafeUtility.MemCpy(end, collider, collider->MemorySize);
end += Math.NextMultipleOf16(collider->MemorySize);
}
childrenPtr[i].m_ColliderOffset = (int)((byte *)dstAddr - (byte *)(&childrenPtr[i].m_ColliderOffset));
childrenPtr[i].CompoundFromChild = children[i].CompoundFromChild;
maxTotalNumColliderKeyBits = math.max(maxTotalNumColliderKeyBits, collider->TotalNumColliderKeyBits);
}
// Build mass properties
compoundCollider->MassProperties = compoundCollider->BuildMassProperties();
// Build bounding volume
int numNodes = compoundCollider->BuildBoundingVolume(out NativeArray <BoundingVolumeHierarchy.Node> nodes);
int bvhSize = numNodes * UnsafeUtility.SizeOf <BoundingVolumeHierarchy.Node>();
compoundCollider->m_BvhNodesBlob.Offset = (int)(end - (byte *)(&compoundCollider->m_BvhNodesBlob.Offset));
compoundCollider->m_BvhNodesBlob.Length = numNodes;
UnsafeUtility.MemCpy(end, nodes.GetUnsafeReadOnlyPtr(), bvhSize);
end += bvhSize;
// Validate nesting level of composite colliders.
compoundCollider->TotalNumColliderKeyBits = maxTotalNumColliderKeyBits + compoundCollider->NumColliderKeyBits;
// If TotalNumColliderKeyBits is greater than 32, it means maximum nesting level of composite colliders has been breached.
// ColliderKey has 32 bits so it can't handle infinite nesting of composite colliders.
if (compoundCollider->TotalNumColliderKeyBits > 32)
{
SafetyChecks.ThrowArgumentException(nameof(children), "Composite collider exceeded maximum level of nesting!");
}
// Copy to blob asset
int usedSize = (int)(end - (byte *)compoundCollider);
UnityEngine.Assertions.Assert.IsTrue(usedSize < totalSize);
compoundCollider->MemorySize = usedSize;
var blob = BlobAssetReference <Collider> .Create(compoundCollider, usedSize);
UnsafeUtility.Free(compoundCollider, Allocator.Temp);
return(blob);
}
// Get the vertices, flags and collision filter of a primitive
internal void GetPrimitive(int primitiveKey, out float3x4 vertices, out PrimitiveFlags flags, out CollisionFilter filter)
{
int sectionIndex = primitiveKey >> 8;
int sectionPrimitiveIndex = primitiveKey & 0xFF;
ref Section section = ref Sections[sectionIndex];
public static BlobAssetReference <Collider> CreateQuad(float3 vertex0, float3 vertex1, float3 vertex2, float3 vertex3, CollisionFilter filter) => CreateQuad(vertex0, vertex1, vertex2, vertex3, filter, Material.Default);
public static unsafe BlobAssetReference <Collider> Create(
NativeArray <float> heights, int2 size, float3 scale, CollisionMethod collisionMethod, CollisionFilter filter, Material material
)
{
SafetyChecks.CheckInRangeAndThrow(size.x, new int2(2, int.MaxValue), nameof(size));
SafetyChecks.CheckInRangeAndThrow(size.y, new int2(2, int.MaxValue), nameof(size));
SafetyChecks.CheckFiniteAndPositiveAndThrow(scale, nameof(scale));
// Allocate memory for the collider
int totalSize = sizeof(TerrainCollider) + Terrain.CalculateDataSize(size);
var collider = (TerrainCollider *)UnsafeUtility.Malloc(totalSize, 16, Allocator.Temp);
// BlobAssetReference<T> does memcpy, so allocate memory first and then assign properly aligned values to result
var blob = BlobAssetReference <Collider> .Create(collider, totalSize);
UnsafeUtility.Free(collider, Allocator.Temp);
collider = (TerrainCollider *)blob.GetUnsafePtr();
UnsafeUtility.MemClear(collider, totalSize);
// Initialize the collider
collider->m_Header.Type = ColliderType.Terrain;
collider->m_Header.CollisionType = (collisionMethod == CollisionMethod.Triangles) ? CollisionType.Composite : CollisionType.Terrain;
collider->m_Header.Version = 1;
collider->m_Header.Magic = 0xff;
collider->m_Header.Filter = filter;
collider->Material = material;
collider->MemorySize = totalSize;
collider->Terrain.Init(size, scale, (float *)heights.GetUnsafePtr());
return(blob);
}
internal void InitNoVertices(CollisionFilter filter, Material material)
{
Init(filter, material);
ConvexHull.VerticesBlob.Length = 0;
}
public static BlobAssetReference <Collider> Create(
NativeArray <float> heights, int2 size, float3 scale, CollisionMethod collisionMethod, CollisionFilter filter
) =>
Create(heights, size, scale, collisionMethod, filter, Material.Default);
internal void InitAsQuad(float3 vertex0, float3 vertex1, float3 vertex2, float3 vertex3, CollisionFilter filter, Material material)
{
Init(filter, material);
SetAsQuad(vertex0, vertex1, vertex2, vertex3);
}
public static unsafe BlobAssetReference <Collider> CreateQuad(float3 vertex0, float3 vertex1, float3 vertex2, float3 vertex3, CollisionFilter filter, Material material)
{
if (math.any(!math.isfinite(vertex0)) || math.any(!math.isfinite(vertex1)) || math.any(!math.isfinite(vertex2)) || math.any(!math.isfinite(vertex3)))
{
throw new ArgumentException("Tried to create triangle collider with nan/inf vertex");
}
// check if vertices are co-planar
float3 normal = math.normalize(math.cross(vertex1 - vertex0, vertex2 - vertex0));
if (math.abs(math.dot(normal, vertex3 - vertex0)) > 1e-3f)
{
throw new ArgumentException("Vertices for quad creation are not co-planar");
}
PolygonCollider collider = default;
collider.InitAsQuad(vertex0, vertex1, vertex2, vertex3, filter, material);
return(BlobAssetReference <Collider> .Create(&collider, UnsafeUtility.SizeOf <PolygonCollider>()));
}
