public void ReadCache(SimplexCache cache, DistanceProxy proxyA, Transform transformA, DistanceProxy proxyB, Transform transformB) { Utilities.Assert(cache.count <= 3); // Copy data from cache. m_count = cache.count; SimplexVertex[] vertices = this.verticies; for (int i = 0; i < m_count; ++i) { SimplexVertex v = vertices[i]; v.indexA = cache.indexA[i]; v.indexB = cache.indexB[i]; Vec2 wALocal = proxyA.GetVertex(v.indexA); Vec2 wBLocal = proxyB.GetVertex(v.indexB); v.wA = Utilities.Mul(transformA, wALocal); v.wB = Utilities.Mul(transformB, wBLocal); v.w = v.wB - v.wA; v.a = 0.0f; } // Compute the new simplex metric, if it is substantially different than // old metric then flush the simplex. if (m_count > 1) { float metric1 = cache.metric; float metric2 = GetMetric(); if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Single.Epsilon) { // Reset the simplex. m_count = 0; } } // If the cache is empty or invalid ... if (m_count == 0) { SimplexVertex v = vertices[0]; v.indexA = 0; v.indexB = 0; Vec2 wALocal = proxyA.GetVertex(0); Vec2 wBLocal = proxyB.GetVertex(0); v.wA = Utilities.Mul(transformA, wALocal); v.wB = Utilities.Mul(transformB, wBLocal); v.w = v.wB - v.wA; v.a = 1.0f; m_count = 1; } }
internal void ReadCache(ref SimplexCache cache, ref DistanceProxy proxyA, ref Transform transformA, ref DistanceProxy proxyB, ref Transform transformB) { //Debug.Assert(cache.count <= 3); // Copy data from cache. _count = cache.count; for (int i = 0; i < _count; ++i) { SimplexVertex v = _v[i]; v.indexA = cache.indexA[i]; v.indexB = cache.indexB[i]; Vector2 wALocal = proxyA.GetVertex(v.indexA); Vector2 wBLocal = proxyB.GetVertex(v.indexB); v.wA = MathUtils.Multiply(ref transformA, wALocal); v.wB = MathUtils.Multiply(ref transformB, wBLocal); v.w = v.wB - v.wA; v.a = 0.0f; _v[i] = v; } // Compute the new simplex metric, if it is substantially different than // old metric then flush the simplex. if (_count > 1) { float metric1 = cache.metric; float metric2 = GetMetric(); if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Settings.b2_epsilon) { // Reset the simplex. _count = 0; } } // If the cache is empty or invalid ... if (_count == 0) { SimplexVertex v = _v[0]; v.indexA = 0; v.indexB = 0; Vector2 wALocal = proxyA.GetVertex(0); Vector2 wBLocal = proxyB.GetVertex(0); v.wA = MathUtils.Multiply(ref transformA, wALocal); v.wB = MathUtils.Multiply(ref transformB, wBLocal); v.w = v.wB - v.wA; _v[0] = v; _count = 1; } }
// TODO_ERIN might not need to return the separation float Initialize(SimplexCache cache, DistanceProxy proxyA, Sweep sweepA, DistanceProxy proxyB, Sweep sweepB, float t1) { m_proxyA = proxyA; m_proxyB = proxyB; int count = cache.count; Utilities.Assert(0 < count && count < 3); m_sweepA = sweepA; m_sweepB = sweepB; Transform xfA, xfB; m_sweepA.GetTransform(out xfA, t1); m_sweepB.GetTransform(out xfB, t1); if (count == 1) { m_type = SeparationType.e_points; Vec2 localPointA = m_proxyA.GetVertex(cache.indexA[0]); Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]); Vec2 pointA = Utilities.Mul(xfA, localPointA); Vec2 pointB = Utilities.Mul(xfB, localPointB); m_axis = pointB - pointA; float s = m_axis.Normalize(); return(s); } else if (cache.indexA[0] == cache.indexA[1]) { // Two points on B and one on A. m_type = SeparationType.e_faceB; Vec2 localPointB1 = proxyB.GetVertex(cache.indexB[0]); Vec2 localPointB2 = proxyB.GetVertex(cache.indexB[1]); m_axis = Utilities.Cross(localPointB2 - localPointB1, 1.0f); m_axis.Normalize(); Vec2 normal = Utilities.Mul(xfB.q, m_axis); m_localPoint = 0.5f * (localPointB1 + localPointB2); Vec2 pointB = Utilities.Mul(xfB, m_localPoint); Vec2 localPointA = proxyA.GetVertex(cache.indexA[0]); Vec2 pointA = Utilities.Mul(xfA, localPointA); float s = Utilities.Dot(pointA - pointB, normal); if (s < 0.0f) { m_axis = -m_axis; s = -s; } return(s); } else { // Two points on A and one or two points on B. m_type = SeparationType.e_faceA; Vec2 localPointA1 = m_proxyA.GetVertex(cache.indexA[0]); Vec2 localPointA2 = m_proxyA.GetVertex(cache.indexA[1]); m_axis = Utilities.Cross(localPointA2 - localPointA1, 1.0f); m_axis.Normalize(); Vec2 normal = Utilities.Mul(xfA.q, m_axis); m_localPoint = 0.5f * (localPointA1 + localPointA2); Vec2 pointA = Utilities.Mul(xfA, m_localPoint); Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]); Vec2 pointB = Utilities.Mul(xfB, localPointB); float s = Utilities.Dot(pointB - pointA, normal); if (s < 0.0f) { m_axis = -m_axis; s = -s; } return(s); } }
public SeparationFunction(ref SimplexCache cache, ref DistanceProxy proxyA, ref Sweep sweepA, ref DistanceProxy proxyB, ref Sweep sweepB, float t1) { _localPoint = Vector2.zero; _proxyA = proxyA; _proxyB = proxyB; int count = cache.count; //Debug.Assert(0 < count && count < 3); _sweepA = sweepA; _sweepB = sweepB; Transform xfA, xfB; _sweepA.GetTransform(out xfA, t1); _sweepB.GetTransform(out xfB, t1); if (count == 1) { _type = SeparationFunctionType.Points; Vector2 localPointA = _proxyA.GetVertex(cache.indexA[0]); Vector2 localPointB = _proxyB.GetVertex(cache.indexB[0]); Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA); Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB); _axis = pointB - pointA; _axis.Normalize(); return; } else if (cache.indexA[0] == cache.indexA[1]) { // Two points on B and one on A. _type = SeparationFunctionType.FaceB; Vector2 localPointB1 = proxyB.GetVertex(cache.indexB[0]); Vector2 localPointB2 = proxyB.GetVertex(cache.indexB[1]); _axis = MathUtils.Cross(localPointB2 - localPointB1, 1.0f); _axis.Normalize(); Vector2 normal = MathUtils.Multiply(ref xfB.R, _axis); _localPoint = 0.5f * (localPointB1 + localPointB2); Vector2 pointB = MathUtils.Multiply(ref xfB, _localPoint); Vector2 localPointA = proxyA.GetVertex(cache.indexA[0]); Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA); float s = Vector2.Dot(pointA - pointB, normal); if (s < 0.0f) { _axis = -_axis; s = -s; } return; } else { // Two points on A and one or two points on B. _type = SeparationFunctionType.FaceA; Vector2 localPointA1 = _proxyA.GetVertex(cache.indexA[0]); Vector2 localPointA2 = _proxyA.GetVertex(cache.indexA[1]); _axis = MathUtils.Cross(localPointA2 - localPointA1, 1.0f); _axis.Normalize(); Vector2 normal = MathUtils.Multiply(ref xfA.R, _axis); _localPoint = 0.5f * (localPointA1 + localPointA2); Vector2 pointA = MathUtils.Multiply(ref xfA, _localPoint); Vector2 localPointB = _proxyB.GetVertex(cache.indexB[0]); Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB); float s = Vector2.Dot(pointB - pointA, normal); if (s < 0.0f) { _axis = -_axis; s = -s; } return; } }
/// Compute the closest points between two shapes. Supports any combination of: /// CircleShape, PolygonShape, EdgeShape. The simplex cache is input/output. /// On the first call set SimplexCache.count to zero. 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, transformA, proxyB, transformB); // Get simplex vertices as an array. SimplexVertex[] vertices = simplex.verticies; 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]; int[] saveB = new int[3]; int saveCount = 0; float distanceSqr1 = Single.MaxValue; float distanceSqr2 = distanceSqr1; // Main iteration loop. int iter = 0; while (iter < k_maxIters) { // Copy simplex so we can identify duplicates. saveCount = simplex.m_count; for (int i = 0; i < saveCount; ++i) { saveA[i] = vertices[i].indexA; saveB[i] = vertices[i].indexB; } switch (simplex.m_count) { case 1: break; case 2: simplex.Solve2(); break; case 3: simplex.Solve3(); break; default: Utilities.Assert(false); break; } // If we have 3 points, then the origin is in the corresponding triangle. if (simplex.m_count == 3) { break; } // Compute closest point. Vec2 p = simplex.GetClosestPoint(); distanceSqr2 = 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() < Single.Epsilon * Single.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.m_count]; vertex.indexA = proxyA.GetSupport(Utilities.MulT(transformA.q, -d)); vertex.wA = Utilities.Mul(transformA, proxyA.GetVertex(vertex.indexA)); Vec2 wBLocal; vertex.indexB = proxyB.GetSupport(Utilities.MulT(transformB.q, d)); vertex.wB = Utilities.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.m_count; } _gjkMaxIters = Math.Max(_gjkMaxIters, iter); // Prepare output. simplex.GetWitnessPoints(out output.pointA, out output.pointB); output.distance = Utilities.Distance(output.pointA, output.pointB); output.iterations = iter; // Cache the simplex. simplex.WriteCache(cache); // Apply radii if requested. if (input.useRadii) { float rA = proxyA.m_radius; float rB = proxyB.m_radius; if (output.distance > rA + rB && output.distance > Single.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; } } }
// TODO_ERIN might not need to return the separation float Initialize(SimplexCache cache, DistanceProxy proxyA, Sweep sweepA, DistanceProxy proxyB, Sweep sweepB, float t1) { m_proxyA = proxyA; m_proxyB = proxyB; int count = cache.count; Utilities.Assert(0 < count && count < 3); m_sweepA = sweepA; m_sweepB = sweepB; Transform xfA, xfB; m_sweepA.GetTransform(out xfA, t1); m_sweepB.GetTransform(out xfB, t1); if (count == 1) { m_type = SeparationType.e_points; Vec2 localPointA = m_proxyA.GetVertex(cache.indexA[0]); Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]); Vec2 pointA = Utilities.Mul(xfA, localPointA); Vec2 pointB = Utilities.Mul(xfB, localPointB); m_axis = pointB - pointA; float s = m_axis.Normalize(); return s; } else if (cache.indexA[0] == cache.indexA[1]) { // Two points on B and one on A. m_type = SeparationType.e_faceB; Vec2 localPointB1 = proxyB.GetVertex(cache.indexB[0]); Vec2 localPointB2 = proxyB.GetVertex(cache.indexB[1]); m_axis = Utilities.Cross(localPointB2 - localPointB1, 1.0f); m_axis.Normalize(); Vec2 normal = Utilities.Mul(xfB.q, m_axis); m_localPoint = 0.5f * (localPointB1 + localPointB2); Vec2 pointB = Utilities.Mul(xfB, m_localPoint); Vec2 localPointA = proxyA.GetVertex(cache.indexA[0]); Vec2 pointA = Utilities.Mul(xfA, localPointA); float s = Utilities.Dot(pointA - pointB, normal); if (s < 0.0f) { m_axis = -m_axis; s = -s; } return s; } else { // Two points on A and one or two points on B. m_type = SeparationType.e_faceA; Vec2 localPointA1 = m_proxyA.GetVertex(cache.indexA[0]); Vec2 localPointA2 = m_proxyA.GetVertex(cache.indexA[1]); m_axis = Utilities.Cross(localPointA2 - localPointA1, 1.0f); m_axis.Normalize(); Vec2 normal = Utilities.Mul(xfA.q, m_axis); m_localPoint = 0.5f * (localPointA1 + localPointA2); Vec2 pointA = Utilities.Mul(xfA, m_localPoint); Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]); Vec2 pointB = Utilities.Mul(xfB, localPointB); float s = Utilities.Dot(pointB - pointA, normal); if (s < 0.0f) { m_axis = -m_axis; s = -s; } return s; } }