/**
* Set the sleep state of the body. A sleeping body has very low CPU cost.
*
* @param flag set to true to put body to sleep, false to wake it.
* @param flag
*/
public void setAwake(boolean flag) {
if (flag) {
if ((m_flags & e_awakeFlag) == 0) {
m_flags |= e_awakeFlag;
m_sleepTime = 0.0f;
}
} else {
m_flags &= ~e_awakeFlag;
m_sleepTime = 0.0f;
m_linearVelocity.setZero();
m_angularVelocity = 0.0f;
m_force.setZero();
m_torque = 0.0f;
}
}
/**
* Build vertices to represent an axis-aligned box.
*
* @param hx the half-width.
* @param hy the half-height.
*/
public final void setAsBox(final float hx, final float hy) {
m_count = 4;
m_vertices[0].set(-hx, -hy);
m_vertices[1].set(hx, -hy);
m_vertices[2].set(hx, hy);
m_vertices[3].set(-hx, hy);
m_normals[0].set(0.0f, -1.0f);
m_normals[1].set(1.0f, 0.0f);
m_normals[2].set(0.0f, 1.0f);
m_normals[3].set(-1.0f, 0.0f);
m_centroid.setZero();
}
public PolygonShape() {
super(ShapeType.POLYGON);
m_count = 0;
m_vertices = new Vec2[Settings.maxPolygonVertices];
for (int i = 0; i < m_vertices.length; i++) {
m_vertices[i] = new Vec2();
}
m_normals = new Vec2[Settings.maxPolygonVertices];
for (int i = 0; i < m_normals.length; i++) {
m_normals[i] = new Vec2();
}
setRadius(Settings.polygonRadius);
m_centroid.setZero();
}
/**
* Set the type of this body. This may alter the mass and velocity.
*
* @param type
*/
public void setType(BodyType type) {
if (m_type == type) {
return;
}
m_type = type;
resetMassData();
if (m_type == BodyType.STATIC) {
m_linearVelocity.setZero();
m_angularVelocity = 0.0f;
}
setAwake(true);
m_force.setZero();
m_torque = 0.0f;
// Since the body type changed, we need to flag contacts for filtering.
for (ContactEdge ce = m_contactList; ce != null; ce = ce.next) {
ce.contact.flagForFiltering();
}
}
protected MouseJoint(IWorldPool argWorld, MouseJointDef def) {
super(argWorld, def);
assert (def.target.isValid());
assert (def.maxForce >= 0);
assert (def.frequencyHz >= 0);
assert (def.dampingRatio >= 0);
m_target.set(def.target);
Transform.mulTransToOut(m_bodyB.getTransform(), m_target, m_localAnchor);
m_maxForce = def.maxForce;
m_impulse.setZero();
m_frequencyHz = def.frequencyHz;
m_dampingRatio = def.dampingRatio;
m_beta = 0;
m_gamma = 0;
}
public final void computeCentroidToOut(final Vec2[] vs, final int count, final Vec2 out) {
assert (count >= 3);
out.set(0.0f, 0.0f);
float area = 0.0f;
// pRef is the reference point for forming triangles.
// It's location doesn't change the result (except for rounding error).
final Vec2 pRef = pool1;
pRef.setZero();
final Vec2 e1 = pool2;
final Vec2 e2 = pool3;
final float inv3 = 1.0f / 3.0f;
for (int i = 0; i < count; ++i) {
// Triangle vertices.
final Vec2 p1 = pRef;
final Vec2 p2 = vs[i];
final Vec2 p3 = i + 1 < count ? vs[i + 1] : vs[0];
e1.set(p2).subLocal(p1);
e2.set(p3).subLocal(p1);
final float D = Vec2.cross(e1, e2);
final float triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
e1.set(p1).addLocal(p2).addLocal(p3).mulLocal(triangleArea * inv3);
out.addLocal(e1);
}
// Centroid
assert (area > Settings.EPSILON);
out.mulLocal(1.0f / area);
}
@Override
public void drawTransform(Transform xf) {
Graphics2D g = getGraphics();
getWorldToScreenToOut(xf.p, temp);
temp2.setZero();
float k_axisScale = 0.4f;
Color c = cpool.getColor(1, 0, 0);
g.setColor(c);
temp2.x = xf.p.x + k_axisScale * xf.q.c;
temp2.y = xf.p.y + k_axisScale * xf.q.s;
getWorldToScreenToOut(temp2, temp2);
g.drawLine((int) temp.x, (int) temp.y, (int) temp2.x, (int) temp2.y);
c = cpool.getColor(0, 1, 0);
g.setColor(c);
temp2.x = xf.p.x + -k_axisScale * xf.q.s;
temp2.y = xf.p.y + k_axisScale * xf.q.c;
getWorldToScreenToOut(temp2, temp2);
g.drawLine((int) temp.x, (int) temp.y, (int) temp2.x, (int) temp2.y);
}
public void computeMass(final MassData massData, float density) {
// Polygon mass, centroid, and inertia.
// Let rho be the polygon density in mass per unit area.
// Then:
// mass = rho * int(dA)
// centroid.x = (1/mass) * rho * int(x * dA)
// centroid.y = (1/mass) * rho * int(y * dA)
// I = rho * int((x*x + y*y) * dA)
//
// We can compute these integrals by summing all the integrals
// for each triangle of the polygon. To evaluate the integral
// for a single triangle, we make a change of variables to
// the (u,v) coordinates of the triangle:
// x = x0 + e1x * u + e2x * v
// y = y0 + e1y * u + e2y * v
// where 0 <= u && 0 <= v && u + v <= 1.
//
// We integrate u from [0,1-v] and then v from [0,1].
// We also need to use the Jacobian of the transformation:
// D = cross(e1, e2)
//
// Simplification: triangle centroid = (1/3) * (p1 + p2 + p3)
//
// The rest of the derivation is handled by computer algebra.
assert (m_count >= 3);
final Vec2 center = pool1;
center.setZero();
float area = 0.0f;
float I = 0.0f;
// pRef is the reference point for forming triangles.
// It's location doesn't change the result (except for rounding error).
final Vec2 s = pool2;
s.setZero();
// This code would put the reference point inside the polygon.
for (int i = 0; i < m_count; ++i) {
s.addLocal(m_vertices[i]);
}
s.mulLocal(1.0f / m_count);
final float k_inv3 = 1.0f / 3.0f;
final Vec2 e1 = pool3;
final Vec2 e2 = pool4;
for (int i = 0; i < m_count; ++i) {
// Triangle vertices.
e1.set(m_vertices[i]).subLocal(s);
e2.set(s).negateLocal().addLocal(i + 1 < m_count ? m_vertices[i + 1] : m_vertices[0]);
final float D = Vec2.cross(e1, e2);
final float triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
center.x += triangleArea * k_inv3 * (e1.x + e2.x);
center.y += triangleArea * k_inv3 * (e1.y + e2.y);
final float ex1 = e1.x, ey1 = e1.y;
final float ex2 = e2.x, ey2 = e2.y;
float intx2 = ex1 * ex1 + ex2 * ex1 + ex2 * ex2;
float inty2 = ey1 * ey1 + ey2 * ey1 + ey2 * ey2;
I += (0.25f * k_inv3 * D) * (intx2 + inty2);
}
// Total mass
massData.mass = density * area;
// Center of mass
assert (area > Settings.EPSILON);
center.mulLocal(1.0f / area);
massData.center.set(center).addLocal(s);
// Inertia tensor relative to the local origin (point s)
massData.I = I * density;
// Shift to center of mass then to original body origin.
massData.I += massData.mass * (Vec2.dot(massData.center, massData.center));
}
public Body(final BodyDef bd, World world) {
assert (bd.position.isValid());
assert (bd.linearVelocity.isValid());
assert (bd.inertiaScale >= 0.0f);
assert (bd.angularDamping >= 0.0f);
assert (bd.linearDamping >= 0.0f);
m_flags = 0;
if (bd.bullet) {
m_flags |= e_bulletFlag;
}
if (bd.fixedRotation) {
m_flags |= e_fixedRotationFlag;
}
if (bd.allowSleep) {
m_flags |= e_autoSleepFlag;
}
if (bd.awake) {
m_flags |= e_awakeFlag;
}
if (bd.active) {
m_flags |= e_activeFlag;
}
m_world = world;
m_xf.position.set(bd.position);
m_xf.R.set(bd.angle);
m_sweep.localCenter.setZero();
m_sweep.a0 = m_sweep.a = bd.angle;
// m_sweep.c0 = m_sweep.c = Transform.mul(m_xf, m_sweep.localCenter);
Transform.mulToOut(m_xf, m_sweep.localCenter, m_sweep.c0);
m_sweep.c.set(m_sweep.c0);
m_jointList = null;
m_contactList = null;
m_prev = null;
m_next = null;
m_linearVelocity.set(bd.linearVelocity);
m_angularVelocity = bd.angularVelocity;
m_linearDamping = bd.linearDamping;
m_angularDamping = bd.angularDamping;
m_force.setZero();
m_torque = 0.0f;
m_sleepTime = 0.0f;
m_type = bd.type;
if (m_type == BodyType.DYNAMIC) {
m_mass = 1f;
m_invMass = 1f;
} else {
m_mass = 0f;
m_invMass = 0f;
}
m_I = 0.0f;
m_invI = 0.0f;
m_userData = bd.userData;
m_fixtureList = null;
m_fixtureCount = 0;
}
/**
* This resets the mass properties to the sum of the mass properties of the fixtures. This
* normally does not need to be called unless you called setMassData to override the mass and you
* later want to reset the mass.
*/
public final void resetMassData() {
// Compute mass data from shapes. Each shape has its own density.
m_mass = 0.0f;
m_invMass = 0.0f;
m_I = 0.0f;
m_invI = 0.0f;
m_sweep.localCenter.setZero();
// Static and kinematic bodies have zero mass.
if (m_type == BodyType.STATIC || m_type == BodyType.KINEMATIC) {
// m_sweep.c0 = m_sweep.c = m_xf.position;
m_sweep.c.set(m_xf.position);
m_sweep.c0.set(m_xf.position);
return;
}
assert (m_type == BodyType.DYNAMIC);
// Accumulate mass over all fixtures.
final Vec2 center = m_world.getPool().popVec2();
center.setZero();
final Vec2 temp = m_world.getPool().popVec2();
final MassData massData = pmd;
for (Fixture f = m_fixtureList; f != null; f = f.m_next) {
if (f.m_density == 0.0f) {
continue;
}
f.getMassData(massData);
m_mass += massData.mass;
// center += massData.mass * massData.center;
temp.set(massData.center).mulLocal(massData.mass);
center.addLocal(temp);
m_I += massData.I;
}
// Compute center of mass.
if (m_mass > 0.0f) {
m_invMass = 1.0f / m_mass;
center.mulLocal(m_invMass);
} else {
// Force all dynamic bodies to have a positive mass.
m_mass = 1.0f;
m_invMass = 1.0f;
}
if (m_I > 0.0f && (m_flags & e_fixedRotationFlag) == 0) {
// Center the inertia about the center of mass.
m_I -= m_mass * Vec2.dot(center, center);
assert (m_I > 0.0f);
m_invI = 1.0f / m_I;
} else {
m_I = 0.0f;
m_invI = 0.0f;
}
Vec2 oldCenter = m_world.getPool().popVec2();
// Move center of mass.
oldCenter.set(m_sweep.c);
m_sweep.localCenter.set(center);
// m_sweep.c0 = m_sweep.c = Mul(m_xf, m_sweep.localCenter);
Transform.mulToOut(m_xf, m_sweep.localCenter, m_sweep.c0);
m_sweep.c.set(m_sweep.c0);
// Update center of mass velocity.
// m_linearVelocity += Cross(m_angularVelocity, m_sweep.c - oldCenter);
temp.set(m_sweep.c).subLocal(oldCenter);
Vec2.crossToOut(m_angularVelocity, temp, temp);
m_linearVelocity.addLocal(temp);
m_world.getPool().pushVec2(3);
}
