예제 #1
0
  @Override
  public void evaluate(final ContactListener listener) {
    final Body b1 = m_shape1.getBody();
    final Body b2 = m_shape2.getBody();

    final Manifold m0 = tlm0.get();
    final Vec2 v1 = tlV1.get();
    final ContactPoint cp = tlCp.get();
    m0.set(m_manifold);

    SingletonPool.getCollidePoly()
        .collidePolyAndEdge(
            m_manifold,
            (PolygonShape) m_shape1,
            b1.getMemberXForm(),
            (EdgeShape) m_shape2,
            b2.getMemberXForm());

    final Boolean[] persisted = tlPersisted.get(2);
    persisted[0] = false;
    persisted[1] = false;

    cp.shape1 = m_shape1;
    cp.shape2 = m_shape2;
    cp.friction = m_friction;
    cp.restitution = m_restitution;
    // TODO: add this once custom friction/restitution mixings are in place
    // cp.friction = b2MixFriction(m_shape1->GetFriction(), m_shape2->GetFriction());
    // cp.restitution = b2MixRestitution(m_shape1->GetRestitution(), m_shape2->GetRestitution());

    // Match contact ids to facilitate warm starting.
    if (m_manifold.pointCount > 0) {
      // Match old contact ids to new contact ids and copy the
      // stored impulses to warm start the solver.
      for (int i = 0; i < m_manifold.pointCount; ++i) {
        final ManifoldPoint mp = m_manifold.points[i];
        mp.normalImpulse = 0.0f;
        mp.tangentImpulse = 0.0f;
        boolean found = false;
        final ContactID id = mp.id;

        for (int j = 0; j < m0.pointCount; ++j) {
          if (persisted[j] == true) {
            continue;
          }

          final ManifoldPoint mp0 = m0.points[j];

          if (mp0.id.isEqual(id)) {
            persisted[j] = true;
            mp.normalImpulse = mp0.normalImpulse;
            mp.tangentImpulse = mp0.tangentImpulse;

            // A persistent point.
            found = true;

            // Report persistent point.
            if (listener != null) {
              // cp.position = b1.getWorldLocation(mp.localPoint1);
              b1.getWorldLocationToOut(mp.localPoint1, cp.position);
              // Vec2 v1 = b1.getLinearVelocityFromLocalPoint(mp.localPoint1);
              b1.getLinearVelocityFromLocalPointToOut(mp.localPoint1, v1);
              // Vec2 v2 = b2.getLinearVelocityFromLocalPoint(mp.localPoint2);
              b2.getLinearVelocityFromLocalPointToOut(mp.localPoint2, cp.velocity);
              // cp.velocity = v2.sub(v1);
              cp.velocity.subLocal(v1);

              cp.normal.set(m_manifold.normal);
              cp.separation = mp.separation;
              cp.id.set(id);
              listener.persist(cp);
            }
            break;
          }
        }

        // Report added point.
        if (found == false && listener != null) {
          b1.getWorldLocationToOut(mp.localPoint1, cp.position);
          // Vec2 v1 = b1.getLinearVelocityFromLocalPoint(mp.localPoint1);
          b1.getLinearVelocityFromLocalPointToOut(mp.localPoint1, v1);
          // Vec2 v2 = b2.getLinearVelocityFromLocalPoint(mp.localPoint2);
          b2.getLinearVelocityFromLocalPointToOut(mp.localPoint2, cp.velocity);
          // cp.velocity = v2.sub(v1);
          cp.velocity.subLocal(v1);

          cp.normal.set(m_manifold.normal);
          cp.separation = mp.separation;
          cp.id.set(id);
          listener.add(cp);
        }
      }

      m_manifoldCount = 1;
    } else {
      m_manifoldCount = 0;
    }

    if (listener == null) {
      return;
    }

    // Report removed points.
    for (int i = 0; i < m0.pointCount; ++i) {
      if (persisted[i]) {
        continue;
      }

      final ManifoldPoint mp0 = m0.points[i];
      b1.getWorldLocationToOut(mp0.localPoint1, cp.position);
      // Vec2 v1 = b1.getLinearVelocityFromLocalPoint(mp.localPoint1);
      b1.getLinearVelocityFromLocalPointToOut(mp0.localPoint1, v1);
      // Vec2 v2 = b2.getLinearVelocityFromLocalPoint(mp.localPoint2);
      b2.getLinearVelocityFromLocalPointToOut(mp0.localPoint2, cp.velocity);
      // cp.velocity = v2.sub(v1);
      cp.velocity.subLocal(v1);

      cp.normal.set(m_manifold.normal);
      cp.separation = mp0.separation;
      cp.id.set(mp0.id);
      listener.remove(cp);
    }
  }
