use of spacegraph.space2d.phys.collision.ManifoldPoint in project narchy by automenta.
the class Contact method update.
public void update(ContactListener listener) {
oldManifold.set(m_manifold);
// Re-enable this contact.
m_flags |= ENABLED_FLAG;
boolean touching = false;
boolean wasTouching = (m_flags & TOUCHING_FLAG) == TOUCHING_FLAG;
boolean sensorA = aFixture.isSensor();
boolean sensorB = bFixture.isSensor();
boolean sensor = sensorA || sensorB;
Body2D bodyA = aFixture.getBody();
Body2D bodyB = bFixture.getBody();
Transform xfA = bodyA;
Transform xfB = bodyB;
if (sensor) {
Shape shapeA = aFixture.shape();
Shape shapeB = bFixture.shape();
touching = pool.getCollision().testOverlap(shapeA, aIndex, shapeB, bIndex, xfA, xfB);
// Sensors don't generate manifolds.
m_manifold.pointCount = 0;
} else {
evaluate(m_manifold, xfA, xfB);
touching = m_manifold.pointCount > 0;
// stored impulses to warm start the solver.
for (int i = 0; i < m_manifold.pointCount; ++i) {
ManifoldPoint mp2 = m_manifold.points[i];
mp2.normalImpulse = 0.0f;
mp2.tangentImpulse = 0.0f;
ContactID id2 = mp2.id;
for (int j = 0; j < oldManifold.pointCount; ++j) {
ManifoldPoint mp1 = oldManifold.points[j];
if (mp1.id.isEqual(id2)) {
mp2.normalImpulse = mp1.normalImpulse;
mp2.tangentImpulse = mp1.tangentImpulse;
break;
}
}
}
if (touching != wasTouching) {
bodyA.setAwake(true);
bodyB.setAwake(true);
}
}
if (touching) {
m_flags |= TOUCHING_FLAG;
} else {
m_flags &= ~TOUCHING_FLAG;
}
if (!sensor && touching) {
m_angularVelocity_bodyA = aFixture.body.velAngular;
m_linearVelocity_bodyA.set(aFixture.body.vel);
m_angularVelocity_bodyB = bFixture.body.velAngular;
m_linearVelocity_bodyB.set(bFixture.body.vel);
}
if (listener == null) {
return;
}
if (!wasTouching && touching) {
if (!listener.beginContact(this))
touching = false;
}
if (wasTouching && !touching) {
listener.endContact(this);
}
if (!sensor && touching) {
listener.preSolve(this, oldManifold);
}
}
use of spacegraph.space2d.phys.collision.ManifoldPoint in project narchy by automenta.
the class PositionSolverManifold method init.
public final void init(ContactSolverDef def) {
// System.out.println("Initializing contact solver");
m_step = def.step;
m_count = def.count;
if (m_positionConstraints.length < m_count) {
ContactPositionConstraint[] old = m_positionConstraints;
m_positionConstraints = new ContactPositionConstraint[MathUtils.max(old.length * 2, m_count)];
System.arraycopy(old, 0, m_positionConstraints, 0, old.length);
for (int i = old.length; i < m_positionConstraints.length; i++) {
m_positionConstraints[i] = new ContactPositionConstraint();
}
}
if (m_velocityConstraints.length < m_count) {
ContactVelocityConstraint[] old = m_velocityConstraints;
m_velocityConstraints = new ContactVelocityConstraint[MathUtils.max(old.length * 2, m_count)];
System.arraycopy(old, 0, m_velocityConstraints, 0, old.length);
for (int i = old.length; i < m_velocityConstraints.length; i++) {
m_velocityConstraints[i] = new ContactVelocityConstraint();
}
}
m_positions = def.positions;
m_velocities = def.velocities;
m_contacts = def.contacts;
for (int i = 0; i < m_count; ++i) {
// System.out.println("contacts: " + m_count);
final Contact contact = m_contacts[i];
final Fixture fixtureA = contact.aFixture;
final Fixture fixtureB = contact.bFixture;
final Shape shapeA = fixtureA.shape();
final Shape shapeB = fixtureB.shape();
final float radiusA = shapeA.radius;
final float radiusB = shapeB.radius;
final Body2D bodyA = fixtureA.getBody();
final Body2D bodyB = fixtureB.getBody();
final Manifold manifold = contact.getManifold();
int pointCount = manifold.pointCount;
assert (pointCount > 0);
ContactVelocityConstraint vc = m_velocityConstraints[i];
vc.friction = contact.m_friction;
vc.restitution = contact.m_restitution;
vc.tangentSpeed = contact.m_tangentSpeed;
vc.indexA = bodyA.island;
vc.indexB = bodyB.island;
vc.invMassA = bodyA.m_invMass;
vc.invMassB = bodyB.m_invMass;
vc.invIA = bodyA.m_invI;
vc.invIB = bodyB.m_invI;
vc.contactIndex = i;
vc.pointCount = pointCount;
vc.K.setZero();
vc.normalMass.setZero();
ContactPositionConstraint pc = m_positionConstraints[i];
pc.indexA = bodyA.island;
pc.indexB = bodyB.island;
pc.invMassA = bodyA.m_invMass;
pc.invMassB = bodyB.m_invMass;
pc.localCenterA.set(bodyA.sweep.localCenter);
pc.localCenterB.set(bodyB.sweep.localCenter);
pc.invIA = bodyA.m_invI;
pc.invIB = bodyB.m_invI;
pc.localNormal.set(manifold.localNormal);
pc.localPoint.set(manifold.localPoint);
pc.pointCount = pointCount;
pc.radiusA = radiusA;
pc.radiusB = radiusB;
pc.type = manifold.type;
// System.out.println("contact point count: " + pointCount);
for (int j = 0; j < pointCount; j++) {
ManifoldPoint cp = manifold.points[j];
VelocityConstraintPoint vcp = vc.points[j];
if (m_step.warmStarting) {
// assert(cp.normalImpulse == 0);
// System.out.println("contact normal impulse: " + cp.normalImpulse);
vcp.normalImpulse = m_step.dtRatio * cp.normalImpulse;
vcp.tangentImpulse = m_step.dtRatio * cp.tangentImpulse;
} else {
vcp.normalImpulse = 0;
vcp.tangentImpulse = 0;
}
vcp.rA.setZero();
vcp.rB.setZero();
vcp.normalMass = 0;
vcp.tangentMass = 0;
vcp.velocityBias = 0;
pc.localPoints[j].x = cp.localPoint.x;
pc.localPoints[j].y = cp.localPoint.y;
}
}
}
Aggregations