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