/**
* Set the active state of the body. An inactive body is not simulated and cannot be collided with
* or woken up. If you pass a flag of true, all fixtures will be added to the broad-phase. If you
* pass a flag of false, all fixtures will be removed from the broad-phase and all contacts will
* be destroyed. Fixtures and joints are otherwise unaffected. You may continue to create/destroy
* fixtures and joints on inactive bodies. Fixtures on an inactive body are implicitly inactive
* and will not participate in collisions, ray-casts, or queries. Joints connected to an inactive
* body are implicitly inactive. An inactive body is still owned by a World object and remains in
* the body list.
*
* @param flag
*/
public void setActive(boolean flag) {
if (flag == isActive()) {
return;
}
if (flag) {
m_flags |= e_activeFlag;
// Create all proxies.
BroadPhase broadPhase = m_world.m_contactManager.m_broadPhase;
for (Fixture f = m_fixtureList; f != null; f = f.m_next) {
f.createProxy(broadPhase, m_xf);
}
// Contacts are created the next time step.
} else {
m_flags &= ~e_activeFlag;
// Destroy all proxies.
BroadPhase broadPhase = m_world.m_contactManager.m_broadPhase;
for (Fixture f = m_fixtureList; f != null; f = f.m_next) {
f.destroyProxy(broadPhase);
}
// Destroy the attached contacts.
ContactEdge ce = m_contactList;
while (ce != null) {
ContactEdge ce0 = ce;
ce = ce.next;
m_world.m_contactManager.destroy(ce0.contact);
}
m_contactList = null;
}
}
/**
* Creates a fixture and attach it to this body. Use this function if you need to set some fixture
* parameters, like friction. Otherwise you can create the fixture directly from a shape. If the
* density is non-zero, this function automatically updates the mass of the body. Contacts are not
* created until the next time step.
*
* @param def the fixture definition.
* @warning This function is locked during callbacks.
*/
public final Fixture createFixture(FixtureDef def) {
assert (m_world.isLocked() == false);
if (m_world.isLocked() == true) {
return null;
}
// djm TODO from pool?
Fixture fixture = new Fixture();
fixture.create(this, def);
if ((m_flags & e_activeFlag) == e_activeFlag) {
BroadPhase broadPhase = m_world.m_contactManager.m_broadPhase;
fixture.createProxy(broadPhase, m_xf);
}
fixture.m_next = m_fixtureList;
m_fixtureList = fixture;
++m_fixtureCount;
fixture.m_body = this;
// Adjust mass properties if needed.
if (fixture.m_density > 0.0f) {
resetMassData();
}
// Let the world know we have a new fixture. This will cause new contacts
// to be created at the beginning of the next time step.
m_world.m_flags |= World.NEW_FIXTURE;
return fixture;
}
public void drawBranch(Branch branch) {
// only draw the branch if it is marked as active
float leafSize =
1.f
+ Main.sqrt(
Main.pow(branch.endX - branch.startX, 2)
+ Main.pow(branch.endY - branch.startY, 2))
/ 160.0f;
if (branch.activeP) {
float alpha = 255.0f;
// if the branch hasn't stopped growing, the alpha is less than 255.0
if (!branch.stoppedGrowing) {
alpha =
255.0f
* (float)
((double) (System.nanoTime() - branch.startGrowTimestamp)
/ (double) Tree.BRANCH_GROW_TIME);
if (alpha >= 255.0f) branch.stoppedGrowing = true;
}
p.stroke(0, 100, 0, alpha);
p.fill(0, 255, 0, alpha);
// Get the shape of the box2d body, and get its coordinates
p.beginShape();
// get the body transform so that we can convert the shape's coordinates to world coordinates
Transform transform = branch.body.getTransform();
Vec2 pos;
for (Fixture f = branch.body.getFixtureList(); f != null; f = f.getNext()) {
PolygonShape shape = (PolygonShape) f.getShape();
for (int i = 0; i < shape.getVertexCount(); i++) {
// apply the transform to the shape coordinates, and
// then convert box2d coordinates to processing coordinates
pos = box2d.coordWorldToPixels(Transform.mul(transform, shape.getVertex(i)));
p.vertex(pos.x, pos.y);
}
}
p.endShape(Main.CLOSE);
// draw leafs after the other branches
p.pushMatrix();
CPoint2 leafPos = branch.getLeafPosition();
p.translate(leafPos.x, leafPos.y);
// p.rotate(leafAngle);
p.rotate(-branch.body.getAngle() + -0.25f * Main.PI); // + Main.PI);
p.noStroke();
p.fill(0, 255, 0, alpha * 0.25f);
p.ellipseMode(Main.CORNER);
float s = 0.0f;
for (int i = 7; i > 2; i--) {
s = leafSize * i;
p.ellipse(0, 0, s, s);
}
p.ellipse(0, 0, s, s);
p.popMatrix();
p.strokeWeight(1.0f);
}
}
public boolean reportFixture(Fixture fixture) {
Body body = fixture.getBody();
if (body.getType() == BodyType.DYNAMIC) {
boolean inside = fixture.testPoint(point);
if (inside) {
this.fixture = fixture;
return false;
}
}
return true;
}
protected final void synchronizeFixtures() {
final Transform xf1 = pxf;
xf1.R.set(m_sweep.a0);
// xf1.position = m_sweep.c0 - Mul(xf1.R, m_sweep.localCenter);
Mat22.mulToOut(xf1.R, m_sweep.localCenter, xf1.position);
xf1.position.mulLocal(-1).addLocal(m_sweep.c0);
BroadPhase broadPhase = m_world.m_contactManager.m_broadPhase;
for (Fixture f = m_fixtureList; f != null; f = f.m_next) {
f.synchronize(broadPhase, xf1, m_xf);
}
}
public float raycastCallback(RayCastInput input, int nodeId) {
Object userData = broadPhase.getUserData(nodeId);
FixtureProxy proxy = (FixtureProxy) userData;
Fixture fixture = proxy.fixture;
int index = proxy.childIndex;
boolean hit = fixture.raycast(output, input, index);
if (hit) {
float fraction = output.fraction;
// Vec2 point = (1.0f - fraction) * input.p1 + fraction * input.p2;
temp.set(input.p2).mulLocal(fraction);
point.set(input.p1).mulLocal(1 - fraction).addLocal(temp);
return callback.reportFixture(fixture, point, output.normal, fraction);
}
return input.maxFraction;
}
@Override
public int hashCode() {
if (f1.m_polygon != null) {
return f1.m_polygon.hashCode();
} else {
return f1.hashCode();
}
}
public Contact popContact(Fixture fixtureA, int indexA, Fixture fixtureB, int indexB) {
final ShapeType type1 = fixtureA.getType();
final ShapeType type2 = fixtureB.getType();
final ContactRegister reg = contactStacks[type1.ordinal()][type2.ordinal()];
final IDynamicStack<Contact> creator = reg.creator;
if (creator != null) {
if (reg.primary) {
Contact c = creator.pop();
c.init(fixtureA, indexA, fixtureB, indexB);
return c;
} else {
Contact c = creator.pop();
c.init(fixtureB, indexB, fixtureA, indexA);
return c;
}
} else {
return null;
}
}
/**
* Set the position of the body's origin and rotation. This breaks any contacts and wakes the
* other bodies. Manipulating a body's transform may cause non-physical behavior.
*
* @param position the world position of the body's local origin.
* @param angle the world rotation in radians.
*/
public final void setTransform(Vec2 position, float angle) {
assert (m_world.isLocked() == false);
if (m_world.isLocked() == true) {
return;
}
m_xf.R.set(angle);
m_xf.position.set(position);
// 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);
m_sweep.a0 = m_sweep.a = angle;
BroadPhase broadPhase = m_world.m_contactManager.m_broadPhase;
for (Fixture f = m_fixtureList; f != null; f = f.m_next) {
f.synchronize(broadPhase, m_xf, m_xf);
}
m_world.m_contactManager.findNewContacts();
}
/** Call this to draw shapes and other debug draw data. */
public void drawDebugData() {
if (m_debugDraw == null) {
return;
}
int flags = m_debugDraw.getFlags();
if ((flags & DebugDraw.e_shapeBit) == DebugDraw.e_shapeBit) {
for (Body b = m_bodyList; b != null; b = b.getNext()) {
xf.set(b.getTransform());
for (Fixture f = b.getFixtureList(); f != null; f = f.getNext()) {
if (b.isActive() == false) {
color.set(0.5f, 0.5f, 0.3f);
drawShape(f, xf, color);
} else if (b.getType() == BodyType.STATIC) {
color.set(0.5f, 0.9f, 0.3f);
drawShape(f, xf, color);
} else if (b.getType() == BodyType.KINEMATIC) {
color.set(0.5f, 0.5f, 0.9f);
drawShape(f, xf, color);
} else if (b.isAwake() == false) {
color.set(0.5f, 0.5f, 0.5f);
drawShape(f, xf, color);
} else {
color.set(0.9f, 0.7f, 0.7f);
drawShape(f, xf, color);
}
}
}
}
if ((flags & DebugDraw.e_jointBit) == DebugDraw.e_jointBit) {
for (Joint j = m_jointList; j != null; j = j.getNext()) {
drawJoint(j);
}
}
if ((flags & DebugDraw.e_pairBit) == DebugDraw.e_pairBit) {
color.set(0.3f, 0.9f, 0.9f);
for (Contact c = m_contactManager.m_contactList; c != null; c = c.getNext()) {
// Fixture fixtureA = c.getFixtureA();
// Fixture fixtureB = c.getFixtureB();
//
// fixtureA.getAABB(childIndex).getCenterToOut(cA);
// fixtureB.getAABB().getCenterToOut(cB);
//
// m_debugDraw.drawSegment(cA, cB, color);
}
}
if ((flags & DebugDraw.e_aabbBit) == DebugDraw.e_aabbBit) {
color.set(0.9f, 0.3f, 0.9f);
for (Body b = m_bodyList; b != null; b = b.getNext()) {
if (b.isActive() == false) {
continue;
}
for (Fixture f = b.getFixtureList(); f != null; f = f.getNext()) {
for (int i = 0; i < f.m_proxyCount; ++i) {
FixtureProxy proxy = f.m_proxies[i];
AABB aabb = m_contactManager.m_broadPhase.getFatAABB(proxy.proxyId);
Vec2[] vs = avs.get(4);
vs[0].set(aabb.lowerBound.x, aabb.lowerBound.y);
vs[1].set(aabb.upperBound.x, aabb.lowerBound.y);
vs[2].set(aabb.upperBound.x, aabb.upperBound.y);
vs[3].set(aabb.lowerBound.x, aabb.upperBound.y);
m_debugDraw.drawPolygon(vs, 4, color);
}
}
}
}
if ((flags & DebugDraw.e_centerOfMassBit) == DebugDraw.e_centerOfMassBit) {
for (Body b = m_bodyList; b != null; b = b.getNext()) {
xf.set(b.getTransform());
xf.p.set(b.getWorldCenter());
m_debugDraw.drawTransform(xf);
}
}
if ((flags & DebugDraw.e_dynamicTreeBit) == DebugDraw.e_dynamicTreeBit) {
m_contactManager.m_broadPhase.drawTree(m_debugDraw);
}
}
/**
* destroy a rigid body given a definition. No reference to the definition is retained. This
* function is locked during callbacks.
*
* @warning This automatically deletes all associated shapes and joints.
* @warning This function is locked during callbacks.
* @param body
*/
public void destroyBody(Body body) {
assert (m_bodyCount > 0);
assert (isLocked() == false);
if (isLocked()) {
return;
}
// Delete the attached joints.
JointEdge je = body.m_jointList;
while (je != null) {
JointEdge je0 = je;
je = je.next;
if (m_destructionListener != null) {
m_destructionListener.sayGoodbye(je0.joint);
}
destroyJoint(je0.joint);
body.m_jointList = je;
}
body.m_jointList = null;
// Delete the attached contacts.
ContactEdge ce = body.m_contactList;
while (ce != null) {
ContactEdge ce0 = ce;
ce = ce.next;
m_contactManager.destroy(ce0.contact);
}
body.m_contactList = null;
Fixture f = body.m_fixtureList;
while (f != null) {
Fixture f0 = f;
f = f.m_next;
if (m_destructionListener != null) {
m_destructionListener.sayGoodbye(f0);
}
f0.destroyProxies(m_contactManager.m_broadPhase);
f0.destroy();
// TODO djm recycle fixtures (here or in that destroy method)
body.m_fixtureList = f;
body.m_fixtureCount -= 1;
}
body.m_fixtureList = null;
body.m_fixtureCount = 0;
// Remove world body list.
if (body.m_prev != null) {
body.m_prev.m_next = body.m_next;
}
if (body.m_next != null) {
body.m_next.m_prev = body.m_prev;
}
if (body == m_bodyList) {
m_bodyList = body.m_next;
}
--m_bodyCount;
// TODO djm recycle body
}
private void drawShape(Fixture fixture, Transform xf, Color3f color) {
switch (fixture.getType()) {
case CIRCLE:
{
CircleShape circle = (CircleShape) fixture.getShape();
// Vec2 center = Mul(xf, circle.m_p);
Transform.mulToOutUnsafe(xf, circle.m_p, center);
float radius = circle.m_radius;
xf.q.getXAxis(axis);
if (fixture.getUserData() != null && fixture.getUserData().equals(LIQUID_INT)) {
Body b = fixture.getBody();
liquidOffset.set(b.m_linearVelocity);
float linVelLength = b.m_linearVelocity.length();
if (averageLinearVel == -1) {
averageLinearVel = linVelLength;
} else {
averageLinearVel = .98f * averageLinearVel + .02f * linVelLength;
}
liquidOffset.mulLocal(liquidLength / averageLinearVel / 2);
circCenterMoved.set(center).addLocal(liquidOffset);
center.subLocal(liquidOffset);
m_debugDraw.drawSegment(center, circCenterMoved, liquidColor);
return;
}
m_debugDraw.drawSolidCircle(center, radius, axis, color);
}
break;
case POLYGON:
{
PolygonShape poly = (PolygonShape) fixture.getShape();
int vertexCount = poly.m_count;
assert (vertexCount <= Settings.maxPolygonVertices);
Vec2[] vertices = tlvertices.get(Settings.maxPolygonVertices);
for (int i = 0; i < vertexCount; ++i) {
// vertices[i] = Mul(xf, poly.m_vertices[i]);
Transform.mulToOutUnsafe(xf, poly.m_vertices[i], vertices[i]);
}
m_debugDraw.drawSolidPolygon(vertices, vertexCount, color);
}
break;
case EDGE:
{
EdgeShape edge = (EdgeShape) fixture.getShape();
Transform.mulToOutUnsafe(xf, edge.m_vertex1, v1);
Transform.mulToOutUnsafe(xf, edge.m_vertex2, v2);
m_debugDraw.drawSegment(v1, v2, color);
}
break;
case CHAIN:
{
ChainShape chain = (ChainShape) fixture.getShape();
int count = chain.m_count;
Vec2[] vertices = chain.m_vertices;
Transform.mulToOutUnsafe(xf, vertices[0], v1);
for (int i = 1; i < count; ++i) {
Transform.mulToOutUnsafe(xf, vertices[i], v2);
m_debugDraw.drawSegment(v1, v2, color);
m_debugDraw.drawCircle(v1, 0.05f, color);
v1.set(v2);
}
}
break;
default:
break;
}
}
private void solveTOI(final TimeStep step) {
final Island island = toiIsland;
island.init(
2 * Settings.maxTOIContacts,
Settings.maxTOIContacts,
0,
m_contactManager.m_contactListener);
if (m_stepComplete) {
for (Body b = m_bodyList; b != null; b = b.m_next) {
b.m_flags &= ~Body.e_islandFlag;
b.m_sweep.alpha0 = 0.0f;
}
for (Contact c = m_contactManager.m_contactList; c != null; c = c.m_next) {
// Invalidate TOI
c.m_flags &= ~(Contact.TOI_FLAG | Contact.ISLAND_FLAG);
c.m_toiCount = 0;
c.m_toi = 1.0f;
}
}
// Find TOI events and solve them.
for (; ; ) {
// Find the first TOI.
Contact minContact = null;
float minAlpha = 1.0f;
for (Contact c = m_contactManager.m_contactList; c != null; c = c.m_next) {
// Is this contact disabled?
if (c.isEnabled() == false) {
continue;
}
// Prevent excessive sub-stepping.
if (c.m_toiCount > Settings.maxSubSteps) {
continue;
}
float alpha = 1.0f;
if ((c.m_flags & Contact.TOI_FLAG) != 0) {
// This contact has a valid cached TOI.
alpha = c.m_toi;
} else {
Fixture fA = c.getFixtureA();
Fixture fB = c.getFixtureB();
// Is there a sensor?
if (fA.isSensor() || fB.isSensor()) {
continue;
}
Body bA = fA.getBody();
Body bB = fB.getBody();
BodyType typeA = bA.m_type;
BodyType typeB = bB.m_type;
assert (typeA == BodyType.DYNAMIC || typeB == BodyType.DYNAMIC);
boolean activeA = bA.isAwake() && typeA != BodyType.STATIC;
boolean activeB = bB.isAwake() && typeB != BodyType.STATIC;
// Is at least one body active (awake and dynamic or kinematic)?
if (activeA == false && activeB == false) {
continue;
}
boolean collideA = bA.isBullet() || typeA != BodyType.DYNAMIC;
boolean collideB = bB.isBullet() || typeB != BodyType.DYNAMIC;
// Are these two non-bullet dynamic bodies?
if (collideA == false && collideB == false) {
continue;
}
// Compute the TOI for this contact.
// Put the sweeps onto the same time interval.
float alpha0 = bA.m_sweep.alpha0;
if (bA.m_sweep.alpha0 < bB.m_sweep.alpha0) {
alpha0 = bB.m_sweep.alpha0;
bA.m_sweep.advance(alpha0);
} else if (bB.m_sweep.alpha0 < bA.m_sweep.alpha0) {
alpha0 = bA.m_sweep.alpha0;
bB.m_sweep.advance(alpha0);
}
assert (alpha0 < 1.0f);
int indexA = c.getChildIndexA();
int indexB = c.getChildIndexB();
// Compute the time of impact in interval [0, minTOI]
final TOIInput input = toiInput;
input.proxyA.set(fA.getShape(), indexA);
input.proxyB.set(fB.getShape(), indexB);
input.sweepA.set(bA.m_sweep);
input.sweepB.set(bB.m_sweep);
input.tMax = 1.0f;
pool.getTimeOfImpact().timeOfImpact(toiOutput, input);
// Beta is the fraction of the remaining portion of the .
float beta = toiOutput.t;
if (toiOutput.state == TOIOutputState.TOUCHING) {
alpha = MathUtils.min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
} else {
alpha = 1.0f;
}
c.m_toi = alpha;
c.m_flags |= Contact.TOI_FLAG;
}
if (alpha < minAlpha) {
// This is the minimum TOI found so far.
minContact = c;
minAlpha = alpha;
}
}
if (minContact == null || 1.0f - 10.0f * Settings.EPSILON < minAlpha) {
// No more TOI events. Done!
m_stepComplete = true;
break;
}
// Advance the bodies to the TOI.
Fixture fA = minContact.getFixtureA();
Fixture fB = minContact.getFixtureB();
Body bA = fA.getBody();
Body bB = fB.getBody();
backup1.set(bA.m_sweep);
backup2.set(bB.m_sweep);
bA.advance(minAlpha);
bB.advance(minAlpha);
// The TOI contact likely has some new contact points.
minContact.update(m_contactManager.m_contactListener);
minContact.m_flags &= ~Contact.TOI_FLAG;
++minContact.m_toiCount;
// Is the contact solid?
if (minContact.isEnabled() == false || minContact.isTouching() == false) {
// Restore the sweeps.
minContact.setEnabled(false);
bA.m_sweep.set(backup1);
bB.m_sweep.set(backup2);
bA.synchronizeTransform();
bB.synchronizeTransform();
continue;
}
bA.setAwake(true);
bB.setAwake(true);
// Build the island
island.clear();
island.add(bA);
island.add(bB);
island.add(minContact);
bA.m_flags |= Body.e_islandFlag;
bB.m_flags |= Body.e_islandFlag;
minContact.m_flags |= Contact.ISLAND_FLAG;
// Get contacts on bodyA and bodyB.
tempBodies[0] = bA;
tempBodies[1] = bB;
for (int i = 0; i < 2; ++i) {
Body body = tempBodies[i];
if (body.m_type == BodyType.DYNAMIC) {
for (ContactEdge ce = body.m_contactList; ce != null; ce = ce.next) {
if (island.m_bodyCount == island.m_bodyCapacity) {
break;
}
if (island.m_contactCount == island.m_contactCapacity) {
break;
}
Contact contact = ce.contact;
// Has this contact already been added to the island?
if ((contact.m_flags & Contact.ISLAND_FLAG) != 0) {
continue;
}
// Only add static, kinematic, or bullet bodies.
Body other = ce.other;
if (other.m_type == BodyType.DYNAMIC
&& body.isBullet() == false
&& other.isBullet() == false) {
continue;
}
// Skip sensors.
boolean sensorA = contact.m_fixtureA.m_isSensor;
boolean sensorB = contact.m_fixtureB.m_isSensor;
if (sensorA || sensorB) {
continue;
}
// Tentatively advance the body to the TOI.
backup1.set(other.m_sweep);
if ((other.m_flags & Body.e_islandFlag) == 0) {
other.advance(minAlpha);
}
// Update the contact points
contact.update(m_contactManager.m_contactListener);
// Was the contact disabled by the user?
if (contact.isEnabled() == false) {
other.m_sweep.set(backup1);
other.synchronizeTransform();
continue;
}
// Are there contact points?
if (contact.isTouching() == false) {
other.m_sweep.set(backup1);
other.synchronizeTransform();
continue;
}
// Add the contact to the island
contact.m_flags |= Contact.ISLAND_FLAG;
island.add(contact);
// Has the other body already been added to the island?
if ((other.m_flags & Body.e_islandFlag) != 0) {
continue;
}
// Add the other body to the island.
other.m_flags |= Body.e_islandFlag;
if (other.m_type != BodyType.STATIC) {
other.setAwake(true);
}
island.add(other);
}
}
}
subStep.dt = (1.0f - minAlpha) * step.dt;
subStep.inv_dt = 1.0f / subStep.dt;
subStep.dtRatio = 1.0f;
subStep.positionIterations = 20;
subStep.velocityIterations = step.velocityIterations;
subStep.warmStarting = false;
island.solveTOI(subStep, bA.m_islandIndex, bB.m_islandIndex);
// Reset island flags and synchronize broad-phase proxies.
for (int i = 0; i < island.m_bodyCount; ++i) {
Body body = island.m_bodies[i];
body.m_flags &= ~Body.e_islandFlag;
if (body.m_type != BodyType.DYNAMIC) {
continue;
}
body.synchronizeFixtures();
// Invalidate all contact TOIs on this displaced body.
for (ContactEdge ce = body.m_contactList; ce != null; ce = ce.next) {
ce.contact.m_flags &= ~(Contact.TOI_FLAG | Contact.ISLAND_FLAG);
}
}
// Commit fixture proxy movements to the broad-phase so that new contacts are created.
// Also, some contacts can be destroyed.
m_contactManager.findNewContacts();
if (m_subStepping) {
m_stepComplete = false;
break;
}
}
}
/**
* 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);
}
/**
* Destroy a fixture. This removes the fixture from the broad-phase and destroys all contacts
* associated with this fixture. This will automatically adjust the mass of the body if the body
* is dynamic and the fixture has positive density. All fixtures attached to a body are implicitly
* destroyed when the body is destroyed.
*
* @param fixture the fixture to be removed.
* @warning This function is locked during callbacks.
*/
public final void destroyFixture(Fixture fixture) {
assert (m_world.isLocked() == false);
if (m_world.isLocked() == true) {
return;
}
assert (fixture.m_body == this);
// Remove the fixture from this body's singly linked list.
assert (m_fixtureCount > 0);
Fixture node = m_fixtureList;
Fixture last = null; // java change
boolean found = false;
while (node != null) {
if (node == fixture) {
node = fixture.m_next;
found = true;
break;
}
last = node;
node = node.m_next;
}
// You tried to remove a shape that is not attached to this body.
assert (found);
// java change, remove it from the list
if (last == null) {
m_fixtureList = fixture.m_next;
} else {
last.m_next = fixture.m_next;
}
// Destroy any contacts associated with the fixture.
ContactEdge edge = m_contactList;
while (edge != null) {
Contact c = edge.contact;
edge = edge.next;
Fixture fixtureA = c.getFixtureA();
Fixture fixtureB = c.getFixtureB();
if (fixture == fixtureA || fixture == fixtureB) {
// This destroys the contact and removes it from
// this body's contact list.
m_world.m_contactManager.destroy(c);
}
}
if ((m_flags & e_activeFlag) == e_activeFlag) {
assert (fixture.m_proxy != null);
BroadPhase broadPhase = m_world.m_contactManager.m_broadPhase;
fixture.destroyProxy(broadPhase);
} else {
assert (fixture.m_proxy == null);
}
fixture.destroy();
fixture.m_body = null;
fixture.m_next = null;
fixture = null;
--m_fixtureCount;
// Reset the mass data.
resetMassData();
}