internal static unsafe void ExecuteImpl(int index, float aabbMargin,
NativeSlice <RigidBody> rigidBodies, NativeArray <Aabb> aabbs, NativeArray <PointAndIndex> points, NativeArray <CollisionFilter> filtersOut)
{
RigidBody body = rigidBodies[index];
Aabb aabb;
if (body.Collider.IsCreated)
{
aabb = body.Collider.Value.CalculateAabb(body.WorldFromBody);
aabb.Expand(aabbMargin);
filtersOut[index] = rigidBodies[index].Collider.Value.Filter;
}
else
{
aabb.Min = body.WorldFromBody.pos;
aabb.Max = body.WorldFromBody.pos;
filtersOut[index] = CollisionFilter.Zero;
}
aabbs[index] = aabb;
points[index] = new BoundingVolumeHierarchy.PointAndIndex
{
Position = aabb.Center,
Index = index
};
}
private unsafe int BuildBoundingVolume(out NativeArray <BoundingVolumeHierarchy.Node> nodes)
{
// Create inputs
var points = new NativeArray <BoundingVolumeHierarchy.PointAndIndex>(NumChildren, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
var aabbs = new NativeArray <Aabb>(NumChildren, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
for (int i = 0; i < NumChildren; ++i)
{
points[i] = new BoundingVolumeHierarchy.PointAndIndex {
Position = Children[i].CompoundFromChild.pos, Index = i
};
aabbs[i] = Children[i].Collider->CalculateAabb(Children[i].CompoundFromChild);
}
// Build BVH
// Todo: cleanup, better size of nodes array
nodes = new NativeArray <BoundingVolumeHierarchy.Node>(2 + NumChildren, Allocator.Temp, NativeArrayOptions.UninitializedMemory)
{
[0] = BoundingVolumeHierarchy.Node.Empty,
[1] = BoundingVolumeHierarchy.Node.Empty
};
var bvh = new BoundingVolumeHierarchy(nodes);
bvh.Build(points, aabbs, out int numNodes);
return(numNodes);
}
internal static unsafe void ExecuteImpl(int index, float aabbMargin, float3 gravity, float timeStep,
NativeArray <RigidBody> rigidBodies, NativeArray <MotionVelocity> motionVelocities,
NativeArray <Aabb> aabbs, NativeArray <PointAndIndex> points,
NativeArray <CollisionFilter> filtersOut, NativeArray <bool> respondsToCollisionOut)
{
RigidBody body = rigidBodies[index];
Aabb aabb;
if (body.Collider.IsCreated)
{
var mv = motionVelocities[index];
// Apply gravity only on a copy to get proper expansion for the AABB,
// actual applying of gravity will be done later in the physics step
mv.LinearVelocity += gravity * timeStep * mv.GravityFactor;
MotionExpansion expansion = mv.CalculateExpansion(timeStep);
aabb = expansion.ExpandAabb(body.Collider.Value.CalculateAabb(body.WorldFromBody));
aabb.Expand(aabbMargin);
filtersOut[index] = body.Collider.Value.Filter;
respondsToCollisionOut[index] = body.Collider.Value.RespondsToCollision;
}
else
{
aabb.Min = body.WorldFromBody.pos;
aabb.Max = body.WorldFromBody.pos;
filtersOut[index] = CollisionFilter.Zero;
respondsToCollisionOut[index] = false;
}
aabbs[index] = aabb;
points[index] = new BoundingVolumeHierarchy.PointAndIndex
{
Position = aabb.Center,
Index = index
};
}
// 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;
}