internal void WriteCache(ref SimplexCache cache) { cache.metric = GetMetric(); cache.count = (UInt16)_count; for (int i = 0; i < _count; ++i) { cache.indexA[i] = (byte)(_v[i].indexA); cache.indexB[i] = (byte)(_v[i].indexB); } }
internal void ReadCache(ref SimplexCache cache, Shape shapeA, ref XForm transformA, Shape shapeB, ref XForm transformB) { Debug.Assert(0 <= cache.count && 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 = shapeA.GetVertex(v.indexA); Vector2 wBLocal = shapeB.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_FLT_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 = shapeA.GetVertex(0); Vector2 wBLocal = shapeB.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; } }
public SeparationFunction(ref SimplexCache cache, Shape shapeA, ref XForm transformA, Shape shapeB, ref XForm transformB) { _localPoint = Vector2.Zero; _shapeA = shapeA; _shapeB = shapeB; int count = cache.count; Debug.Assert(0 < count && count < 3); if (count == 1) { _type = SeparationFunctionType.Points; Vector2 localPointA = _shapeA.GetVertex(cache.indexA[0]); Vector2 localPointB = _shapeB.GetVertex(cache.indexB[0]); Vector2 pointA = MathUtils.Multiply(ref transformA, localPointA); Vector2 pointB = MathUtils.Multiply(ref transformB, localPointB); _axis = pointB - pointA; _axis.Normalize(); } else if (cache.indexB[0] == cache.indexB[1]) { // Two points on A and one on B _type = SeparationFunctionType.FaceA; Vector2 localPointA1 = _shapeA.GetVertex(cache.indexA[0]); Vector2 localPointA2 = _shapeA.GetVertex(cache.indexA[1]); Vector2 localPointB = _shapeB.GetVertex(cache.indexB[0]); _localPoint = 0.5f * (localPointA1 + localPointA2); _axis = MathUtils.Cross(localPointA2 - localPointA1, 1.0f); _axis.Normalize(); Vector2 normal = MathUtils.Multiply(ref transformA.R, _axis); Vector2 pointA = MathUtils.Multiply(ref transformA, _localPoint); Vector2 pointB = MathUtils.Multiply(ref transformB, localPointB); float s = Vector2.Dot(pointB - pointA, normal); if (s < 0.0f) { _axis = -_axis; } } else if (cache.indexA[0] == cache.indexA[1]) { // Two points on B and one on A. _type = SeparationFunctionType.FaceB; Vector2 localPointA = shapeA.GetVertex(cache.indexA[0]); Vector2 localPointB1 = shapeB.GetVertex(cache.indexB[0]); Vector2 localPointB2 = shapeB.GetVertex(cache.indexB[1]); _localPoint = 0.5f * (localPointB1 + localPointB2); _axis = MathUtils.Cross(localPointB2 - localPointB1, 1.0f); _axis.Normalize(); Vector2 normal = MathUtils.Multiply(ref transformB.R, _axis); Vector2 pointB = MathUtils.Multiply(ref transformB, _localPoint); Vector2 pointA = MathUtils.Multiply(ref transformA, localPointA); float s = Vector2.Dot(pointA - pointB, normal); if (s < 0.0f) { _axis = -_axis; } } else { // Two points on B and two points on A. // The faces are parallel. Vector2 localPointA1 = _shapeA.GetVertex(cache.indexA[0]); Vector2 localPointA2 = _shapeA.GetVertex(cache.indexA[1]); Vector2 localPointB1 = _shapeB.GetVertex(cache.indexB[0]); Vector2 localPointB2 = _shapeB.GetVertex(cache.indexB[1]); Vector2 pA = MathUtils.Multiply(ref transformA, localPointA1); Vector2 dA = MathUtils.Multiply(ref transformA.R, localPointA2 - localPointA1); Vector2 pB = MathUtils.Multiply(ref transformB, localPointB1); Vector2 dB = MathUtils.Multiply(ref transformB.R, localPointB2 - localPointB1); float a = Vector2.Dot(dA, dA); float e = Vector2.Dot(dB, dB); Vector2 r = pA - pB; float c = Vector2.Dot(dA, r); float f = Vector2.Dot(dB, r); float b = Vector2.Dot(dA, dB); float denom = a * e - b * b; float s = 0.0f; if (denom != 0.0f) { s = MathUtils.Clamp((b * f - c * e) / denom, 0.0f, 1.0f); } float t = (b * s + f) / e; if (t < 0.0f) { t = 0.0f; s = MathUtils.Clamp(-c / a, 0.0f, 1.0f); } else if (t > 1.0f) { t = 1.0f; s = MathUtils.Clamp((b - c) / a, 0.0f, 1.0f); } Vector2 localPointA = localPointA1 + s * (localPointA2 - localPointA1); Vector2 localPointB = localPointB1 + t * (localPointB2 - localPointB1); if (s == 0.0f || s == 1.0f) { _type = SeparationFunctionType.FaceB; _axis = MathUtils.Cross(localPointB2 - localPointB1, 1.0f); _axis.Normalize(); _localPoint = localPointB; Vector2 normal = MathUtils.Multiply(ref transformB.R, _axis); Vector2 pointA = MathUtils.Multiply(ref transformA, localPointA); Vector2 pointB = MathUtils.Multiply(ref transformB, localPointB); float sgn = Vector2.Dot(pointA - pointB, normal); if (sgn < 0.0f) { _axis = -_axis; } } else { _type = SeparationFunctionType.FaceA; _axis = MathUtils.Cross(localPointA2 - localPointA1, 1.0f); _axis.Normalize(); _localPoint = localPointA; Vector2 normal = MathUtils.Multiply(ref transformA.R, _axis); Vector2 pointA = MathUtils.Multiply(ref transformA, localPointA); Vector2 pointB = MathUtils.Multiply(ref transformB, localPointB); float sgn = Vector2.Dot(pointB - pointA, normal); if (sgn < 0.0f) { _axis = -_axis; } } } }
public static void ComputeDistance(out DistanceOutput output, out SimplexCache cache, ref DistanceInput input, Shape shapeA, Shape shapeB) { cache = new SimplexCache(); ++b2_gjkCalls; XForm transformA = input.transformA; XForm transformB = input.transformB; // Initialize the simplex. Simplex simplex = new Simplex(); simplex.ReadCache(ref cache, shapeA, ref transformA, shapeB, ref transformB); // Get simplex vertices as an array. int k_maxIters = 20; // These store the vertices of the last simplex so that we // can check for duplicates and prevent cycling. FixedArray3<int> saveA = new FixedArray3<int>(); FixedArray3<int> saveB = new FixedArray3<int>(); int saveCount = 0; Vector2 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] = simplex._v[i].indexA; saveB[i] = simplex._v[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. Vector2 p = simplex.GetClosestPoint(); distanceSqr2 = p.LengthSquared(); // Ensure progress if (distanceSqr2 >= distanceSqr1) { //break; } distanceSqr1 = distanceSqr2; // Get search direction. Vector2 d = simplex.GetSearchDirection(); // Ensure the search direction is numerically fit. if (d.LengthSquared() < Settings.b2_FLT_EPSILON * Settings.b2_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 = simplex._v[simplex._count]; vertex.indexA = shapeA.GetSupport(MathUtils.MultiplyT(ref transformA.R, -d)); vertex.wA = MathUtils.Multiply(ref transformA, shapeA.GetVertex(vertex.indexA)); //Vector2 wBLocal; vertex.indexB = shapeB.GetSupport(MathUtils.MultiplyT(ref transformB.R, d)); vertex.wB = MathUtils.Multiply(ref transformB, shapeB.GetVertex(vertex.indexB)); vertex.w = vertex.wB - vertex.wA; simplex._v[simplex._count] = vertex; // Iteration count is equated to the number of support point calls. ++iter; ++b2_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; } b2_gjkMaxIters = Math.Max(b2_gjkMaxIters, iter); // Prepare output. simplex.GetWitnessPoints(out output.pointA, out output.pointB); output.distance = (output.pointA - output.pointB).Length(); output.iterations = iter; // Cache the simplex. simplex.WriteCache(ref cache); // Apply radii if requested. if (input.useRadii) { float rA = shapeA._radius; float rB = shapeB._radius; if (output.distance > rA + rB && output.distance > Settings.b2_FLT_EPSILON) { // Shapes are still no overlapped. // Move the witness points to the outer surface. output.distance -= rA + rB; Vector2 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. Vector2 p = 0.5f * (output.pointA + output.pointB); output.pointA = p; output.pointB = p; output.distance = 0.0f; } } }
internal void ReadCache( ref SimplexCache cache, Shape shapeA, ref XForm transformA, Shape shapeB, ref XForm transformB) { Debug.Assert(0 <= cache.count && 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 = shapeA.GetVertex(v.indexA); Vector2 wBLocal = shapeB.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_FLT_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 = shapeA.GetVertex(0); Vector2 wBLocal = shapeB.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; } }
public static void ComputeDistance(out DistanceOutput output, out SimplexCache cache, ref DistanceInput input, Shape shapeA, Shape shapeB) { cache = new SimplexCache(); ++b2_gjkCalls; XForm transformA = input.transformA; XForm transformB = input.transformB; // Initialize the simplex. Simplex simplex = new Simplex(); simplex.ReadCache(ref cache, shapeA, ref transformA, shapeB, ref transformB); // Get simplex vertices as an array. int k_maxIters = 20; // These store the vertices of the last simplex so that we // can check for duplicates and prevent cycling. FixedArray3 <int> saveA = new FixedArray3 <int>(); FixedArray3 <int> saveB = new FixedArray3 <int>(); int saveCount = 0; Vector2 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] = simplex._v[i].indexA; saveB[i] = simplex._v[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. Vector2 p = simplex.GetClosestPoint(); distanceSqr2 = p.LengthSquared(); // Ensure progress if (distanceSqr2 >= distanceSqr1) { //break; } distanceSqr1 = distanceSqr2; // Get search direction. Vector2 d = simplex.GetSearchDirection(); // Ensure the search direction is numerically fit. if (d.LengthSquared() < Settings.b2_FLT_EPSILON * Settings.b2_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 = simplex._v[simplex._count]; vertex.indexA = shapeA.GetSupport(MathUtils.MultiplyT(ref transformA.R, -d)); vertex.wA = MathUtils.Multiply(ref transformA, shapeA.GetVertex(vertex.indexA)); //Vector2 wBLocal; vertex.indexB = shapeB.GetSupport(MathUtils.MultiplyT(ref transformB.R, d)); vertex.wB = MathUtils.Multiply(ref transformB, shapeB.GetVertex(vertex.indexB)); vertex.w = vertex.wB - vertex.wA; simplex._v[simplex._count] = vertex; // Iteration count is equated to the number of support point calls. ++iter; ++b2_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; } b2_gjkMaxIters = Math.Max(b2_gjkMaxIters, iter); // Prepare output. simplex.GetWitnessPoints(out output.pointA, out output.pointB); output.distance = (output.pointA - output.pointB).Length(); output.iterations = iter; // Cache the simplex. simplex.WriteCache(ref cache); // Apply radii if requested. if (input.useRadii) { float rA = shapeA._radius; float rB = shapeB._radius; if (output.distance > rA + rB && output.distance > Settings.b2_FLT_EPSILON) { // Shapes are still no overlapped. // Move the witness points to the outer surface. output.distance -= rA + rB; Vector2 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. Vector2 p = 0.5f * (output.pointA + output.pointB); output.pointA = p; output.pointB = p; output.distance = 0.0f; } } }