use of org.jbox2d.dynamics.Body in project libgdx by libgdx.
the class Contact method getWorldManifold.
/**
* Get the world manifold.
*/
public void getWorldManifold(WorldManifold worldManifold) {
final Body bodyA = m_fixtureA.getBody();
final Body bodyB = m_fixtureB.getBody();
final Shape shapeA = m_fixtureA.getShape();
final Shape shapeB = m_fixtureB.getShape();
worldManifold.initialize(m_manifold, bodyA.getTransform(), shapeA.m_radius, bodyB.getTransform(), shapeB.m_radius);
}
use of org.jbox2d.dynamics.Body in project libgdx by libgdx.
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.m_fixtureA;
final Fixture fixtureB = contact.m_fixtureB;
final Shape shapeA = fixtureA.getShape();
final Shape shapeB = fixtureB.getShape();
final float radiusA = shapeA.m_radius;
final float radiusB = shapeB.m_radius;
final Body bodyA = fixtureA.getBody();
final Body 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.m_islandIndex;
vc.indexB = bodyB.m_islandIndex;
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.m_islandIndex;
pc.indexB = bodyB.m_islandIndex;
pc.invMassA = bodyA.m_invMass;
pc.invMassB = bodyB.m_invMass;
pc.localCenterA.set(bodyA.m_sweep.localCenter);
pc.localCenterB.set(bodyB.m_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;
}
}
}
use of org.jbox2d.dynamics.Body in project libgdx by libgdx.
the class PrismaticJoint method getJointSpeed.
/**
* Get the current joint translation, usually in meters.
*/
public float getJointSpeed() {
Body bA = m_bodyA;
Body bB = m_bodyB;
Vec2 temp = pool.popVec2();
Vec2 rA = pool.popVec2();
Vec2 rB = pool.popVec2();
Vec2 p1 = pool.popVec2();
Vec2 p2 = pool.popVec2();
Vec2 d = pool.popVec2();
Vec2 axis = pool.popVec2();
Vec2 temp2 = pool.popVec2();
Vec2 temp3 = pool.popVec2();
temp.set(m_localAnchorA).subLocal(bA.m_sweep.localCenter);
Rot.mulToOutUnsafe(bA.m_xf.q, temp, rA);
temp.set(m_localAnchorB).subLocal(bB.m_sweep.localCenter);
Rot.mulToOutUnsafe(bB.m_xf.q, temp, rB);
p1.set(bA.m_sweep.c).addLocal(rA);
p2.set(bB.m_sweep.c).addLocal(rB);
d.set(p2).subLocal(p1);
Rot.mulToOutUnsafe(bA.m_xf.q, m_localXAxisA, axis);
Vec2 vA = bA.m_linearVelocity;
Vec2 vB = bB.m_linearVelocity;
float wA = bA.m_angularVelocity;
float wB = bB.m_angularVelocity;
Vec2.crossToOutUnsafe(wA, axis, temp);
Vec2.crossToOutUnsafe(wB, rB, temp2);
Vec2.crossToOutUnsafe(wA, rA, temp3);
temp2.addLocal(vB).subLocal(vA).subLocal(temp3);
float speed = Vec2.dot(d, temp) + Vec2.dot(axis, temp2);
pool.pushVec2(9);
return speed;
}
use of org.jbox2d.dynamics.Body in project libgdx by libgdx.
the class ParticleSystem method solveViscous.
void solveViscous(final TimeStep step) {
float viscousStrength = m_viscousStrength;
for (int k = 0; k < m_bodyContactCount; k++) {
final ParticleBodyContact contact = m_bodyContactBuffer[k];
int a = contact.index;
if ((m_flagsBuffer.data[a] & ParticleType.b2_viscousParticle) != 0) {
Body b = contact.body;
float w = contact.weight;
float m = contact.mass;
Vec2 p = m_positionBuffer.data[a];
final Vec2 va = m_velocityBuffer.data[a];
final float tempX = p.x - b.m_sweep.c.x;
final float tempY = p.y - b.m_sweep.c.y;
final float vx = -b.m_angularVelocity * tempY + b.m_linearVelocity.x - va.x;
final float vy = b.m_angularVelocity * tempX + b.m_linearVelocity.y - va.y;
final Vec2 f = tempVec;
final float pInvMass = getParticleInvMass();
f.x = viscousStrength * m * w * vx;
f.y = viscousStrength * m * w * vy;
va.x += pInvMass * f.x;
va.y += pInvMass * f.y;
f.x = -f.x;
f.y = -f.y;
b.applyLinearImpulse(f, p, true);
}
}
for (int k = 0; k < m_contactCount; k++) {
final ParticleContact contact = m_contactBuffer[k];
if ((contact.flags & ParticleType.b2_viscousParticle) != 0) {
int a = contact.indexA;
int b = contact.indexB;
float w = contact.weight;
final Vec2 va = m_velocityBuffer.data[a];
final Vec2 vb = m_velocityBuffer.data[b];
final float vx = vb.x - va.x;
final float vy = vb.y - va.y;
final float fx = viscousStrength * w * vx;
final float fy = viscousStrength * w * vy;
va.x += fx;
va.y += fy;
vb.x -= fx;
vb.y -= fy;
}
}
}
use of org.jbox2d.dynamics.Body in project libgdx by libgdx.
the class WheelJoint method getJointTranslation.
public float getJointTranslation() {
Body b1 = m_bodyA;
Body b2 = m_bodyB;
Vec2 p1 = pool.popVec2();
Vec2 p2 = pool.popVec2();
Vec2 axis = pool.popVec2();
b1.getWorldPointToOut(m_localAnchorA, p1);
b2.getWorldPointToOut(m_localAnchorA, p2);
p2.subLocal(p1);
b1.getWorldVectorToOut(m_localXAxisA, axis);
float translation = Vec2.dot(p2, axis);
pool.pushVec2(3);
return translation;
}
Aggregations