Ejemplo n.º 1
0
    // djm pooled, from above
    public float getMetric() {
      switch (m_count) {
        case 0:
          assert (false);
          return 0.0f;

        case 1:
          return 0.0f;

        case 2:
          return MathUtils.distance(m_v1.w, m_v2.w);

        case 3:
          case3.set(m_v2.w).subLocal(m_v1.w);
          case33.set(m_v3.w).subLocal(m_v1.w);
          // return Vec2.cross(m_v2.w - m_v1.w, m_v3.w - m_v1.w);
          return Vec2.cross(case3, case33);

        default:
          assert (false);
          return 0.0f;
      }
    }
Ejemplo n.º 2
0
  @Override
  public boolean solvePositionConstraints(final SolverData data) {
    final Rot qA = pool.popRot();
    final Rot qB = pool.popRot();
    final Vec2 rA = pool.popVec2();
    final Vec2 rB = pool.popVec2();
    final Vec2 uA = pool.popVec2();
    final Vec2 uB = pool.popVec2();
    final Vec2 temp = pool.popVec2();
    final Vec2 PA = pool.popVec2();
    final Vec2 PB = pool.popVec2();

    Vec2 cA = data.positions[m_indexA].c;
    float aA = data.positions[m_indexA].a;
    Vec2 cB = data.positions[m_indexB].c;
    float aB = data.positions[m_indexB].a;

    qA.set(aA);
    qB.set(aB);

    Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA);
    Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB);

    uA.set(cA).addLocal(rA).subLocal(m_groundAnchorA);
    uB.set(cB).addLocal(rB).subLocal(m_groundAnchorB);

    float lengthA = uA.length();
    float lengthB = uB.length();

    if (lengthA > 10.0f * Settings.linearSlop) {
      uA.mulLocal(1.0f / lengthA);
    } else {
      uA.setZero();
    }

    if (lengthB > 10.0f * Settings.linearSlop) {
      uB.mulLocal(1.0f / lengthB);
    } else {
      uB.setZero();
    }

    // Compute effective mass.
    float ruA = Vec2.cross(rA, uA);
    float ruB = Vec2.cross(rB, uB);

    float mA = m_invMassA + m_invIA * ruA * ruA;
    float mB = m_invMassB + m_invIB * ruB * ruB;

    float mass = mA + m_ratio * m_ratio * mB;

    if (mass > 0.0f) {
      mass = 1.0f / mass;
    }

    float C = m_constant - lengthA - m_ratio * lengthB;
    float linearError = MathUtils.abs(C);

    float impulse = -mass * C;

    PA.set(uA).mulLocal(-impulse);
    PB.set(uB).mulLocal(-m_ratio * impulse);

    cA.x += m_invMassA * PA.x;
    cA.y += m_invMassA * PA.y;
    aA += m_invIA * Vec2.cross(rA, PA);
    cB.x += m_invMassB * PB.x;
    cB.y += m_invMassB * PB.y;
    aB += m_invIB * Vec2.cross(rB, PB);

    //    data.positions[m_indexA].c.set(cA);
    data.positions[m_indexA].a = aA;
    //    data.positions[m_indexB].c.set(cB);
    data.positions[m_indexB].a = aB;

    pool.pushRot(2);
    pool.pushVec2(7);

    return linearError < Settings.linearSlop;
  }
Ejemplo 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;
      }
    }
  }