예제 #2
0
  @Override
  public void evaluate(ContactListener listener) {
    Body b1 = m_shape1.getBody();
    Body b2 = m_shape2.getBody();
    // Manifold m0 = m_manifold;
    Manifold m0 = new Manifold(m_manifold);
    // This next stuff might be unnecessary now [ewj: nope, we need it]
    for (int k = 0; k < m_manifold.pointCount; k++) {
      m0.points[k] = new ManifoldPoint(m_manifold.points[k]);
      m0.points[k].normalImpulse = m_manifold.points[k].normalImpulse;
      m0.points[k].tangentImpulse = m_manifold.points[k].tangentImpulse;
      m0.points[k].separation = m_manifold.points[k].separation;
      // m0.points[k].id.key = m_manifold.points[k].id.key;
      m0.points[k].id.features.set(m_manifold.points[k].id.features);
      // System.out.println(m_manifold.points[k].normalForce);
    }
    m0.pointCount = m_manifold.pointCount;

    CollidePoly.collidePolygons(
        m_manifold, (PolygonShape) m_shape1, b1.getXForm(), (PolygonShape) m_shape2, b2.getXForm());

    boolean[] persisted = {false, false};

    ContactPoint cp = new ContactPoint();
    cp.shape1 = m_shape1;
    cp.shape2 = m_shape2;
    cp.friction = m_friction;
    cp.restitution = m_restitution;

    // Match contact ids to facilitate warm starting.
    // Watch out (Java note):
    // m_manifold.pointCount != m_manifold.points.length!!!

    // Match contact ids to facilitate warm starting.
    if (m_manifold.pointCount > 0) {
      // Match old contact ids to new contact ids and copy the
      // stored impulses to warm start the solver.
      for (int i = 0; i < m_manifold.pointCount; ++i) {
        ManifoldPoint mp = m_manifold.points[i];
        mp.normalImpulse = 0.0f;
        mp.tangentImpulse = 0.0f;
        boolean found = false;
        ContactID id = new ContactID(mp.id);

        for (int j = 0; j < m0.pointCount; ++j) {
          if (persisted[j] == true) {
            continue;
          }

          ManifoldPoint mp0 = m0.points[j];

          if (mp0.id.isEqual(id)) {
            persisted[j] = true;
            mp.normalImpulse = mp0.normalImpulse;
            mp.tangentImpulse = mp0.tangentImpulse;

            // A persistent point.
            found = true;

            // Report persistent point.
            if (listener != null) {
              cp.position = b1.getWorldPoint(mp.localPoint1);
              Vec2 v1 = b1.getLinearVelocityFromLocalPoint(mp.localPoint1);
              Vec2 v2 = b2.getLinearVelocityFromLocalPoint(mp.localPoint2);
              cp.velocity = v2.sub(v1);
              cp.normal = m_manifold.normal.clone();
              cp.separation = mp.separation;
              cp.id = new ContactID(id);
              listener.persist(cp);
            }
            break;
          }
        }

        // Report added point.
        if (found == false && listener != null) {
          cp.position = b1.getWorldPoint(mp.localPoint1);
          Vec2 v1 = b1.getLinearVelocityFromLocalPoint(mp.localPoint1);
          Vec2 v2 = b2.getLinearVelocityFromLocalPoint(mp.localPoint2);
          cp.velocity = v2.sub(v1);
          cp.normal = m_manifold.normal.clone();
          cp.separation = mp.separation;
          cp.id = new ContactID(id);
          listener.add(cp);
        }
      }

      m_manifoldCount = 1;
    } else {
      m_manifoldCount = 0;
    }

    if (listener == null) {
      return;
    }

    // Report removed points.
    for (int i = 0; i < m0.pointCount; ++i) {
      if (persisted[i]) {
        continue;
      }

      ManifoldPoint mp0 = m0.points[i];
      cp.position = b1.getWorldPoint(mp0.localPoint1);
      Vec2 v1 = b1.getLinearVelocityFromLocalPoint(mp0.localPoint1);
      Vec2 v2 = b2.getLinearVelocityFromLocalPoint(mp0.localPoint2);
      cp.velocity = v2.sub(v1);
      cp.normal = m0.normal.clone();
      cp.separation = mp0.separation;
      cp.id = new ContactID(mp0.id);
      listener.remove(cp);
    }
  }
