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