public SimulationContext(HavokConfiguration config) { // Unlock the plugin if it hasn't already been done. // If it remains locked, the simulation will do nothing. Plugin.EnsureUnlocked(); TimeStep = default; InputVelocities = default; Camera = default; unsafe { // Allocate this at a fixed memory location. The plugin writes to it. StepContext = (HavokSimulation.StepContext *)UnsafeUtility.Malloc(sizeof(HavokSimulation.StepContext), 16, Allocator.Persistent); UnsafeUtility.MemClear(StepContext, sizeof(HavokSimulation.StepContext)); VisualDebuggerEnabled = config.VisualDebugger.Enable != 0; WorldIndex = Plugin.HP_AllocateWorld(ref config, StepContext); m_WorldAllocated = true; } }
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; }
// 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) { 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); }
// 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); }
// 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; }