public void ReadCache(SimplexCache cache, DistanceProxy shapeA, ref Transform transformA, DistanceProxy shapeB, ref Transform transformB) { Box2DXDebug.Assert(0 <= cache.Count && cache.Count <= 3); // Copy data from cache. Count = cache.Count; for (int i = 0; i < Count; ++i) { SimplexVertex v = Vertices[i]; v.IndexA = cache.IndexA[i]; v.IndexB = cache.IndexB[i]; Vec2 wALocal = shapeA.GetVertex(v.IndexA); Vec2 wBLocal = shapeB.GetVertex(v.IndexB); v.WA = Math.Mul(transformA, wALocal); v.WB = Math.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 (Count > 1) { float metric1 = cache.Metric; float metric2 = GetMetric(); if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Settings.FLT_EPSILON) { // Reset the simplex. Count = 0; } } // If the cache is empty or invalid ... if (Count == 0) { SimplexVertex v = Vertices[0]; v.IndexA = 0; v.IndexB = 0; Vec2 wALocal = shapeA.GetVertex(0); Vec2 wBLocal = shapeB.GetVertex(0); v.WA = Math.Mul(transformA, wALocal); v.WB = Math.Mul(transformB, wBLocal); v.W = v.WB - v.WA; Count = 1; } }
public void ReadCache(SimplexCache cache, DistanceProxy proxyA, Transform transformA, DistanceProxy proxyB, Transform transformB) { Debug.Assert(cache.Count <= 3); // Copy data from cache. Count = cache.Count; for (int i = 0; i < 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); Transform.MulToOutUnsafe(transformA, wALocal, v.WA); Transform.MulToOutUnsafe(transformB, wBLocal, v.WB); v.W.Set(v.WB).SubLocal(v.WA); v.A = 0.0f; } // 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 = Metric; if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Settings.EPSILON) { // Reset the simplex. Count = 0; } } // If the cache is empty or invalid ... if (Count == 0) { SimplexVertex v = Vertices[0]; v.IndexA = 0; v.IndexB = 0; Vec2 wALocal = proxyA.GetVertex(0); Vec2 wBLocal = proxyB.GetVertex(0); Transform.MulToOutUnsafe(transformA, wALocal, v.WA); Transform.MulToOutUnsafe(transformB, wBLocal, v.WB); v.W.Set(v.WB).SubLocal(v.WA); Count = 1; } }
void ReadCache(SimplexCache cache, DistanceProxy proxyA, XForm transformA, DistanceProxy proxyB, XForm transformB) { // Copy data from cache. m_count = cache.count; SimplexVertex[] vertices = m_v1; 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 = MathB2.Mul(transformA, wALocal); v.wB = MathB2.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 < Settings.FLT_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 = MathB2.Mul(transformA, wALocal); v.wB = MathB2.Mul(transformB, wBLocal); v.w = v.wB - v.wA; v.a = 1.0f; m_count = 1; } }
public void Initialize(SimplexCache cache, DistanceProxy proxyA, Transform transformA, DistanceProxy proxyB, Transform transformB) { _proxyA = proxyA; _proxyB = proxyB; int count = cache.Count; Box2DXDebug.Assert(0 < count && count < 3); if (count == 1) { _type = Type.Points; Vec2 localPointA = _proxyA.GetVertex(cache.IndexA[0]); Vec2 localPointB = _proxyB.GetVertex(cache.IndexB[0]); Vec2 pointA = Math.Mul(transformA, localPointA); Vec2 pointB = Math.Mul(transformB, localPointB); _axis = pointB - pointA; _axis.Normalize(); } else if (cache.IndexB[0] == cache.IndexB[1]) { // Two points on A and one on B _type = Type.FaceA; Vec2 localPointA1 = _proxyA.GetVertex(cache.IndexA[0]); Vec2 localPointA2 = _proxyA.GetVertex(cache.IndexA[1]); Vec2 localPointB = _proxyB.GetVertex(cache.IndexB[0]); _localPoint = 0.5f*(localPointA1 + localPointA2); _axis = Vec2.Cross(localPointA2 - localPointA1, 1.0f); _axis.Normalize(); Vec2 normal = Math.Mul(transformA.R, _axis); Vec2 pointA = Math.Mul(transformA, _localPoint); Vec2 pointB = Math.Mul(transformB, localPointB); float s = Vec2.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 = Type.FaceB; Vec2 localPointA = proxyA.GetVertex(cache.IndexA[0]); Vec2 localPointB1 = proxyB.GetVertex(cache.IndexB[0]); Vec2 localPointB2 = proxyB.GetVertex(cache.IndexB[1]); _localPoint = 0.5f*(localPointB1 + localPointB2); _axis = Vec2.Cross(localPointB2 - localPointB1, 1.0f); _axis.Normalize(); Vec2 normal = Math.Mul(transformB.R, _axis); Vec2 pointB = Math.Mul(transformB, _localPoint); Vec2 pointA = Math.Mul(transformA, localPointA); float s = Vec2.Dot(pointA - pointB, normal); if (s < 0.0f) { _axis = -_axis; } } else { // Two points on B and two points on A. // The faces are parallel. Vec2 localPointA1 = _proxyA.GetVertex(cache.IndexA[0]); Vec2 localPointA2 = _proxyA.GetVertex(cache.IndexA[1]); Vec2 localPointB1 = _proxyB.GetVertex(cache.IndexB[0]); Vec2 localPointB2 = _proxyB.GetVertex(cache.IndexB[1]); Vec2 pA = Math.Mul(transformA, localPointA1); Vec2 dA = Math.Mul(transformA.R, localPointA2 - localPointA1); Vec2 pB = Math.Mul(transformB, localPointB1); Vec2 dB = Math.Mul(transformB.R, localPointB2 - localPointB1); float a = Vec2.Dot(dA, dA); float e = Vec2.Dot(dB, dB); Vec2 r = pA - pB; float c = Vec2.Dot(dA, r); float f = Vec2.Dot(dB, r); float b = Vec2.Dot(dA, dB); float denom = a*e - b*b; float s = 0.0f; if (denom != 0.0f) { s = Math.Clamp((b*f - c*e)/denom, 0.0f, 1.0f); } float t = (b*s + f)/e; if (t < 0.0f) { t = 0.0f; s = Math.Clamp(-c/a, 0.0f, 1.0f); } else if (t > 1.0f) { t = 1.0f; s = Math.Clamp((b - c)/a, 0.0f, 1.0f); } Vec2 localPointA = localPointA1 + s*(localPointA2 - localPointA1); Vec2 localPointB = localPointB1 + t*(localPointB2 - localPointB1); if (s == 0.0f || s == 1.0f) { _type = Type.FaceB; _axis = Vec2.Cross(localPointB2 - localPointB1, 1.0f); _axis.Normalize(); _localPoint = localPointB; Vec2 normal = Math.Mul(transformB.R, _axis); Vec2 pointA = Math.Mul(transformA, localPointA); Vec2 pointB = Math.Mul(transformB, localPointB); float sgn = Vec2.Dot(pointA - pointB, normal); if (sgn < 0.0f) { _axis = -_axis; } } else { _type = Type.FaceA; _axis = Vec2.Cross(localPointA2 - localPointA1, 1.0f); _axis.Normalize(); _localPoint = localPointA; Vec2 normal = Math.Mul(transformA.R, _axis); Vec2 pointA = Math.Mul(transformA, localPointA); Vec2 pointB = Math.Mul(transformB, localPointB); float sgn = Vec2.Dot(pointB - pointA, normal); if (sgn < 0.0f) { _axis = -_axis; } } } }
/// <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; } } }
public void Initialize(SimplexCache cache, DistanceProxy proxyA, Transform transformA, DistanceProxy proxyB, Transform transformB) { _proxyA = proxyA; _proxyB = proxyB; int count = cache.Count; Box2DXDebug.Assert(0 < count && count < 3); if (count == 1) { _type = Type.Points; Vec2 localPointA = _proxyA.GetVertex(cache.IndexA[0]); Vec2 localPointB = _proxyB.GetVertex(cache.IndexB[0]); Vec2 pointA = Math.Mul(transformA, localPointA); Vec2 pointB = Math.Mul(transformB, localPointB); _axis = pointB - pointA; _axis.Normalize(); } else if (cache.IndexB[0] == cache.IndexB[1]) { // Two points on A and one on B _type = Type.FaceA; Vec2 localPointA1 = _proxyA.GetVertex(cache.IndexA[0]); Vec2 localPointA2 = _proxyA.GetVertex(cache.IndexA[1]); Vec2 localPointB = _proxyB.GetVertex(cache.IndexB[0]); _localPoint = 0.5f * (localPointA1 + localPointA2); _axis = Vec2.Cross(localPointA2 - localPointA1, 1.0f); _axis.Normalize(); Vec2 normal = Math.Mul(transformA.R, _axis); Vec2 pointA = Math.Mul(transformA, _localPoint); Vec2 pointB = Math.Mul(transformB, localPointB); float s = Vec2.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 = Type.FaceB; Vec2 localPointA = proxyA.GetVertex(cache.IndexA[0]); Vec2 localPointB1 = proxyB.GetVertex(cache.IndexB[0]); Vec2 localPointB2 = proxyB.GetVertex(cache.IndexB[1]); _localPoint = 0.5f * (localPointB1 + localPointB2); _axis = Vec2.Cross(localPointB2 - localPointB1, 1.0f); _axis.Normalize(); Vec2 normal = Math.Mul(transformB.R, _axis); Vec2 pointB = Math.Mul(transformB, _localPoint); Vec2 pointA = Math.Mul(transformA, localPointA); float s = Vec2.Dot(pointA - pointB, normal); if (s < 0.0f) { _axis = -_axis; } } else { // Two points on B and two points on A. // The faces are parallel. Vec2 localPointA1 = _proxyA.GetVertex(cache.IndexA[0]); Vec2 localPointA2 = _proxyA.GetVertex(cache.IndexA[1]); Vec2 localPointB1 = _proxyB.GetVertex(cache.IndexB[0]); Vec2 localPointB2 = _proxyB.GetVertex(cache.IndexB[1]); Vec2 pA = Math.Mul(transformA, localPointA1); Vec2 dA = Math.Mul(transformA.R, localPointA2 - localPointA1); Vec2 pB = Math.Mul(transformB, localPointB1); Vec2 dB = Math.Mul(transformB.R, localPointB2 - localPointB1); float a = Vec2.Dot(dA, dA); float e = Vec2.Dot(dB, dB); Vec2 r = pA - pB; float c = Vec2.Dot(dA, r); float f = Vec2.Dot(dB, r); float b = Vec2.Dot(dA, dB); float denom = a * e - b * b; float s = 0.0f; if (denom != 0.0f) { s = Math.Clamp((b * f - c * e) / denom, 0.0f, 1.0f); } float t = (b * s + f) / e; if (t < 0.0f) { t = 0.0f; s = Math.Clamp(-c / a, 0.0f, 1.0f); } else if (t > 1.0f) { t = 1.0f; s = Math.Clamp((b - c) / a, 0.0f, 1.0f); } Vec2 localPointA = localPointA1 + s * (localPointA2 - localPointA1); Vec2 localPointB = localPointB1 + t * (localPointB2 - localPointB1); if (s == 0.0f || s == 1.0f) { _type = Type.FaceB; _axis = Vec2.Cross(localPointB2 - localPointB1, 1.0f); _axis.Normalize(); _localPoint = localPointB; Vec2 normal = Math.Mul(transformB.R, _axis); Vec2 pointA = Math.Mul(transformA, localPointA); Vec2 pointB = Math.Mul(transformB, localPointB); float sgn = Vec2.Dot(pointA - pointB, normal); if (sgn < 0.0f) { _axis = -_axis; } } else { _type = Type.FaceA; _axis = Vec2.Cross(localPointA2 - localPointA1, 1.0f); _axis.Normalize(); _localPoint = localPointA; Vec2 normal = Math.Mul(transformA.R, _axis); Vec2 pointA = Math.Mul(transformA, localPointA); Vec2 pointB = Math.Mul(transformB, localPointB); float sgn = Vec2.Dot(pointB - pointA, normal); if (sgn < 0.0f) { _axis = -_axis; } } } }