distance method

void distance(
  1. DistanceOutput output,
  2. SimplexCache cache,
  3. DistanceInput input
)

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.

Implementation

void distance(final DistanceOutput output, final SimplexCache cache,
    final DistanceInput input) {
  GJK_CALLS++;

  final DistanceProxy proxyA = input.proxyA;
  final 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.
  List<_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.length2;
  double distanceSqr2 = distanceSqr1;

  // Main iteration loop
  int iter = 0;
  while (iter < MAX_ITERS) {
    // 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:
        assert(false);
    }

    // If we have 3 points, then the origin is in the corresponding triangle.
    if (_simplex.count == 3) {
      break;
    }

    // Compute closest point.
    _simplex.getClosestPoint(_closestPoint);
    distanceSqr2 = _closestPoint.length2;

    // ensure progress
    if (distanceSqr2 >= distanceSqr1) {
      // break;
    }
    distanceSqr1 = distanceSqr2;

    // get search direction;
    _simplex.getSearchDirection(_d);

    // Ensure the search direction is numerically fit.
    if (_d.length2 < 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.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.mulTransUnsafeVec2(transformA.q, _d..negate(), _temp);
    vertex.indexA = proxyA.getSupport(_temp);
    Transform.mulToOutUnsafeVec2(
        transformA, proxyA.getVertex(vertex.indexA), vertex.wA);
    // Vec2 wBLocal;
    Rot.mulTransUnsafeVec2(transformB.q, _d..negate(), _temp);
    vertex.indexB = proxyB.getSupport(_temp);
    Transform.mulToOutUnsafeVec2(
        transformB, proxyB.getVertex(vertex.indexB), vertex.wB);
    (vertex.w..setFrom(vertex.wB)).sub(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 = Math.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.radius;
    double 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
        ..setFrom(output.pointB)
        ..sub(output.pointA);
      _normal.normalize();
      _temp
        ..setFrom(_normal)
        ..scale(rA);
      output.pointA.add(_temp);
      _temp
        ..setFrom(_normal)
        ..scale(rB);
      output.pointB.sub(_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
        ..add(output.pointB)
        ..scale(.5);
      output.pointB.setFrom(output.pointA);
      output.distance = 0.0;
    }
  }
}