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