Beispiel #1
0
  public boolean testSegment(RaycastResult out, Segment segment, float maxLambda) {
    Vec2 s = segment.p1;
    Vec2 r = tlR.get().set(segment.p2);
    r.subLocal(s);
    Vec2 d = tlD.get().set(p2);
    d.subLocal(p1);
    Vec2 n = tlN.get();
    Vec2.crossToOut(d, 1.0f, n);
    Vec2 b = tlB.get();

    float k_slop = 100.0f * Settings.EPSILON;
    float denom = -Vec2.dot(r, n);

    // Cull back facing gwt.ns.gwtbox2d.collision and ignore parallel segments.
    if (denom > k_slop) {

      // Does the segment intersect the infinite line associated with this segment?
      b.set(s);
      b.subLocal(p1);
      float a = Vec2.dot(b, n);

      if (0.0f <= a && a <= maxLambda * denom) {
        float mu2 = -r.x * b.y + r.y * b.x;

        // Does the segment intersect this segment?
        if (-k_slop * denom <= mu2 && mu2 <= denom * (1.0f + k_slop)) {
          a /= denom;
          n.normalize();
          out.lambda = a;
          out.normal.set(n);
          return true;
        }
      }
    }
    return false;
  }
  @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);
    }
  }
Beispiel #3
0
  public void destroy(final Contact c) {

    final Vec2 v1 = tlV1.get();
    final ContactPoint cp = tlCp.get();

    final Shape shape1 = c.getShape1();
    final Shape shape2 = c.getShape2();

    // Inform the user that this contact is ending.
    final int manifoldCount = c.getManifoldCount();
    if (manifoldCount > 0 && (m_world.m_contactListener != null)) {
      final Body b1 = shape1.getBody();
      final Body b2 = shape2.getBody();
      final List<Manifold> manifolds = c.getManifolds();
      cp.shape1 = c.getShape1();
      cp.shape2 = c.getShape2();
      cp.friction = c.m_friction;
      cp.restitution = c.m_restitution;
      for (int i = 0; i < manifoldCount; ++i) {
        final Manifold manifold = manifolds.get(i);
        cp.normal.set(manifold.normal);
        for (int j = 0; j < manifold.pointCount; ++j) {

          final ManifoldPoint mp = manifold.points[j];
          b1.getWorldLocationToOut(mp.localPoint1, cp.position);
          b1.getLinearVelocityFromLocalPointToOut(mp.localPoint1, v1);
          // velocity isn't initialized in the contact point
          b2.getLinearVelocityFromLocalPointToOut(mp.localPoint2, cp.velocity);
          cp.velocity.subLocal(v1);
          cp.separation = mp.separation;
          cp.id.set(mp.id);
          m_world.m_contactListener.remove(cp);
        }
      }
    }

    // Remove from the world.
    if (c.m_prev != null) {
      c.m_prev.m_next = c.m_next;
    }

    if (c.m_next != null) {
      c.m_next.m_prev = c.m_prev;
    }

    if (c == m_world.m_contactList) {
      m_world.m_contactList = c.m_next;
    }

    final Body body1 = shape1.getBody();
    final Body body2 = shape2.getBody();

    // Remove from body 1
    if (c.m_node1.prev != null) {
      c.m_node1.prev.next = c.m_node1.next;
    }

    if (c.m_node1.next != null) {
      c.m_node1.next.prev = c.m_node1.prev;
    }

    if (c.m_node1 == body1.m_contactList) {
      body1.m_contactList = c.m_node1.next;
    }

    // Remove from body 2
    if (c.m_node2.prev != null) {
      c.m_node2.prev.next = c.m_node2.next;
    }

    if (c.m_node2.next != null) {
      c.m_node2.next.prev = c.m_node2.prev;
    }

    if (c.m_node2 == body2.m_contactList) {
      body2.m_contactList = c.m_node2.next;
    }

    // Call the factory.
    Contact.destroy(c);
    --m_world.m_contactCount;
  }