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); } }
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; }