public unsafe MassProperties GetMassProperties() { var vertexCount = Length; SafetyChecks.IsTrue(vertexCount >= 3); var area = 0f; var localCenterOfMass = float2.zero; var inertia = 0f; var vertices = Vertices.GetUnsafePtr(); // Find a reference point inside the hull. var referencePoint = float2.zero; for (var i = 0; i < vertexCount; ++i) { referencePoint += vertices[i]; } referencePoint *= 1.0f / vertexCount; var oneThird = math.rcp(3.0f); var oneQuarter = math.rcp(4.0f); // Calculate the area, center of mass and inertia. for (var i = 0; i < vertexCount; ++i) { var edge1 = vertices[i] - referencePoint; var edge2 = (i + 1 < vertexCount ? vertices[i + 1] : vertices[0]) - referencePoint; var crossEdge = PhysicsMath.cross(edge1, edge2); var triangleArea = crossEdge * 0.5f; area += triangleArea; localCenterOfMass += triangleArea * oneThird * (edge1 + edge2); var intX = (edge1.x * edge1.x) + (edge2.x * edge1.x) + (edge2.x * edge2.x); var intY = (edge1.y * edge1.y) + (edge2.y * edge1.y) + (edge2.y * edge2.y); inertia += (oneQuarter * oneThird * crossEdge) * (intX + intY); } area = math.abs(area); SafetyChecks.IsTrue(area > float.Epsilon); localCenterOfMass *= math.rcp(area); localCenterOfMass += referencePoint; // Calculate the angular expansion factor. var angularExpansionFactor = 0f; for (var i = 0; i < vertexCount; ++i) { angularExpansionFactor = math.max(angularExpansionFactor, math.lengthsq(vertices[i] - localCenterOfMass)); } angularExpansionFactor = math.sqrt(angularExpansionFactor); return(new MassProperties( localCenterOfMass: localCenterOfMass, inertia: inertia, area: area, angularExpansionFactor: angularExpansionFactor)); }
public bool AddHit(T hit) { SafetyChecks.IsTrue(hit.Fraction <= MaxFraction); MaxFraction = hit.Fraction; m_ClosestHit = hit; NumHits = 1; return(true); }
void Segregate(int axis, float pivot, Range range, int minItems, ref Range lRange, ref Range rRange) { // Range length must be greater than 1. SafetyChecks.IsTrue(range.Length > 1); var lDomain = Aabb.Empty; var rDomain = Aabb.Empty; var p = PointsAsFloat3; var start = p + range.Start; var end = start + range.Length - 1; do { // Consume left. while (start <= end && (*start)[axis] < pivot) { lDomain.Include((*start++).xy); } // Consume right. while (end > start && (*end)[axis] >= pivot) { rDomain.Include((*end--).xy); } if (start >= end) { goto FINISHED; } lDomain.Include((*end).xy); rDomain.Include((*start).xy); Swap(ref *(start++), ref *(end--)); } while (true); FINISHED: // Build sub-ranges. var lSize = (int)(start - p); var 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, PointsAsFloat3 + lRange.Start, lRange.Length); SetAabbFromPoints(ref rDomain, PointsAsFloat3 + rRange.Start, rRange.Length); } else { SplitRange(ref range, lSize, ref lRange, ref rRange); } lRange.Domain = lDomain; rRange.Domain = rDomain; }
// Get the PhysicBody via an Entity lookup. public PhysicsBody GetPhysicsBody(Entity entity) { var physicsBodyIndex = GetPhysicsBodyIndex(entity); if (physicsBodyIndex != PhysicsBody.Constants.InvalidBodyIndex) { var physicsBody = PhysicsWorld.AllBodies[physicsBodyIndex]; SafetyChecks.IsTrue(entity == physicsBody.Entity); return(physicsBody); } return(default);
void ProcessSmallRange(Range baseRange, ref int freeNodeIndex) { var range = baseRange; ComputeAxisAndPivot(ref range, out var axis, out var pivot); SortRange(axis, ref range); var subRanges = stackalloc Range[4]; var hasLeftOvers = 1; do { var numSubRanges = 0; while (range.Length > 4 && numSubRanges < 3) { subRanges[numSubRanges].Start = range.Start; subRanges[numSubRanges].Length = 4; numSubRanges++; range.Start += 4; range.Length -= 4; } if (range.Length > 0) { subRanges[numSubRanges].Start = range.Start; subRanges[numSubRanges].Length = range.Length; numSubRanges++; } hasLeftOvers = 0; CreateChildren(subRanges, numSubRanges, range.Root, ref freeNodeIndex, (Range *)UnsafeUtility.AddressOf(ref range), ref hasLeftOvers); // Internal error. SafetyChecks.IsTrue(hasLeftOvers <= 1); } while (hasLeftOvers > 0); }
public bool AddHit(T hit) { SafetyChecks.IsTrue(hit.Fraction < MaxFraction); AllHits.Add(hit); return(true); }
public unsafe void SetAndGiftWrap(NativeArray <float2> points) { SafetyChecks.IsTrue(Length == points.Length); // Find rightmost point. var maxX = points[0].x; var maxIndex = 0; for (var i = 0; i < Length; ++i) { var vertex = points[i]; var x = vertex.x; if (x > maxX || (x == maxX && vertex.y < points[maxIndex].y)) { maxIndex = i; maxX = x; } } // Find convex hull. var hullIndices = new NativeArray <int>(Length, Allocator.Temp, NativeArrayOptions.UninitializedMemory); var m = 0; var ih = maxIndex; while (true) { SafetyChecks.IsTrue(m < Length); hullIndices[m] = ih; var ie = 0; for (var j = 1; j < Length; ++j) { if (ie == ih) { ie = j; continue; } var r = points[ie] - points[hullIndices[m]]; var v = points[j] - points[hullIndices[m]]; var crossEdge = PhysicsMath.cross(r, v); // Check hull point or being collinear. if (crossEdge < 0f || (math.abs(crossEdge) < float.Epsilon && math.lengthsq(v) > math.lengthsq(r))) { ie = j; } } ++m; ih = ie; if (ie == maxIndex) { break; } } // Trim lengths for vertices and normals. Length = m_Vertices.Length = m_Normals.Length = m; // Copy hull vertices. var vertices = Vertices.GetUnsafePtr(); for (var i = 0; i < Length; ++i) { vertices[i] = points[hullIndices[i]]; } hullIndices.Dispose(); // Calculate normals. var normals = Normals.GetUnsafePtr(); for (var i = 0; i < Length; ++i) { var i1 = i; var i2 = i + 1 < Length ? i + 1 : 0; var edge = vertices[i2] - vertices[i1]; SafetyChecks.IsTrue(math.lengthsq(edge) > float.Epsilon); normals[i] = math.normalize(PhysicsMath.cross(edge, 1.0f)); } }