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));
}
// Collision Detection in Interactive 3D Environments by Gino van den Bergen
// From Section 3.4.1 via Box2D.
public static bool RaycastSegment(
ref Ray ray, float2 vertex0, float2 vertex1,
ref float fraction, out float2 normal)
{
// Cull back facing collision and ignore parallel segments.
var rayDirection = ray.Displacement;
var segmentNormal = PhysicsMath.cross(vertex1 - vertex0, 1.0f);
var denominator = -math.dot(rayDirection, segmentNormal);
var slop = float.Epsilon * 100f;
if (denominator > slop)
{
// Does the segment intersect the infinite line associated with this segment?
var offset = ray.Origin - vertex0;
var hitFraction = math.dot(offset, segmentNormal);
if (hitFraction >= 0f && hitFraction <= fraction * denominator)
{
// Does the segment intersect this segment?
var mu2 = -rayDirection.x * offset.y + rayDirection.y * offset.x;
if (-slop * denominator <= mu2 && mu2 <= denominator * (1.0f + slop))
{
normal = math.normalize(segmentNormal);
fraction = hitFraction / denominator;
return(true);
}
}
}
normal = float2.zero;
return(false);
}
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));
}
}
