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