public void set(SimplexCache sc) { ArrayHelper.Copy(sc.indexA, 0, indexA, 0, indexA.Length); ArrayHelper.Copy(sc.indexB, 0, indexB, 0, indexB.Length); metric = sc.metric; count = sc.count; }
public void writeCache(SimplexCache cache) { cache.metric = getMetric(); cache.count = m_count; for (int i = 0; i < m_count; ++i) { cache.indexA[i] = (vertices[i].indexA); cache.indexB[i] = (vertices[i].indexB); } }
public void readCache(SimplexCache cache, DistanceProxy proxyA, Transform transformA, DistanceProxy proxyB, Transform transformB) { // Copy data from cache. m_count = cache.count; 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); Transform.mulToOutUnsafe(transformA, wALocal, v.wA); Transform.mulToOutUnsafe(transformB, wBLocal, v.wB); v.w.set(v.wB).subLocal(v.wA); v.a = 0.0d; } // Compute the new simplex metric, if it is substantially different than // old metric then flush the simplex. if (m_count > 1) { double metric1 = cache.metric; double metric2 = getMetric(); if (metric2 < 0.5d * metric1 || 2.0d * metric1 < metric2 || metric2 < Settings.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); Transform.mulToOutUnsafe(transformA, wALocal, v.wA); Transform.mulToOutUnsafe(transformB, wBLocal, v.wB); v.w.set(v.wB).subLocal(v.wA); m_count = 1; } }
// TODO_ERIN might not need to return the separation public double initialize(SimplexCache cache, DistanceProxy proxyA, Sweep sweepA, DistanceProxy proxyB, Sweep sweepB, double t1) { m_proxyA = proxyA; m_proxyB = proxyB; int count = cache.count; m_sweepA = sweepA; m_sweepB = sweepB; m_sweepA.getTransform(xfa, t1); m_sweepB.getTransform(xfb, t1); // log.debug("initializing separation." + // "cache: "+cache.count+"-"+cache.metric+"-"+cache.indexA+"-"+cache.indexB+"" // "distance: "+proxyA. if (count == 1) { m_type = Type.POINTS; /* * Vec2 localPointA = m_proxyA.GetVertex(cache.indexA[0]); Vec2 localPointB = * m_proxyB.GetVertex(cache.indexB[0]); Vec2 pointA = Mul(transformA, localPointA); Vec2 * pointB = Mul(transformB, localPointB); m_axis = pointB - pointA; m_axis.Normalize(); */ localPointA.set(m_proxyA.getVertex(cache.indexA[0])); localPointB.set(m_proxyB.getVertex(cache.indexB[0])); Transform.mulToOutUnsafe(xfa, localPointA, pointA); Transform.mulToOutUnsafe(xfb, localPointB, pointB); m_axis.set(pointB).subLocal(pointA); double s = m_axis.normalize(); return(s); } if (cache.indexA[0] == cache.indexA[1]) { // Two points on B and one on A. m_type = Type.FACE_B; localPointB1.set(m_proxyB.getVertex(cache.indexB[0])); localPointB2.set(m_proxyB.getVertex(cache.indexB[1])); temp.set(localPointB2).subLocal(localPointB1); Vec2.crossToOutUnsafe(temp, 1d, m_axis); m_axis.normalize(); Rot.mulToOutUnsafe(xfb.q, m_axis, normal); m_localPoint.set(localPointB1).addLocal(localPointB2).mulLocal(.5d); Transform.mulToOutUnsafe(xfb, m_localPoint, pointB); localPointA.set(proxyA.getVertex(cache.indexA[0])); Transform.mulToOutUnsafe(xfa, localPointA, pointA); temp.set(pointA).subLocal(pointB); double s = Vec2.dot(temp, normal); if (s < 0.0d) { m_axis.negateLocal(); s = -s; } return(s); } else { // Two points on A and one or two points on B. m_type = Type.FACE_A; localPointA1.set(m_proxyA.getVertex(cache.indexA[0])); localPointA2.set(m_proxyA.getVertex(cache.indexA[1])); temp.set(localPointA2).subLocal(localPointA1); Vec2.crossToOutUnsafe(temp, 1.0d, m_axis); m_axis.normalize(); Rot.mulToOutUnsafe(xfa.q, m_axis, normal); m_localPoint.set(localPointA1).addLocal(localPointA2).mulLocal(.5d); Transform.mulToOutUnsafe(xfa, m_localPoint, pointA); localPointB.set(m_proxyB.getVertex(cache.indexB[0])); Transform.mulToOutUnsafe(xfb, localPointB, pointB); temp.set(pointB).subLocal(pointA); double s = Vec2.dot(temp, normal); if (s < 0.0d) { m_axis.negateLocal(); s = -s; } return(s); } }
/** * 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. * * @param output * @param cache * @param input */ 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); double distanceSqr1 = closestPoint.lengthSquared(); double 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; } // 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(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) { double rA = proxyA.m_radius; double rB = proxyB.m_radius; if (output.distance > rA + rB && output.distance > Settings.EPSILON) { // Shapes are still no overlapped. // Move the witness points to the out_er 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.5d * (output.pointA + output.pointB); output.pointA.addLocal(output.pointB).mulLocal(.5d); output.pointB.set(output.pointA); output.distance = 0.0d; } } }