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