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