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