public float FindMinSeparation(out int indexA, out int indexB, float t) { WorldTransform xfA, xfB; _sweepA.GetTransform(out xfA, t); _sweepB.GetTransform(out xfB, t); switch (_type) { case Type.Points: { var axisA = -xfA.Rotation * _axis; var axisB = -xfB.Rotation * -_axis; indexA = _proxyA.GetSupport(axisA); indexB = _proxyB.GetSupport(axisB); var localPointA = _proxyA.Vertices[indexA]; var localPointB = _proxyB.Vertices[indexB]; var pointA = xfA.ToGlobal(localPointA); var pointB = xfB.ToGlobal(localPointB); // ReSharper disable RedundantCast Necessary for FarPhysics. return(Vector2Util.Dot((Vector2)(pointB - pointA), _axis)); // ReSharper restore RedundantCast } case Type.FaceA: { var normal = xfA.Rotation * _axis; var pointA = xfA.ToGlobal(_localPoint); var axisB = -xfB.Rotation * -normal; indexA = -1; indexB = _proxyB.GetSupport(axisB); var localPointB = _proxyB.Vertices[indexB]; var pointB = xfB.ToGlobal(localPointB); // ReSharper disable RedundantCast Necessary for FarPhysics. return(Vector2Util.Dot((Vector2)(pointB - pointA), normal)); // ReSharper restore RedundantCast } case Type.FaceB: { var normal = xfB.Rotation * _axis; var pointB = xfB.ToGlobal(_localPoint); var axisA = -xfA.Rotation * -normal; indexB = -1; indexA = _proxyA.GetSupport(axisA); var localPointA = _proxyA.Vertices[indexA]; var pointA = xfA.ToGlobal(localPointA); // ReSharper disable RedundantCast Necessary for FarPhysics. return(Vector2Util.Dot((Vector2)(pointA - pointB), normal)); // ReSharper restore RedundantCast } default: throw new ArgumentOutOfRangeException(); } }
public static float FindMinSeparation(out int indexA, out int indexB, float t, DistanceProxy proxyA, ref Sweep sweepA, DistanceProxy proxyB, ref Sweep sweepB, ref Vector2 axis, ref Vector2 localPoint, SeparationFunctionType type) { sweepA.GetTransform(out Transform xfA, t); sweepB.GetTransform(out Transform xfB, t); switch (type) { case SeparationFunctionType.Points: { Vector2 axisA = MathUtils.MulT(ref xfA.q, axis); Vector2 axisB = MathUtils.MulT(ref xfB.q, -axis); indexA = proxyA.GetSupport(axisA); indexB = proxyB.GetSupport(axisB); Vector2 localPointA = proxyA._vertices[indexA]; Vector2 localPointB = proxyB._vertices[indexB]; Vector2 pointA = MathUtils.Mul(ref xfA, localPointA); Vector2 pointB = MathUtils.Mul(ref xfB, localPointB); float separation = Vector2.Dot(pointB - pointA, axis); return(separation); } case SeparationFunctionType.FaceA: { Vector2 normal = MathUtils.Mul(ref xfA.q, axis); Vector2 pointA = MathUtils.Mul(ref xfA, localPoint); Vector2 axisB = MathUtils.MulT(ref xfB.q, -normal); indexA = -1; indexB = proxyB.GetSupport(axisB); Vector2 localPointB = proxyB._vertices[indexB]; Vector2 pointB = MathUtils.Mul(ref xfB, localPointB); float separation = Vector2.Dot(pointB - pointA, normal); return(separation); } case SeparationFunctionType.FaceB: { Vector2 normal = MathUtils.Mul(ref xfB.q, axis); Vector2 pointB = MathUtils.Mul(ref xfB, localPoint); Vector2 axisA = MathUtils.MulT(ref xfA.q, -normal); indexB = -1; indexA = proxyA.GetSupport(axisA); Vector2 localPointA = proxyA._vertices[indexA]; Vector2 pointA = MathUtils.Mul(ref xfA, localPointA); float separation = Vector2.Dot(pointA - pointB, normal); return(separation); } default: Debug.Assert(false); indexA = -1; indexB = -1; return(0.0f); } }
/// <summary> /// Computes the distance between two shapes represented by the specified proxies, positioned as defined by the /// specified transforms. The cache is read-write and will be updated for future calls. /// </summary> private static float Distance( ref SimplexCache cache, DistanceProxy proxyA, WorldTransform xfA, DistanceProxy proxyB, WorldTransform xfB, bool useRadii = false) { // Initialize the simplex. Simplex simplex; Simplex.ReadCache(out simplex, cache, xfA, proxyA, xfB, proxyB); // These store the vertices of the last simplex so that we // can check for duplicates and prevent cycling. var saveA = new FixedArray3 <int>(); var saveB = new FixedArray3 <int>(); // Main iteration loop. for (var i = 0; i < 20; ++i) { // Copy simplex so we can identify duplicates. var saveCount = simplex.Count; for (var j = 0; j < saveCount; ++j) { saveA[j] = simplex.Vertices[j].IndexA; saveB[j] = simplex.Vertices[j].IndexB; } switch (simplex.Count) { case 1: break; case 2: simplex.Solve2(); break; case 3: simplex.Solve3(); break; default: throw new ArgumentOutOfRangeException(); } // If we have 3 points, then the origin is in the corresponding triangle. if (simplex.Count == 3) { break; } // Get search direction. var d = simplex.GetSearchDirection(); // Ensure the search direction is numerically fit. if (d.LengthSquared() < Settings.Epsilon * Settings.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. SimplexVertex v; v.IndexA = proxyA.GetSupport(-xfA.Rotation * -d); v.IndexB = proxyB.GetSupport(-xfB.Rotation * d); v.VertexA = xfA.ToGlobal(proxyA.Vertices[v.IndexA]); v.VertexB = xfB.ToGlobal(proxyB.Vertices[v.IndexB]); // ReSharper disable RedundantCast Necessary for FarPhysics. v.VertexDelta = (LocalPoint)(v.VertexB - v.VertexA); // ReSharper restore RedundantCast v.Alpha = 0; simplex.Vertices[simplex.Count] = v; // Check for duplicate support points. This is the main termination criteria. var duplicate = false; for (var j = 0; j < saveCount; ++j) { if (v.IndexA == saveA[j] && v.IndexB == saveB[j]) { duplicate = true; break; } } // If we found a duplicate support point we must exit to avoid cycling. if (duplicate) { break; } // New vertex is ok and needed. ++simplex.Count; } // Cache the simplex. simplex.WriteCache(ref cache); // Prepare output. var distance = simplex.GetWitnessPointDistance(); // Apply radii if requested. if (useRadii) { var rA = proxyA.Radius; var rB = proxyB.Radius; if (distance > rA + rB && distance > Settings.Epsilon) { // Shapes are still not overlapped. return(distance - (rA + rB)); } // Shapes are overlapped when radii are considered. return(0.0f); } return(distance); }
static unsafe bool CastCollider( Ray ray, ref float2x2 rotation, ref DistanceProxy proxySource, ref DistanceProxy proxyTarget, out ColliderCastHit hit) { hit = default; var transformSource = new PhysicsTransform { Translation = ray.Origin, Rotation = rotation }; // Check we're not initially overlapped. if ((proxySource.VertexCount < 3 || proxyTarget.VertexCount < 3) && OverlapQueries.OverlapConvexConvex(ref transformSource, ref proxySource, ref proxyTarget)) { return(false); } // B = Source // A = Target var radiusSource = proxySource.ConvexRadius; var radiusTarget = proxyTarget.ConvexRadius; var totalRadius = radiusSource + radiusTarget; var invRotation = math.inverse(rotation); var sweepDirection = ray.Displacement; var normal = float2.zero; var lambda = 0.0f; // Initialize the simplex. var simplex = new Simplex(); simplex.Count = 0; var vertices = &simplex.Vertex1; // Get a support point in the inverse direction. var indexTarget = proxyTarget.GetSupport(-sweepDirection); var supportTarget = proxyTarget.Vertices[indexTarget]; var indexSource = proxySource.GetSupport(PhysicsMath.mul(invRotation, sweepDirection)); var supportSource = PhysicsMath.mul(transformSource, proxySource.Vertices[indexSource]); var v = supportTarget - supportSource; // Sigma is the target distance between polygons var sigma = math.max(PhysicsSettings.Constants.MinimumConvexRadius, totalRadius - PhysicsSettings.Constants.MinimumConvexRadius); const float tolerance = PhysicsSettings.Constants.LinearSlop * 0.5f; var iteration = 0; while ( iteration++ < PhysicsSettings.Constants.MaxGJKInterations && math.abs(math.length(v) - sigma) > tolerance ) { if (simplex.Count >= 3) { SafetyChecks.ThrowInvalidOperationException("ColliderCast Simplex must have less than 3 vertex."); } // Support in direction -supportV (Target - Source) indexTarget = proxyTarget.GetSupport(-v); supportTarget = proxyTarget.Vertices[indexTarget]; indexSource = proxySource.GetSupport(PhysicsMath.mul(invRotation, v)); supportSource = PhysicsMath.mul(transformSource, proxySource.Vertices[indexSource]); var p = supportTarget - supportSource; v = math.normalizesafe(v); // Intersect ray with plane. var vp = math.dot(v, p); var vr = math.dot(v, sweepDirection); if (vp - sigma > lambda * vr) { if (vr <= 0.0f) { return(false); } lambda = (vp - sigma) / vr; if (lambda > 1.0f) { return(false); } normal = -v; simplex.Count = 0; } // Reverse simplex since it works with B - A. // Shift by lambda * r because we want the closest point to the current clip point. // Note that the support point p is not shifted because we want the plane equation // to be formed in un-shifted space. var vertex = vertices + simplex.Count; vertex->IndexA = indexSource; vertex->SupportA = supportSource + lambda * sweepDirection; vertex->IndexB = indexTarget; vertex->SupportB = supportTarget; vertex->W = vertex->SupportB - vertex->SupportA; vertex->A = 1.0f; simplex.Count += 1; switch (simplex.Count) { 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) { // Overlap. return(false); } // Get search direction. v = simplex.GetClosestPoint(); } // Ensure we don't process an empty simplex. if (simplex.Count == 0) { return(false); } // Prepare result. var pointSource = float2.zero; var pointTarget = float2.zero; simplex.GetWitnessPoints(ref pointSource, ref pointTarget); normal = math.normalizesafe(-v); hit = new ColliderCastHit { Position = pointTarget + (normal * radiusTarget), SurfaceNormal = normal, Fraction = lambda }; return(true); }
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 }); }
/// <summary> /// Compute the closest points between two shapes. Supports any combination of: CircleShape and /// PolygonShape. The simplex cache is input/output. On the first call set SimplexCache.count to /// zero. /// </summary> /// <param name="output"></param> /// <param name="cache"></param> /// <param name="input"></param> public void GetDistance(DistanceOutput output, SimplexCache cache, DistanceInput input) { GJK_CALLS++; DistanceProxy proxyA = input.ProxyA; DistanceProxy proxyB = input.ProxyB; Transform transformA = input.TransformA; Transform transformB = input.TransformB; // Initialize the simplex. simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB); // Get simplex vertices as an array. SimplexVertex[] vertices = simplex.Vertices; // These store the vertices of the last simplex so that we // can check for duplicates and prevent cycling. // (pooled above) simplex.GetClosestPoint(closestPoint); float distanceSqr1 = closestPoint.LengthSquared(); // Main iteration loop int iter = 0; while (iter < GJK_MAX_ITERS) { // Copy simplex so we can identify duplicates. int saveCount = simplex.Count; for (int i = 0; i < saveCount; i++) { saveA[i] = vertices[i].IndexA; saveB[i] = vertices[i].IndexB; } switch (simplex.Count) { case 1: break; case 2: simplex.Solve2(); break; case 3: simplex.Solve3(); break; default: Debug.Assert(false); break; } // If we have 3 points, then the origin is in the corresponding triangle. if (simplex.Count == 3) { break; } // Compute closest point. simplex.GetClosestPoint(closestPoint); float distanceSqr2 = closestPoint.LengthSquared(); // ensure progress if (distanceSqr2 >= distanceSqr1) { // break; } distanceSqr1 = distanceSqr2; // get search direction; simplex.GetSearchDirection(d); // Ensure the search direction is numerically fit. if (d.LengthSquared() < Settings.EPSILON * Settings.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; } /* * SimplexVertex* vertex = vertices + simplex.m_count; vertex.indexA = * proxyA.GetSupport(MulT(transformA.R, -d)); vertex.wA = Mul(transformA, * proxyA.GetVertex(vertex.indexA)); Vec2 wBLocal; vertex.indexB = * proxyB.GetSupport(MulT(transformB.R, d)); vertex.wB = Mul(transformB, * proxyB.GetVertex(vertex.indexB)); vertex.w = vertex.wB - vertex.wA; */ // Compute a tentative new simplex vertex using support points. SimplexVertex vertex = vertices[simplex.Count]; Rot.MulTransUnsafe(transformA.Q, d.NegateLocal(), temp); vertex.IndexA = proxyA.GetSupport(temp); Transform.MulToOutUnsafe(transformA, proxyA.GetVertex(vertex.IndexA), vertex.WA); // Vec2 wBLocal; Rot.MulTransUnsafe(transformB.Q, d.NegateLocal(), temp); vertex.IndexB = proxyB.GetSupport(temp); Transform.MulToOutUnsafe(transformB, proxyB.GetVertex(vertex.IndexB), vertex.WB); vertex.W.Set(vertex.WB).SubLocal(vertex.WA); // Iteration count is equated to the number of support point calls. ++iter; ++GJK_ITERS; // Check for duplicate support points. This is the main termination criteria. bool duplicate = false; for (int i = 0; i < saveCount; ++i) { if (vertex.IndexA == saveA[i] && vertex.IndexB == saveB[i]) { duplicate = true; break; } } // If we found a duplicate support point we must exit to avoid cycling. if (duplicate) { break; } // New vertex is ok and needed. ++simplex.Count; } GJK_MAX_ITERS = MathUtils.Max(GJK_MAX_ITERS, iter); // Prepare output. simplex.GetWitnessPoints(output.PointA, output.PointB); output.Distance = MathUtils.Distance(output.PointA, output.PointB); output.Iterations = iter; // Cache the simplex. simplex.WriteCache(cache); // Apply radii if requested. if (input.UseRadii) { float rA = proxyA.Radius; float rB = proxyB.Radius; if (output.Distance > rA + rB && output.Distance > Settings.EPSILON) { // Shapes are still no overlapped. // Move the witness points to the outer surface. output.Distance -= (rA + rB); normal.Set(output.PointB).SubLocal(output.PointA); normal.Normalize(); temp.Set(normal).MulLocal(rA); output.PointA.AddLocal(temp); temp.Set(normal).MulLocal(rB); output.PointB.SubLocal(temp); } else { // Shapes are overlapped when radii are considered. // Move the witness points to the middle. // Vec2 p = 0.5f * (output.pointA + output.pointB); output.PointA.AddLocal(output.PointB).MulLocal(.5f); output.PointB.Set(output.PointA); output.Distance = 0.0f; } } }
/// <summary> /// Compute the closest points between two shapes. Supports any combination of: /// b2CircleShape, b2PolygonShape, b2EdgeShape. The simplex cache is input/output. /// On the first call set b2SimplexCache.count to zero. /// </summary> public static void Distance(out DistanceOutput output, SimplexCache cache, DistanceInput input) { ++GjkCalls; DistanceProxy proxyA = input.proxyA; DistanceProxy proxyB = input.proxyB; Transform transformA = input.TransformA; Transform transformB = input.TransformB; // Initialize the simplex. Simplex simplex = new Simplex(); simplex.ReadCache(cache, proxyA, ref transformA, proxyB, ref transformB); // Get simplex vertices as an array. SimplexVertex[] vertices = simplex.Vertices; const int k_maxIters = 20; // These store the vertices of the last simplex so that we // can check for duplicates and prevent cycling. int[] saveA = new int[3], saveB = new int[3]; int saveCount = 0; Vec2 closestPoint = simplex.GetClosestPoint(); float distanceSqr1 = closestPoint.LengthSquared(); float distanceSqr2 = distanceSqr1; // Main iteration loop. int iter = 0; while (iter < k_maxIters) { // Copy simplex so we can identify duplicates. saveCount = simplex.Count; for (int i = 0; i < saveCount; ++i) { saveA[i] = vertices[i].IndexA; saveB[i] = vertices[i].IndexB; } switch (simplex.Count) { case 1: break; case 2: simplex.Solve2(); break; case 3: simplex.Solve3(); break; default: Box2DXDebug.Assert(false); break; } // If we have 3 points, then the origin is in the corresponding triangle. if (simplex.Count == 3) { break; } // Compute closest point. Vec2 p = simplex.GetClosestPoint(); float distanceSqr = p.LengthSquared(); // Ensure progress if (distanceSqr2 >= distanceSqr1) { //break; } distanceSqr1 = distanceSqr2; // Get search direction. Vec2 d = simplex.GetSearchDirection(); // Ensure the search direction is numerically fit. if (d.LengthSquared() < Settings.FLT_EPSILON * Settings.FLT_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. SimplexVertex vertex = vertices[simplex.Count]; vertex.IndexA = proxyA.GetSupport(Math.MulT(transformA.R, -d)); vertex.WA = Math.Mul(transformA, proxyA.GetVertex(vertex.IndexA)); vertex.IndexB = proxyB.GetSupport(Math.MulT(transformB.R, d)); vertex.WB = Math.Mul(transformB, proxyB.GetVertex(vertex.IndexB)); vertex.W = vertex.WB - vertex.WA; // Iteration count is equated to the number of support point calls. ++iter; ++GjkIters; // Check for duplicate support points. This is the main termination criteria. bool duplicate = false; for (int i = 0; i < saveCount; ++i) { if (vertex.IndexA == saveA[i] && vertex.IndexB == saveB[i]) { duplicate = true; break; } } // If we found a duplicate support point we must exit to avoid cycling. if (duplicate) { break; } // New vertex is ok and needed. ++simplex.Count; } GjkMaxIters = Math.Max(GjkMaxIters, iter); // Prepare output. simplex.GetWitnessPoints(out output.PointA, out output.PointB); output.Distance = Vec2.Distance(output.PointA, output.PointB); output.Iterations = iter; // Cache the simplex. simplex.WriteCache(cache); // Apply radii if requested. if (input.UseRadii) { float rA = proxyA._radius; float rB = proxyB._radius; if (output.Distance > rA + rB && output.Distance > Settings.FLT_EPSILON) { // Shapes are still no overlapped. // Move the witness points to the outer surface. output.Distance -= rA + rB; Vec2 normal = output.PointB - output.PointA; normal.Normalize(); output.PointA += rA * normal; output.PointB -= rB * normal; } else { // Shapes are overlapped when radii are considered. // Move the witness points to the middle. Vec2 p = 0.5f * (output.PointA + output.PointB); output.PointA = p; output.PointB = p; output.Distance = 0.0f; } } }