void ProcessAxis(Range range, int axis, NativeArray <float> scores, NativeArray <float4> points, ref int bestAxis, ref int pivot, ref float minScore) { CompareVertices comparator; comparator.SortAxis = axis; points.Sort(comparator); PointAndIndex *p = (PointAndIndex *)PointsAsFloat4 + range.Start; Aabb runningAabb = Aabb.Empty; for (int i = 0; i < points.Length; i++) { runningAabb.Include(Aabbs[p[i].Index]); scores[i] = (i + 1) * runningAabb.SurfaceArea; } runningAabb = Aabb.Empty; for (int i = points.Length - 1, j = 1; i > 0; --i, ++j) { runningAabb.Include(Aabbs[p[i].Index]); float sum = scores[i - 1] + j * runningAabb.SurfaceArea; if (sum < minScore) { pivot = i; bestAxis = axis; minScore = sum; } } }
void ProcessAxis(int rangeLength, int axis, NativeArray <float> scores, NativeArray <float4> points, ref int bestAxis, ref int pivot, ref float minScore) { CompareVertices comparator; comparator.SortAxis = axis; NativeSortExtension.Sort((float4 *)points.GetUnsafePtr(), rangeLength, comparator); PointAndIndex *p = (PointAndIndex *)points.GetUnsafePtr(); Aabb runningAabb = Aabb.Empty; for (int i = 0; i < rangeLength; i++) { runningAabb.Include(Aabbs[p[i].Index]); scores[i] = (i + 1) * runningAabb.SurfaceArea; } runningAabb = Aabb.Empty; for (int i = rangeLength - 1, j = 1; i > 0; --i, ++j) { runningAabb.Include(Aabbs[p[i].Index]); float sum = scores[i - 1] + j * runningAabb.SurfaceArea; if (sum < minScore) { pivot = i; bestAxis = axis; minScore = sum; } } }
void Segregate(int axis, float pivot, Range range, int minItems, ref Range lRange, ref Range rRange) { Assert.IsTrue(range.Length > 1 /*, "Range length must be greater than 1."*/); Aabb lDomain = Aabb.Empty; Aabb rDomain = Aabb.Empty; float4 *p = PointsAsFloat4; float4 *start = p + range.Start; float4 *end = p + range.Length - 1; do { // Consume left. while (start <= end && (*start)[axis] < pivot) { lDomain.Include((*(start++)).xyz); } // Consume right. while (end > start && (*end)[axis] >= pivot) { rDomain.Include((*(end--)).xyz); } if (start >= end) { goto FINISHED; } lDomain.Include((*end).xyz); rDomain.Include((*start).xyz); Swap(ref *(start++), ref *(end--)); } while (true); FINISHED: // Build sub-ranges. int lSize = (int)(start - p); int rSize = range.Length - lSize; if (lSize < minItems || rSize < minItems) { // Make sure sub-ranges contains at least minItems nodes, in these rare cases (i.e. all points at the same position), we just split the set in half regardless of positions. SplitRange(ref range, range.Length / 2, ref lRange, ref rRange); SetAabbFromPoints(ref lDomain, PointsAsFloat4 + lRange.Start, lRange.Length); SetAabbFromPoints(ref rDomain, PointsAsFloat4 + rRange.Start, rRange.Length); } else { SplitRange(ref range, lSize, ref lRange, ref rRange); } lRange.Domain = lDomain; rRange.Domain = rDomain; }
public static Aabb Union(Aabb a, Aabb b) { a.Include(b); return(a); }
// 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) }; } }