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 ref T this[int index]
{
get
{
SafetyChecks.CheckIndexAndThrow(index, Length);
return(ref UnsafeUtility.ArrayElementAsRef <T>((byte *)m_OffsetPtr + *m_OffsetPtr, index));
}
}
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);
internal static unsafe bool PointDistance <T>(PointDistanceInput input, Collider *collider, ref T collector) where T : struct, ICollector <DistanceHit>
{
if (!CollisionFilter.IsCollisionEnabled(input.Filter, collider->Filter))
{
return(false);
}
// Ensure the query context is initialized.
input.QueryContext.EnsureIsInitialized();
var proxySource = new DistanceProxy(1, &input.Position, 0f);
DistanceProxy proxyTarget;
switch (collider->ColliderType)
{
case ColliderType.Box:
case ColliderType.Polygon:
case ColliderType.Capsule:
case ColliderType.Circle:
{
var convexCollider = (ConvexCollider *)collider;
proxyTarget = new DistanceProxy(ref convexCollider->m_ConvexHull);
break;
}
case ColliderType.Compound:
return(PointDistanceCompound(input, (PhysicsCompoundCollider *)collider, ref collector));
default:
SafetyChecks.ThrowNotImplementedException();
return(default);
}
var hit = ColliderDistance(PhysicsTransform.Identity, ref proxySource, ref proxyTarget);
if (hit.Distance < collector.MaxFraction)
{
hit.PhysicsBodyIndex = input.QueryContext.PhysicsBodyIndex;
hit.ColliderKey = input.QueryContext.ColliderKey;
hit.Entity = input.QueryContext.Entity;
hit.PointA = PhysicsMath.mul(input.QueryContext.LocalToWorldTransform, hit.PointA);
hit.PointB = PhysicsMath.mul(input.QueryContext.LocalToWorldTransform, hit.PointB);
return(collector.AddHit(hit));
}
return(false);
}
// Create a mask given a selection of layers.
public static uint CreateMask(params int[] collisionLayers)
{
uint mask = 0;
for (var i = 0; i < collisionLayers.Length; ++i)
{
var layer = collisionLayers[i];
if (layer >= 0 && layer <= 32)
{
mask |= (uint)1 << layer;
continue;
}
SafetyChecks.ThrowArgumentException("Collision mask layers must be in the range 0-31.");
return(default);
// Create a Dynamic body with the specified mass.
public static PhysicsMass CreateDynamic(MassProperties massProperties, float mass)
{
if (!(mass <= 0f) && math.isfinite(mass))
{
return new PhysicsMass
{
InverseMass = math.rcp(mass),
InverseInertia = massProperties.MassDistribution.InverseInertia,
LocalCenterOfMass = massProperties.MassDistribution.LocalCenterOfMass,
}
}
;
SafetyChecks.ThrowArgumentException("Cannot specify less than zero or Infinite/NaN.", "mass");
return(default);
// Schedule all the jobs for the simulation step.
public SimulationJobHandles ScheduleStepJobs(PhysicsWorld physicsWorld, PhysicsCallbacks physicsCallbacks, JobHandle inputDeps)
{
var physicsSettings = physicsWorld.Settings;
SafetyChecks.IsFalse(physicsWorld.TimeStep < 0f);
SimulationContext.Reset(ref physicsWorld, false);
SimulationContext.TimeStep = physicsWorld.TimeStep;
if (physicsWorld.DynamicBodyCount == 0)
{
// No need to do anything, since nothing can move
m_StepHandles = new SimulationJobHandles(inputDeps);
return(m_StepHandles);
}
// Execute phase callback.
var handle = physicsCallbacks.ScheduleCallbacksForPhase(PhysicsCallbacks.Phase.PreStepSimulation, ref physicsWorld, inputDeps);
// Apply gravity and copy input velocities at this point (in parallel with the scheduler, but before the callbacks)
handle = Solver.ScheduleApplyGravityAndCopyInputVelocitiesJob(
ref physicsWorld.DynamicsWorld, SimulationContext.InputVelocities, physicsWorld.TimeStep * physicsSettings.Gravity, handle, physicsSettings.NumberOfThreadsHint);
handle = physicsCallbacks.ScheduleCallbacksForPhase(PhysicsCallbacks.Phase.PostCreateOverlapBodies, ref physicsWorld, handle);
handle = physicsCallbacks.ScheduleCallbacksForPhase(PhysicsCallbacks.Phase.PostCreateContacts, ref physicsWorld, handle);
handle = physicsCallbacks.ScheduleCallbacksForPhase(PhysicsCallbacks.Phase.PostCreateConstraints, ref physicsWorld, handle);
// Integrate motions.
handle = Integrator.ScheduleIntegrateJobs(ref physicsWorld, handle, physicsSettings.NumberOfThreadsHint);
// Schedule phase callback.
handle = physicsCallbacks.ScheduleCallbacksForPhase(PhysicsCallbacks.Phase.PostIntegrate, ref physicsWorld, handle);
m_StepHandles.FinalExecutionHandle = handle;
m_StepHandles.FinalDisposeHandle = handle;
return(m_StepHandles);
}
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);
}
internal static unsafe bool OverlapPoint <T>(OverlapPointInput input, Collider *collider, ref T collector) where T : struct, ICollector <OverlapPointHit>
{
// Nothing to do if:
// - MaxFraction is zero.
// - Filtered out.
if (math.abs(collector.MaxFraction) < 0f ||
!CollisionFilter.IsCollisionEnabled(input.Filter, collider->Filter))
{
return(false);
}
// Ensure the query context is initialized.
input.QueryContext.EnsureIsInitialized();
bool hadHit;
switch (collider->ColliderType)
{
case ColliderType.Box:
{
var box = (PhysicsBoxCollider *)collider;
hadHit = PointConvex(input.Position, ref box->m_ConvexHull);
break;
}
case ColliderType.Polygon:
{
var polygon = (PhysicsPolygonCollider *)collider;
hadHit = PointConvex(input.Position, ref polygon->m_ConvexHull);
break;
}
case ColliderType.Capsule:
{
var capsule = (PhysicsCapsuleCollider *)collider;
hadHit = PointCapsule(input.Position, capsule->Vertex0, capsule->Vertex1, capsule->Radius);
break;
}
case ColliderType.Circle:
{
var circle = (PhysicsCircleCollider *)collider;
hadHit = PointCircle(input.Position, circle->Center, circle->Radius);
break;
}
case ColliderType.Compound:
{
return(PointCompound(input, (PhysicsCompoundCollider *)collider, ref collector));
}
default:
SafetyChecks.ThrowNotImplementedException();
return(default);
}
if (hadHit)
{
var hit = new OverlapPointHit
{
Fraction = 0f,
Position = PhysicsMath.mul(input.QueryContext.LocalToWorldTransform, input.Position),
PhysicsBodyIndex = input.QueryContext.PhysicsBodyIndex,
ColliderKey = input.QueryContext.ColliderKey,
Entity = input.QueryContext.Entity
};
return(collector.AddHit(hit));
}
return(false);
}
internal static unsafe DistanceHit ColliderDistance(PhysicsTransform transformA, ref DistanceProxy proxyA, ref DistanceProxy proxyB)
{
var simplex = new Simplex();
simplex.Reset(transformA, proxyA, proxyB);
var inverseRotationA = math.transpose(transformA.Rotation);
var vertices = &simplex.Vertex1;
Simplex.VertexIndexTriple saveA;
Simplex.VertexIndexTriple saveB;
var iteration = 0;
while (iteration < PhysicsSettings.Constants.MaxGJKInterations)
{
// Copy simplex so we can identify duplicates.
var saveCount = simplex.Count;
for (var i = 0; i < saveCount; ++i)
{
saveA.Index[i] = vertices[i].IndexA;
saveB.Index[i] = vertices[i].IndexB;
}
switch (saveCount)
{
case 1:
break;
case 2:
simplex.Solve2();
break;
case 3:
simplex.Solve3();
break;
default:
SafetyChecks.ThrowInvalidOperationException("Simplex has invalid count.");
return(default);
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex.Count == 3)
{
break;
}
// Get search direction.
var direction = simplex.GetSearchDirection();
// Ensure the search direction is numerically fit.
if (math.lengthsq(direction) < float.Epsilon * float.Epsilon)
{
// The origin is probably contained by a line segment
// or triangle. Thus the shapes are overlapped.
// We can't return zero here even though there may be overlap.
// In case the simplex is a point, segment, or triangle it is difficult
// to determine if the origin is contained in the CSO or very close to it.
break;
}
// Compute a tentative new simplex vertex using support points.
var vertex = vertices + simplex.Count;
vertex->IndexA = proxyA.GetSupport(PhysicsMath.mul(inverseRotationA, -direction));
vertex->SupportA = PhysicsMath.mul(transformA, proxyA.Vertices[vertex->IndexA]);
vertex->IndexB = proxyB.GetSupport(direction);
vertex->SupportB = proxyB.Vertices[vertex->IndexB];
vertex->W = vertex->SupportB - vertex->SupportA;
// Iteration count is equated to the number of support point calls.
++iteration;
// Check for duplicate support points. This is the main termination criteria.
var duplicate = false;
for (var i = 0; i < saveCount; ++i)
{
if (vertex->IndexA == saveA.Index[i] && vertex->IndexB == saveB.Index[i])
{
duplicate = true;
break;
}
}
// If we found a duplicate support point we must exit to avoid cycling.
if (duplicate)
{
break;
}
// New vertex is okay and needed.
simplex.Count++;
}
// Prepare result.
var pointA = float2.zero;
var pointB = float2.zero;
simplex.GetWitnessPoints(ref pointA, ref pointB);
var distance = math.distance(pointA, pointB);
var radiusA = proxyA.ConvexRadius;
var radiusB = proxyB.ConvexRadius;
if (distance > (radiusA + radiusB) && distance > float.Epsilon)
{
// Shapes not overlapped.
// Move the witness points to the outer surface.
distance -= radiusA + radiusB;
var normal = math.normalize(pointB - pointA);
pointA += radiusA * normal;
pointB -= radiusB * normal;
}
else
{
// Shapes are overlapped.
// Move the witness points to the middle.
pointA = pointB = 0.5f * (pointA + pointB);
distance = 0f;
}
return(new DistanceHit
{
PointA = pointA,
PointB = pointB,
Fraction = distance
});
}
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));
}
}
