Esempio n. 1
0
    /** Solve a line segment using barycentric coordinates. */
    public void solve2() {
      // Solve a line segment using barycentric coordinates.
      //
      // p = a1 * w1 + a2 * w2
      // a1 + a2 = 1
      //
      // The vector from the origin to the closest point on the line is
      // perpendicular to the line.
      // e12 = w2 - w1
      // dot(p, e) = 0
      // a1 * dot(w1, e) + a2 * dot(w2, e) = 0
      //
      // 2-by-2 linear system
      // [1 1 ][a1] = [1]
      // [w1.e12 w2.e12][a2] = [0]
      //
      // Define
      // d12_1 = dot(w2, e12)
      // d12_2 = -dot(w1, e12)
      // d12 = d12_1 + d12_2
      //
      // Solution
      // a1 = d12_1 / d12
      // a2 = d12_2 / d12
      final Vec2 w1 = m_v1.w;
      final Vec2 w2 = m_v2.w;
      e12.set(w2).subLocal(w1);

      // w1 region
      float d12_2 = -Vec2.dot(w1, e12);
      if (d12_2 <= 0.0f) {
        // a2 <= 0, so we clamp it to 0
        m_v1.a = 1.0f;
        m_count = 1;
        return;
      }

      // w2 region
      float d12_1 = Vec2.dot(w2, e12);
      if (d12_1 <= 0.0f) {
        // a1 <= 0, so we clamp it to 0
        m_v2.a = 1.0f;
        m_count = 1;
        m_v1.set(m_v2);
        return;
      }

      // Must be in e12 region.
      float inv_d12 = 1.0f / (d12_1 + d12_2);
      m_v1.a = d12_1 * inv_d12;
      m_v2.a = d12_2 * inv_d12;
      m_count = 2;
    }
Esempio n. 2
0
    public void readCache(
        SimplexCache cache,
        DistanceProxy proxyA,
        Transform transformA,
        DistanceProxy proxyB,
        Transform transformB) {
      assert (cache.count <= 3);

      // 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.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.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;
      }
    }
Esempio n. 3
0
  /**
   * 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 final 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.
    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 < 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:
          assert (false);
      }

      // 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.
      boolean 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(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;
      }
    }
  }
Esempio n. 4
0
    /**
     * Solve a line segment using barycentric coordinates.<br>
     * Possible regions:<br>
     * - points[2]<br>
     * - edge points[0]-points[2]<br>
     * - edge points[1]-points[2]<br>
     * - inside the triangle
     */
    public void solve3() {
      w1.set(m_v1.w);
      w2.set(m_v2.w);
      w3.set(m_v3.w);

      // Edge12
      // [1 1 ][a1] = [1]
      // [w1.e12 w2.e12][a2] = [0]
      // a3 = 0
      e12.set(w2).subLocal(w1);
      float w1e12 = Vec2.dot(w1, e12);
      float w2e12 = Vec2.dot(w2, e12);
      float d12_1 = w2e12;
      float d12_2 = -w1e12;

      // Edge13
      // [1 1 ][a1] = [1]
      // [w1.e13 w3.e13][a3] = [0]
      // a2 = 0
      e13.set(w3).subLocal(w1);
      float w1e13 = Vec2.dot(w1, e13);
      float w3e13 = Vec2.dot(w3, e13);
      float d13_1 = w3e13;
      float d13_2 = -w1e13;

      // Edge23
      // [1 1 ][a2] = [1]
      // [w2.e23 w3.e23][a3] = [0]
      // a1 = 0
      e23.set(w3).subLocal(w2);
      float w2e23 = Vec2.dot(w2, e23);
      float w3e23 = Vec2.dot(w3, e23);
      float d23_1 = w3e23;
      float d23_2 = -w2e23;

      // Triangle123
      float n123 = Vec2.cross(e12, e13);

      float d123_1 = n123 * Vec2.cross(w2, w3);
      float d123_2 = n123 * Vec2.cross(w3, w1);
      float d123_3 = n123 * Vec2.cross(w1, w2);

      // w1 region
      if (d12_2 <= 0.0f && d13_2 <= 0.0f) {
        m_v1.a = 1.0f;
        m_count = 1;
        return;
      }

      // e12
      if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f) {
        float inv_d12 = 1.0f / (d12_1 + d12_2);
        m_v1.a = d12_1 * inv_d12;
        m_v2.a = d12_2 * inv_d12;
        m_count = 2;
        return;
      }

      // e13
      if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f) {
        float inv_d13 = 1.0f / (d13_1 + d13_2);
        m_v1.a = d13_1 * inv_d13;
        m_v3.a = d13_2 * inv_d13;
        m_count = 2;
        m_v2.set(m_v3);
        return;
      }

      // w2 region
      if (d12_1 <= 0.0f && d23_2 <= 0.0f) {
        m_v2.a = 1.0f;
        m_count = 1;
        m_v1.set(m_v2);
        return;
      }

      // w3 region
      if (d13_1 <= 0.0f && d23_1 <= 0.0f) {
        m_v3.a = 1.0f;
        m_count = 1;
        m_v1.set(m_v3);
        return;
      }

      // e23
      if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f) {
        float inv_d23 = 1.0f / (d23_1 + d23_2);
        m_v2.a = d23_1 * inv_d23;
        m_v3.a = d23_2 * inv_d23;
        m_count = 2;
        m_v1.set(m_v3);
        return;
      }

      // Must be in triangle123
      float inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);
      m_v1.a = d123_1 * inv_d123;
      m_v2.a = d123_2 * inv_d123;
      m_v3.a = d123_3 * inv_d123;
      m_count = 3;
    }