/// <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 distance(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) int saveCount = 0; simplex.getClosestPoint(closestPoint); float distanceSqr1 = closestPoint.lengthSquared(); float distanceSqr2 = distanceSqr1; // Main iteration loop int iter = 0; while (iter < GJK_MAX_ITERS) { // 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: Debug.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. simplex.getClosestPoint(closestPoint); 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.m_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_Renamed(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.m_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.m_radius; float rB = proxyB.m_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_Renamed(output.pointB).subLocal(output.pointA); normal.normalize(); temp.set_Renamed(normal).mulLocal(rA); output.pointA.addLocal(temp); temp.set_Renamed(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_Renamed(output.pointA); output.distance = 0.0f; } } }
/// <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; } } }