public Aabb CalculateAabb(PhysicsTransform transform) { return(Collider.IsCreated ? Collider.Value.CalculateAabb(PhysicsMath.mul(transform, WorldTransform)) : new Aabb { Min = WorldTransform.Translation, Max = WorldTransform.Translation }); }
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); }
internal static void Execute(int index, NativeArray <PhysicsBody.MotionData> motionDatas, NativeArray <PhysicsBody> physicsbodies) { var motionData = motionDatas[index]; var physicsBody = physicsbodies[index]; var rotation = float2x2.Rotate(motionData.WorldAngle); var translation = motionData.WorldPosition - PhysicsMath.mul(rotation, motionData.LocalCenterOfMass); physicsBody.WorldTransform = new PhysicsTransform { Rotation = rotation, Translation = translation }; physicsbodies[index] = physicsBody; }
public unsafe Aabb CalculateAabb(PhysicsTransform transform) { var min = new float2(float.MaxValue); var max = new float2(float.MinValue); var vertices = Vertices.GetUnsafePtr(); for (var i = 0; i < Length; ++i, ++vertices) { min = math.min(min, PhysicsMath.mul(transform, *vertices)); max = math.max(max, PhysicsMath.mul(transform, *vertices)); } return(new Aabb { Min = min, Max = max }); }
public Aabb CalculateAabb(PhysicsTransform transform) { return(PhysicsMath.mul(transform, Broadphase.Domain)); }
public void SetQuaternionRotation(quaternion rotation) { Rotation = float2x2.Rotate(PhysicsMath.ZRotationFromQuaternion(rotation)); }
public PhysicsTransform(RigidTransform rigidTransform) { Translation = rigidTransform.pos.xy; Rotation = float2x2.Rotate(PhysicsMath.ZRotationFromQuaternion(rigidTransform.rot)); }
public PhysicsTransform(float3 translation, quaternion rotation) { Translation = translation.xy; Rotation = float2x2.Rotate(PhysicsMath.ZRotationFromQuaternion(rotation)); }
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 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)); } }
public Aabb CalculateAabb(PhysicsTransform transform) { // TODO: Store a convex hull wrapping all the children, and use that to calculate tighter AABBs? return(PhysicsMath.mul(transform, BoundingVolumeHierarchy.Domain)); }
/// <summary> /// Compute axis and pivot of a given range. /// </summary> /// <param name="range"></param> /// <param name="axis"></param> /// <param name="pivot"></param> static void ComputeAxisAndPivot(ref Range range, out int axis, out float pivot) { // Compute axis and pivot. axis = PhysicsMath.IndexOfMaxComponent(range.Domain.Extents); pivot = ((range.Domain.Min + range.Domain.Max) * 0.5f)[axis]; }