예제 #3
0
  public void evaluate(ContactListener listener) {
    // CollideCircle.collideCircle(m_manifold, (CircleShape) m_shape1,
    //        (CircleShape) m_shape2, false);
    Body b1 = m_shape1.getBody();
    Body b2 = m_shape2.getBody();

    Manifold m0 = new Manifold(m_manifold);
    for (int k = 0; k < m_manifold.pointCount; k++) {
      m0.points[k] = new ManifoldPoint(m_manifold.points[k]);
      m0.points[k].normalImpulse = m_manifold.points[k].normalImpulse;
      m0.points[k].tangentImpulse = m_manifold.points[k].tangentImpulse;
      m0.points[k].separation = m_manifold.points[k].separation;
      // m0.points[k].id.key = m_manifold.points[k].id.key;
      m0.points[k].id.features.set(m_manifold.points[k].id.features);
      // System.out.println(m_manifold.points[k].id.key);
    }
    m0.pointCount = m_manifold.pointCount;

    CollideCircle.collideCircles(
        m_manifold, (CircleShape) m_shape1, b1.m_xf, (CircleShape) m_shape2, b2.m_xf);

    ContactPoint cp = new ContactPoint();
    cp.shape1 = m_shape1;
    cp.shape2 = m_shape2;
    cp.friction = m_friction;
    cp.restitution = m_restitution;

    if (m_manifold.pointCount > 0) {
      m_manifoldCount = 1;
      ManifoldPoint mp = m_manifold.points[0];
      if (m0.pointCount == 0) {
        mp.normalImpulse = 0.0f;
        mp.tangentImpulse = 0.0f;

        if (listener != null) {
          cp.position = b1.getWorldPoint(mp.localPoint1);
          Vec2 v1 = b1.getLinearVelocityFromLocalPoint(mp.localPoint1);
          Vec2 v2 = b2.getLinearVelocityFromLocalPoint(mp.localPoint2);
          cp.velocity = v2.sub(v1);
          cp.normal = m_manifold.normal.clone();
          cp.separation = mp.separation;
          cp.id = new ContactID(mp.id);
          listener.add(cp);
        }
      } else {
        ManifoldPoint mp0 = m0.points[0];
        mp.normalImpulse = mp0.normalImpulse;
        mp.tangentImpulse = mp0.tangentImpulse;

        if (listener != null) {
          cp.position = b1.getWorldPoint(mp.localPoint1);
          Vec2 v1 = b1.getLinearVelocityFromLocalPoint(mp.localPoint1);
          Vec2 v2 = b2.getLinearVelocityFromLocalPoint(mp.localPoint2);
          cp.velocity = v2.sub(v1);
          cp.normal = m_manifold.normal.clone();
          cp.separation = mp.separation;
          cp.id = new ContactID(mp.id);
          listener.persist(cp);
        }
      }
    } else {
      m_manifoldCount = 0;
      if (m0.pointCount > 0 && (listener != null)) {
        ManifoldPoint mp0 = m0.points[0];
        cp.position = b1.getWorldPoint(mp0.localPoint1);
        Vec2 v1 = b1.getLinearVelocityFromLocalPoint(mp0.localPoint1);
        Vec2 v2 = b2.getLinearVelocityFromLocalPoint(mp0.localPoint2);
        cp.velocity = v2.sub(v1);
        cp.normal = m0.normal.clone();
        cp.separation = mp0.separation;
        cp.id = new ContactID(mp0.id);
        listener.remove(cp);
      }
    }
  }