private void computeBounds(int[] lowerValues, int[] upperValues, AABB aabb) {
if (debugPrint) {
System.out.println("ComputeBounds()");
}
assert (aabb.upperBound.x > aabb.lowerBound.x);
assert (aabb.upperBound.y > aabb.lowerBound.y);
Vec2 minVertex =
MathUtils.clamp(aabb.lowerBound, m_worldAABB.lowerBound, m_worldAABB.upperBound);
Vec2 maxVertex =
MathUtils.clamp(aabb.upperBound, m_worldAABB.lowerBound, m_worldAABB.upperBound);
// System.out.printf("minV = %f %f, maxV = %f %f
// \n",aabb.minVertex.x,aabb.minVertex.y,aabb.maxVertex.x,aabb.maxVertex.y);
// Bump lower bounds downs and upper bounds up. This ensures correct
// sorting of
// lower/upper bounds that would have equal values.
// TODO_ERIN implement fast float to int conversion.
lowerValues[0] =
(int) (m_quantizationFactor.x * (minVertex.x - m_worldAABB.lowerBound.x))
& (Integer.MAX_VALUE - 1);
upperValues[0] = (int) (m_quantizationFactor.x * (maxVertex.x - m_worldAABB.lowerBound.x)) | 1;
lowerValues[1] =
(int) (m_quantizationFactor.y * (minVertex.y - m_worldAABB.lowerBound.y))
& (Integer.MAX_VALUE - 1);
upperValues[1] = (int) (m_quantizationFactor.y * (maxVertex.y - m_worldAABB.lowerBound.y)) | 1;
}
public void paint(Clock clock) {
if (!hasLoaded) return;
// body.setLinearVelocity(new Vec2(30 * MathUtils.cos(GameScreen.angle), 30 *
// MathUtils.sin(GameScreen.angle)));
sprite
.layer()
.setTranslation(
(body.getPosition().x / GameScreen.M_PER_PIXEL),
(body.getPosition().y / GameScreen.M_PER_PIXEL));
Vec2 delta = new Vec2(80f - body.getPosition().x, 400f - body.getPosition().y);
float angle = MathUtils.atan2(delta.x, delta.y);
body.setLinearVelocity(new Vec2(8 * -MathUtils.cos(angle), 8 * MathUtils.sin(angle)));
}
private void validateMetrics(DynamicTreeNode node) {
if (node == null) {
return;
}
DynamicTreeNode child1 = node.child1;
DynamicTreeNode child2 = node.child2;
if (node.child1 == null) {
assert (child1 == null);
assert (child2 == null);
assert (node.height == 0);
return;
}
assert (child1 != null && 0 <= child1.id && child1.id < m_nodeCapacity);
assert (child2 != null && 0 <= child2.id && child2.id < m_nodeCapacity);
int height1 = child1.height;
int height2 = child2.height;
int height;
height = 1 + org.jbox2d.common.MathUtils.max(height1, height2);
assert (node.height == height);
org.jbox2d.collision.AABB aabb = new org.jbox2d.collision.AABB();
aabb.combine(child1.aabb, child2.aabb);
assert (aabb.lowerBound.equals(node.aabb.lowerBound));
assert (aabb.upperBound.equals(node.aabb.upperBound));
validateMetrics(child1);
validateMetrics(child2);
}
@Override
public int getMaxBalance() {
int maxBalance = 0;
for (int i = 0; i < m_nodeCapacity; ++i) {
final DynamicTreeNode node = m_nodes[i];
if (node.height <= 1) {
continue;
}
assert (node.child1 == null == false);
DynamicTreeNode child1 = node.child1;
DynamicTreeNode child2 = node.child2;
int balance = org.jbox2d.common.MathUtils.abs(child2.height - child1.height);
maxBalance = org.jbox2d.common.MathUtils.max(maxBalance, balance);
}
return maxBalance;
}
private final int computeHeight(DynamicTreeNode node) {
assert (0 <= node.id && node.id < m_nodeCapacity);
if (node.child1 == null) {
return 0;
}
int height1 = computeHeight(node.child1);
int height2 = computeHeight(node.child2);
return 1 + org.jbox2d.common.MathUtils.max(height1, height2);
}
public final void synchronizeTransform() {
// m_xf.R.set(m_sweep.a);
//
// //m_xf.position = m_sweep.c - Mul(m_xf.R, m_sweep.localCenter);
// Mat22.mulToOut(m_xf.R, m_sweep.localCenter, m_xf.position);
// m_xf.position.mulLocal(-1).addLocal(m_sweep.c);
final float c = MathUtils.cos(m_sweep.a), s = MathUtils.sin(m_sweep.a);
m_xf.R.m11 = c;
m_xf.R.m21 = -s;
m_xf.R.m12 = s;
m_xf.R.m22 = c;
m_xf.position.x = m_xf.R.m11 * m_sweep.localCenter.x + m_xf.R.m21 * m_sweep.localCenter.y;
m_xf.position.y = m_xf.R.m12 * m_sweep.localCenter.x + m_xf.R.m22 * m_sweep.localCenter.y;
m_xf.position.x *= -1f;
m_xf.position.y *= -1f;
m_xf.position.x += m_sweep.c.x;
m_xf.position.y += m_sweep.c.y;
}
/** @see Shape#updateSweepRadius(Vec2) */
@Override
public void updateSweepRadius(final Vec2 center) {
// Update the sweep radius (maximum radius) as measured from
// a local center point.
final float dx = m_coreV1.x - center.x;
final float dy = m_coreV1.y - center.y;
final float d1 = dx * dx + dy * dy;
final float dx2 = m_coreV2.x - center.x;
final float dy2 = m_coreV2.y - center.y;
final float d2 = dx2 * dx2 + dy2 * dy2;
m_sweepRadius = MathUtils.sqrt(d1 > d2 ? d1 : d2);
}
@Override
public void drawCircle(Vec2 center, float radius, Vec2 axis, Color3f color) {
Graphics2D g = getGraphics();
saveState(g);
transformGraphics(g, center);
g.setStroke(stroke);
Color s = cpool.getColor(color.x, color.y, color.z, 1f);
g.scale(radius, radius);
g.setColor(s);
g.draw(circle);
if (axis != null) {
g.rotate(MathUtils.atan2(axis.y, axis.x));
g.drawLine(0, 0, 1, 0);
}
restoreState(g);
}
private final void removeLeaf(DynamicTreeNode leaf) {
if (leaf == m_root) {
m_root = null;
return;
}
DynamicTreeNode parent = leaf.parent;
DynamicTreeNode grandParent = parent.parent;
DynamicTreeNode sibling;
if (parent.child1 == leaf) {
sibling = parent.child2;
} else {
sibling = parent.child1;
}
if (grandParent != null) {
// Destroy parent and connect sibling to grandParent.
if (grandParent.child1 == parent) {
grandParent.child1 = sibling;
} else {
grandParent.child2 = sibling;
}
sibling.parent = grandParent;
freeNode(parent);
// Adjust ancestor bounds.
DynamicTreeNode index = grandParent;
while (index != null) {
index = balance(index);
DynamicTreeNode child1 = index.child1;
DynamicTreeNode child2 = index.child2;
index.aabb.combine(child1.aabb, child2.aabb);
index.height = 1 + org.jbox2d.common.MathUtils.max(child1.height, child2.height);
index = index.parent;
}
} else {
m_root = sibling;
sibling.parent = null;
freeNode(parent);
}
// validate();
}
public void create(Body body, FixtureDef def) {
m_userData = def.userData;
m_friction = def.friction;
m_restitution = def.restitution;
m_body = body;
m_next = null;
m_filter.set(def.filter);
m_isSensor = def.isSensor;
m_shape = def.shape.clone();
// moj doplneny kod
m_material = def.material;
m_polygon = def.polygon;
// Reserve proxy space
int childCount = m_shape.getChildCount();
if (m_proxies == null) {
m_proxies = new FixtureProxy[childCount];
for (int i = 0; i < childCount; i++) {
m_proxies[i] = new FixtureProxy();
m_proxies[i].fixture = null;
m_proxies[i].proxyId = BroadPhase.NULL_PROXY;
}
}
if (m_proxies.length < childCount) {
FixtureProxy[] old = m_proxies;
int newLen = MathUtils.max(old.length * 2, childCount);
m_proxies = new FixtureProxy[newLen];
System.arraycopy(old, 0, m_proxies, 0, old.length);
for (int i = 0; i < newLen; i++) {
if (i >= old.length) {
m_proxies[i] = new FixtureProxy();
}
m_proxies[i].fixture = null;
m_proxies[i].proxyId = BroadPhase.NULL_PROXY;
}
}
m_proxyCount = 0;
m_density = def.density;
}
public final void initialize(
final Manifold manifold,
final Transform xfA,
float radiusA,
final Transform xfB,
float radiusB) {
if (manifold.pointCount == 0) {
return;
}
switch (manifold.type) {
case CIRCLES:
{
// final Vec2 pointA = pool3;
// final Vec2 pointB = pool4;
//
// normal.set(1, 0);
// Transform.mulToOut(xfA, manifold.localPoint, pointA);
// Transform.mulToOut(xfB, manifold.points[0].localPoint, pointB);
//
// if (MathUtils.distanceSquared(pointA, pointB) > Settings.EPSILON * Settings.EPSILON)
// {
// normal.set(pointB).subLocal(pointA);
// normal.normalize();
// }
//
// cA.set(normal).mulLocal(radiusA).addLocal(pointA);
// cB.set(normal).mulLocal(radiusB).subLocal(pointB).negateLocal();
// points[0].set(cA).addLocal(cB).mulLocal(0.5f);
final Vec2 pointA = pool3;
final Vec2 pointB = pool4;
normal.x = 1;
normal.y = 0;
pointA.x =
xfA.position.x
+ xfA.R.col1.x * manifold.localPoint.x
+ xfA.R.col2.x * manifold.localPoint.y;
pointA.y =
xfA.position.y
+ xfA.R.col1.y * manifold.localPoint.x
+ xfA.R.col2.y * manifold.localPoint.y;
pointB.x =
xfB.position.x
+ xfB.R.col1.x * manifold.points[0].localPoint.x
+ xfB.R.col2.x * manifold.points[0].localPoint.y;
pointB.y =
xfB.position.y
+ xfB.R.col1.y * manifold.points[0].localPoint.x
+ xfB.R.col2.y * manifold.points[0].localPoint.y;
if (MathUtils.distanceSquared(pointA, pointB) > Settings.EPSILON * Settings.EPSILON) {
normal.x = pointB.x - pointA.x;
normal.y = pointB.y - pointA.y;
normal.normalize();
}
final float cAx = normal.x * radiusA + pointA.x;
final float cAy = normal.y * radiusA + pointA.y;
final float cBx = -normal.x * radiusB + pointB.x;
final float cBy = -normal.y * radiusB + pointB.y;
points[0].x = (cAx + cBx) * .5f;
points[0].y = (cAy + cBy) * .5f;
}
break;
case FACE_A:
{
// final Vec2 planePoint = pool3;
//
// Mat22.mulToOut(xfA.R, manifold.localNormal, normal);
// Transform.mulToOut(xfA, manifold.localPoint, planePoint);
//
// final Vec2 clipPoint = pool4;
//
// for (int i = 0; i < manifold.pointCount; i++) {
// // b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint);
// // b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint,
// // normal)) * normal;
// // b2Vec2 cB = clipPoint - radiusB * normal;
// // points[i] = 0.5f * (cA + cB);
// Transform.mulToOut(xfB, manifold.points[i].localPoint, clipPoint);
// // use cA as temporary for now
// cA.set(clipPoint).subLocal(planePoint);
// float scalar = radiusA - Vec2.dot(cA, normal);
// cA.set(normal).mulLocal(scalar).addLocal(clipPoint);
// cB.set(normal).mulLocal(radiusB).subLocal(clipPoint).negateLocal();
// points[i].set(cA).addLocal(cB).mulLocal(0.5f);
// }
final Vec2 planePoint = pool3;
normal.x = xfA.R.col1.x * manifold.localNormal.x + xfA.R.col2.x * manifold.localNormal.y;
normal.y = xfA.R.col1.y * manifold.localNormal.x + xfA.R.col2.y * manifold.localNormal.y;
planePoint.x =
xfA.position.x
+ xfA.R.col1.x * manifold.localPoint.x
+ xfA.R.col2.x * manifold.localPoint.y;
planePoint.y =
xfA.position.y
+ xfA.R.col1.y * manifold.localPoint.x
+ xfA.R.col2.y * manifold.localPoint.y;
final Vec2 clipPoint = pool4;
for (int i = 0; i < manifold.pointCount; i++) {
// b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint);
// b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint,
// normal)) * normal;
// b2Vec2 cB = clipPoint - radiusB * normal;
// points[i] = 0.5f * (cA + cB);
clipPoint.x =
xfB.position.x
+ xfB.R.col1.x * manifold.points[i].localPoint.x
+ xfB.R.col2.x * manifold.points[i].localPoint.y;
clipPoint.y =
xfB.position.y
+ xfB.R.col1.y * manifold.points[i].localPoint.x
+ xfB.R.col2.y * manifold.points[i].localPoint.y;
final float scalar =
radiusA
- ((clipPoint.x - planePoint.x) * normal.x
+ (clipPoint.y - planePoint.y) * normal.y);
final float cAx = normal.x * scalar + clipPoint.x;
final float cAy = normal.y * scalar + clipPoint.y;
final float cBx = -normal.x * radiusB + clipPoint.x;
final float cBy = -normal.y * radiusB + clipPoint.y;
points[i].x = (cAx + cBx) * .5f;
points[i].y = (cAy + cBy) * .5f;
}
}
break;
case FACE_B:
final Vec2 planePoint = pool3;
final Mat22 R = xfB.R;
normal.x = R.col1.x * manifold.localNormal.x + R.col2.x * manifold.localNormal.y;
normal.y = R.col1.y * manifold.localNormal.x + R.col2.y * manifold.localNormal.y;
final Vec2 v = manifold.localPoint;
planePoint.x = xfB.position.x + xfB.R.col1.x * v.x + xfB.R.col2.x * v.y;
planePoint.y = xfB.position.y + xfB.R.col1.y * v.x + xfB.R.col2.y * v.y;
final Vec2 clipPoint = pool4;
for (int i = 0; i < manifold.pointCount; i++) {
// b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint);
// b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint,
// normal)) * normal;
// b2Vec2 cA = clipPoint - radiusA * normal;
// points[i] = 0.5f * (cA + cB);
// Transform.mulToOut(xfA, manifold.points[i].localPoint, clipPoint);
// cB.set(clipPoint).subLocal(planePoint);
// float scalar = radiusB - Vec2.dot(cB, normal);
// cB.set(normal).mulLocal(scalar).addLocal(clipPoint);
// cA.set(normal).mulLocal(radiusA).subLocal(clipPoint).negateLocal();
// points[i].set(cA).addLocal(cB).mulLocal(0.5f);
// points[i] = 0.5f * (cA + cB);
clipPoint.x =
xfA.position.x
+ xfA.R.col1.x * manifold.points[i].localPoint.x
+ xfA.R.col2.x * manifold.points[i].localPoint.y;
clipPoint.y =
xfA.position.y
+ xfA.R.col1.y * manifold.points[i].localPoint.x
+ xfA.R.col2.y * manifold.points[i].localPoint.y;
final float scalar =
radiusB
- ((clipPoint.x - planePoint.x) * normal.x
+ (clipPoint.y - planePoint.y) * normal.y);
final float cBx = normal.x * scalar + clipPoint.x;
final float cBy = normal.y * scalar + clipPoint.y;
final float cAx = -normal.x * radiusA + clipPoint.x;
final float cAy = -normal.y * radiusA + clipPoint.y;
points[i].x = (cAx + cBx) * .5f;
points[i].y = (cAy + cBy) * .5f;
}
// Ensure normal points from A to B.
normal.x = -normal.x;
normal.y = -normal.y;
break;
}
}
/**
* Create a convex hull from the given array of points. The count must be in the range [3,
* Settings.maxPolygonVertices]. This method takes an arraypool for pooling
*
* @warning the points may be re-ordered, even if they form a convex polygon
* @warning collinear points are handled but not removed. Collinear points may lead to poor
* stacking behavior.
*/
public final void set(
final Vec2[] verts, final int num, final Vec2Array vecPool, final IntArray intPool) {
assert (3 <= num && num <= Settings.maxPolygonVertices);
if (num < 3) {
setAsBox(1.0f, 1.0f);
return;
}
int n = MathUtils.min(num, Settings.maxPolygonVertices);
// Copy the vertices into a local buffer
Vec2[] ps = (vecPool != null) ? vecPool.get(n) : new Vec2[n];
for (int i = 0; i < n; ++i) {
ps[i] = verts[i];
}
// Create the convex hull using the Gift wrapping algorithm
// http://en.wikipedia.org/wiki/Gift_wrapping_algorithm
// Find the right most point on the hull
int i0 = 0;
float x0 = ps[0].x;
for (int i = 1; i < num; ++i) {
float x = ps[i].x;
if (x > x0 || (x == x0 && ps[i].y < ps[i0].y)) {
i0 = i;
x0 = x;
}
}
int[] hull =
(intPool != null)
? intPool.get(Settings.maxPolygonVertices)
: new int[Settings.maxPolygonVertices];
int m = 0;
int ih = i0;
while (true) {
hull[m] = ih;
int ie = 0;
for (int j = 1; j < n; ++j) {
if (ie == ih) {
ie = j;
continue;
}
Vec2 r = pool1.set(ps[ie]).subLocal(ps[hull[m]]);
Vec2 v = pool2.set(ps[j]).subLocal(ps[hull[m]]);
float c = Vec2.cross(r, v);
if (c < 0.0f) {
ie = j;
}
// Collinearity check
if (c == 0.0f && v.lengthSquared() > r.lengthSquared()) {
ie = j;
}
}
++m;
ih = ie;
if (ie == i0) {
break;
}
}
this.m_count = m;
// Copy vertices.
for (int i = 0; i < m_count; ++i) {
if (m_vertices[i] == null) {
m_vertices[i] = new Vec2();
}
m_vertices[i].set(ps[hull[i]]);
}
final Vec2 edge = pool1;
// Compute normals. Ensure the edges have non-zero length.
for (int i = 0; i < m_count; ++i) {
final int i1 = i;
final int i2 = i + 1 < m_count ? i + 1 : 0;
edge.set(m_vertices[i2]).subLocal(m_vertices[i1]);
assert (edge.lengthSquared() > Settings.EPSILON * Settings.EPSILON);
Vec2.crossToOutUnsafe(edge, 1f, m_normals[i]);
m_normals[i].normalize();
}
// Compute the polygon centroid.
computeCentroidToOut(m_vertices, m_count, m_centroid);
}
@Override
public void initVelocityConstraints(final org.jbox2d.dynamics.SolverData data) {
m_indexA = m_bodyA.m_islandIndex;
m_indexB = m_bodyB.m_islandIndex;
m_localCenterA.set(m_bodyA.m_sweep.localCenter);
m_localCenterB.set(m_bodyB.m_sweep.localCenter);
m_invMassA = m_bodyA.m_invMass;
m_invMassB = m_bodyB.m_invMass;
m_invIA = m_bodyA.m_invI;
m_invIB = m_bodyB.m_invI;
// Vec2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
Vec2 vA = data.velocities[m_indexA].v;
float wA = data.velocities[m_indexA].w;
// Vec2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
Vec2 vB = data.velocities[m_indexB].v;
float wB = data.velocities[m_indexB].w;
final Rot qA = pool.popRot();
final Rot qB = pool.popRot();
final Vec2 temp = pool.popVec2();
qA.set(aA);
qB.set(aB);
// Compute the effective masses.
Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), m_rA);
Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), m_rB);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
boolean fixedRotation = (iA + iB == 0.0f);
m_mass.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;
m_mass.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;
m_mass.ez.x = -m_rA.y * iA - m_rB.y * iB;
m_mass.ex.y = m_mass.ey.x;
m_mass.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;
m_mass.ez.y = m_rA.x * iA + m_rB.x * iB;
m_mass.ex.z = m_mass.ez.x;
m_mass.ey.z = m_mass.ez.y;
m_mass.ez.z = iA + iB;
m_motorMass = iA + iB;
if (m_motorMass > 0.0f) {
m_motorMass = 1.0f / m_motorMass;
}
if (m_enableMotor == false || fixedRotation) {
m_motorImpulse = 0.0f;
}
if (m_enableLimit && fixedRotation == false) {
float jointAngle = aB - aA - m_referenceAngle;
if (MathUtils.abs(m_upperAngle - m_lowerAngle) < 2.0f * Settings.angularSlop) {
m_limitState = LimitState.EQUAL;
} else if (jointAngle <= m_lowerAngle) {
if (m_limitState != LimitState.AT_LOWER) {
m_impulse.z = 0.0f;
}
m_limitState = LimitState.AT_LOWER;
} else if (jointAngle >= m_upperAngle) {
if (m_limitState != LimitState.AT_UPPER) {
m_impulse.z = 0.0f;
}
m_limitState = LimitState.AT_UPPER;
} else {
m_limitState = LimitState.INACTIVE;
m_impulse.z = 0.0f;
}
} else {
m_limitState = LimitState.INACTIVE;
}
if (data.step.warmStarting) {
final Vec2 P = pool.popVec2();
// Scale impulses to support a variable time step.
m_impulse.x *= data.step.dtRatio;
m_impulse.y *= data.step.dtRatio;
m_motorImpulse *= data.step.dtRatio;
P.x = m_impulse.x;
P.y = m_impulse.y;
vA.x -= mA * P.x;
vA.y -= mA * P.y;
wA -= iA * (Vec2.cross(m_rA, P) + m_motorImpulse + m_impulse.z);
vB.x += mB * P.x;
vB.y += mB * P.y;
wB += iB * (Vec2.cross(m_rB, P) + m_motorImpulse + m_impulse.z);
pool.pushVec2(1);
} else {
m_impulse.setZero();
m_motorImpulse = 0.0f;
}
// data.velocities[m_indexA].v.set(vA);
data.velocities[m_indexA].w = wA;
// data.velocities[m_indexB].v.set(vB);
data.velocities[m_indexB].w = wB;
pool.pushVec2(1);
pool.pushRot(2);
}
// Perform a left or right rotation if node A is imbalanced.
// Returns the new root index.
private DynamicTreeNode balance(DynamicTreeNode iA) {
assert (iA != null);
DynamicTreeNode A = iA;
if (A.child1 == null || A.height < 2) {
return iA;
}
DynamicTreeNode iB = A.child1;
DynamicTreeNode iC = A.child2;
assert (0 <= iB.id && iB.id < m_nodeCapacity);
assert (0 <= iC.id && iC.id < m_nodeCapacity);
DynamicTreeNode B = iB;
DynamicTreeNode C = iC;
int balance = C.height - B.height;
// Rotate C up
if (balance > 1) {
DynamicTreeNode iF = C.child1;
DynamicTreeNode iG = C.child2;
DynamicTreeNode F = iF;
DynamicTreeNode G = iG;
assert (F != null);
assert (G != null);
assert (0 <= iF.id && iF.id < m_nodeCapacity);
assert (0 <= iG.id && iG.id < m_nodeCapacity);
// Swap A and C
C.child1 = iA;
C.parent = A.parent;
A.parent = iC;
// A's old parent should point to C
if (C.parent != null) {
if (C.parent.child1 == iA) {
C.parent.child1 = iC;
} else {
assert (C.parent.child2 == iA);
C.parent.child2 = iC;
}
} else {
m_root = iC;
}
// Rotate
if (F.height > G.height) {
C.child2 = iF;
A.child2 = iG;
G.parent = iA;
A.aabb.combine(B.aabb, G.aabb);
C.aabb.combine(A.aabb, F.aabb);
A.height = 1 + org.jbox2d.common.MathUtils.max(B.height, G.height);
C.height = 1 + org.jbox2d.common.MathUtils.max(A.height, F.height);
} else {
C.child2 = iG;
A.child2 = iF;
F.parent = iA;
A.aabb.combine(B.aabb, F.aabb);
C.aabb.combine(A.aabb, G.aabb);
A.height = 1 + org.jbox2d.common.MathUtils.max(B.height, F.height);
C.height = 1 + org.jbox2d.common.MathUtils.max(A.height, G.height);
}
return iC;
}
// Rotate B up
if (balance < -1) {
DynamicTreeNode iD = B.child1;
DynamicTreeNode iE = B.child2;
DynamicTreeNode D = iD;
DynamicTreeNode E = iE;
assert (0 <= iD.id && iD.id < m_nodeCapacity);
assert (0 <= iE.id && iE.id < m_nodeCapacity);
// Swap A and B
B.child1 = iA;
B.parent = A.parent;
A.parent = iB;
// A's old parent should point to B
if (B.parent != null) {
if (B.parent.child1 == iA) {
B.parent.child1 = iB;
} else {
assert (B.parent.child2 == iA);
B.parent.child2 = iB;
}
} else {
m_root = iB;
}
// Rotate
if (D.height > E.height) {
B.child2 = iD;
A.child1 = iE;
E.parent = iA;
A.aabb.combine(C.aabb, E.aabb);
B.aabb.combine(A.aabb, D.aabb);
A.height = 1 + org.jbox2d.common.MathUtils.max(C.height, E.height);
B.height = 1 + org.jbox2d.common.MathUtils.max(A.height, D.height);
} else {
B.child2 = iE;
A.child1 = iD;
D.parent = iA;
A.aabb.combine(C.aabb, D.aabb);
B.aabb.combine(A.aabb, E.aabb);
A.height = 1 + org.jbox2d.common.MathUtils.max(C.height, D.height);
B.height = 1 + org.jbox2d.common.MathUtils.max(A.height, E.height);
}
return iB;
}
return iA;
}
@Override
public boolean solvePositionConstraints(final org.jbox2d.dynamics.SolverData data) {
final Rot qA = pool.popRot();
final Rot qB = pool.popRot();
Vec2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
Vec2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
qA.set(aA);
qB.set(aB);
float angularError = 0.0f;
float positionError = 0.0f;
boolean fixedRotation = (m_invIA + m_invIB == 0.0f);
// Solve angular limit constraint.
if (m_enableLimit && m_limitState != LimitState.INACTIVE && fixedRotation == false) {
float angle = aB - aA - m_referenceAngle;
float limitImpulse = 0.0f;
if (m_limitState == LimitState.EQUAL) {
// Prevent large angular corrections
float C =
MathUtils.clamp(
angle - m_lowerAngle,
-Settings.maxAngularCorrection,
Settings.maxAngularCorrection);
limitImpulse = -m_motorMass * C;
angularError = MathUtils.abs(C);
} else if (m_limitState == LimitState.AT_LOWER) {
float C = angle - m_lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.clamp(C + Settings.angularSlop, -Settings.maxAngularCorrection, 0.0f);
limitImpulse = -m_motorMass * C;
} else if (m_limitState == LimitState.AT_UPPER) {
float C = angle - m_upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.clamp(C - Settings.angularSlop, 0.0f, Settings.maxAngularCorrection);
limitImpulse = -m_motorMass * C;
}
aA -= m_invIA * limitImpulse;
aB += m_invIB * limitImpulse;
}
// Solve point-to-point constraint.
{
qA.set(aA);
qB.set(aB);
final Vec2 rA = pool.popVec2();
final Vec2 rB = pool.popVec2();
final Vec2 C = pool.popVec2();
final Vec2 impulse = pool.popVec2();
Rot.mulToOutUnsafe(qA, C.set(m_localAnchorA).subLocal(m_localCenterA), rA);
Rot.mulToOutUnsafe(qB, C.set(m_localAnchorB).subLocal(m_localCenterB), rB);
C.set(cB).addLocal(rB).subLocal(cA).subLocal(rA);
positionError = C.length();
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
final org.jbox2d.common.Mat22 K = pool.popMat22();
K.ex.x = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y;
K.ex.y = -iA * rA.x * rA.y - iB * rB.x * rB.y;
K.ey.x = K.ex.y;
K.ey.y = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x;
K.solveToOut(C, impulse);
impulse.negateLocal();
cA.x -= mA * impulse.x;
cA.y -= mA * impulse.y;
aA -= iA * Vec2.cross(rA, impulse);
cB.x += mB * impulse.x;
cB.y += mB * impulse.y;
aB += iB * Vec2.cross(rB, impulse);
pool.pushVec2(4);
pool.pushMat22(1);
}
// data.positions[m_indexA].c.set(cA);
data.positions[m_indexA].a = aA;
// data.positions[m_indexB].c.set(cB);
data.positions[m_indexB].a = aB;
pool.pushRot(2);
return positionError <= Settings.linearSlop && angularError <= Settings.angularSlop;
}
/** Build an optimal tree. Very expensive. For testing. */
public void rebuildBottomUp() {
int[] nodes = new int[m_nodeCount];
int count = 0;
// Build array of leaves. Free the rest.
for (int i = 0; i < m_nodeCapacity; ++i) {
if (m_nodes[i].height < 0) {
// free node in pool
continue;
}
DynamicTreeNode node = m_nodes[i];
if (node.child1 == null) {
node.parent = null;
nodes[count] = i;
++count;
} else {
freeNode(node);
}
}
org.jbox2d.collision.AABB b = new org.jbox2d.collision.AABB();
while (count > 1) {
float minCost = Float.MAX_VALUE;
int iMin = -1, jMin = -1;
for (int i = 0; i < count; ++i) {
org.jbox2d.collision.AABB aabbi = m_nodes[nodes[i]].aabb;
for (int j = i + 1; j < count; ++j) {
org.jbox2d.collision.AABB aabbj = m_nodes[nodes[j]].aabb;
b.combine(aabbi, aabbj);
float cost = b.getPerimeter();
if (cost < minCost) {
iMin = i;
jMin = j;
minCost = cost;
}
}
}
int index1 = nodes[iMin];
int index2 = nodes[jMin];
DynamicTreeNode child1 = m_nodes[index1];
DynamicTreeNode child2 = m_nodes[index2];
DynamicTreeNode parent = allocateNode();
parent.child1 = child1;
parent.child2 = child2;
parent.height = 1 + org.jbox2d.common.MathUtils.max(child1.height, child2.height);
parent.aabb.combine(child1.aabb, child2.aabb);
parent.parent = null;
child1.parent = parent;
child2.parent = parent;
nodes[jMin] = nodes[count - 1];
nodes[iMin] = parent.id;
--count;
}
m_root = m_nodes[nodes[0]];
validate();
}
@Override
public void raycast(
org.jbox2d.callbacks.TreeRayCastCallback callback, org.jbox2d.collision.RayCastInput input) {
final Vec2 p1 = input.p1;
final Vec2 p2 = input.p2;
float p1x = p1.x, p2x = p2.x, p1y = p1.y, p2y = p2.y;
float vx, vy;
float rx, ry;
float absVx, absVy;
float cx, cy;
float hx, hy;
float tempx, tempy;
r.x = p2x - p1x;
r.y = p2y - p1y;
assert ((r.x * r.x + r.y * r.y) > 0f);
r.normalize();
rx = r.x;
ry = r.y;
// v is perpendicular to the segment.
vx = -1f * ry;
vy = 1f * rx;
absVx = org.jbox2d.common.MathUtils.abs(vx);
absVy = org.jbox2d.common.MathUtils.abs(vy);
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
float maxFraction = input.maxFraction;
// Build a bounding box for the segment.
final org.jbox2d.collision.AABB segAABB = aabb;
// Vec2 t = p1 + maxFraction * (p2 - p1);
// before inline
// temp.set(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1);
// Vec2.minToOut(p1, temp, segAABB.lowerBound);
// Vec2.maxToOut(p1, temp, segAABB.upperBound);
tempx = (p2x - p1x) * maxFraction + p1x;
tempy = (p2y - p1y) * maxFraction + p1y;
segAABB.lowerBound.x = p1x < tempx ? p1x : tempx;
segAABB.lowerBound.y = p1y < tempy ? p1y : tempy;
segAABB.upperBound.x = p1x > tempx ? p1x : tempx;
segAABB.upperBound.y = p1y > tempy ? p1y : tempy;
// end inline
nodeStackIndex = 0;
nodeStack[nodeStackIndex++] = m_root;
while (nodeStackIndex > 0) {
final DynamicTreeNode node = nodeStack[--nodeStackIndex];
if (node == null) {
continue;
}
final org.jbox2d.collision.AABB nodeAABB = node.aabb;
if (!org.jbox2d.collision.AABB.testOverlap(nodeAABB, segAABB)) {
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
// node.aabb.getCenterToOut(c);
// node.aabb.getExtentsToOut(h);
cx = (nodeAABB.lowerBound.x + nodeAABB.upperBound.x) * .5f;
cy = (nodeAABB.lowerBound.y + nodeAABB.upperBound.y) * .5f;
hx = (nodeAABB.upperBound.x - nodeAABB.lowerBound.x) * .5f;
hy = (nodeAABB.upperBound.y - nodeAABB.lowerBound.y) * .5f;
tempx = p1x - cx;
tempy = p1y - cy;
float separation =
org.jbox2d.common.MathUtils.abs(vx * tempx + vy * tempy) - (absVx * hx + absVy * hy);
if (separation > 0.0f) {
continue;
}
if (node.child1 == null) {
subInput.p1.x = p1x;
subInput.p1.y = p1y;
subInput.p2.x = p2x;
subInput.p2.y = p2y;
subInput.maxFraction = maxFraction;
float value = callback.raycastCallback(subInput, node.id);
if (value == 0.0f) {
// The client has terminated the ray cast.
return;
}
if (value > 0.0f) {
// Update segment bounding box.
maxFraction = value;
// temp.set(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1);
// Vec2.minToOut(p1, temp, segAABB.lowerBound);
// Vec2.maxToOut(p1, temp, segAABB.upperBound);
tempx = (p2x - p1x) * maxFraction + p1x;
tempy = (p2y - p1y) * maxFraction + p1y;
segAABB.lowerBound.x = p1x < tempx ? p1x : tempx;
segAABB.lowerBound.y = p1y < tempy ? p1y : tempy;
segAABB.upperBound.x = p1x > tempx ? p1x : tempx;
segAABB.upperBound.y = p1y > tempy ? p1y : tempy;
}
} else {
if (nodeStack.length - nodeStackIndex - 2 <= 0) {
DynamicTreeNode[] newBuffer = new DynamicTreeNode[nodeStack.length * 2];
System.arraycopy(nodeStack, 0, newBuffer, 0, nodeStack.length);
nodeStack = newBuffer;
}
nodeStack[nodeStackIndex++] = node.child1;
nodeStack[nodeStackIndex++] = node.child2;
}
}
}
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;
}
}
}
/**
* Compute the collision manifold between a polygon and a circle.
*
* @param manifold
* @param polygon
* @param xfA
* @param circle
* @param xfB
*/
public final void collidePolygonAndCircle(
Manifold manifold,
final PolygonShape polygon,
final Transform xfA,
final CircleShape circle,
final Transform xfB) {
manifold.pointCount = 0;
// Compute circle position in the frame of the polygon.
Transform.mulToOut(xfB, circle.m_p, c);
Transform.mulTransToOut(xfA, c, cLocal);
// Find the min separating edge.
int normalIndex = 0;
float separation = Float.MIN_VALUE;
float radius = polygon.m_radius + circle.m_radius;
int vertexCount = polygon.m_vertexCount;
Vec2[] vertices = polygon.m_vertices;
Vec2[] normals = polygon.m_normals;
for (int i = 0; i < vertexCount; i++) {
temp.set(cLocal).subLocal(vertices[i]);
float s = Vec2.dot(normals[i], temp);
if (s > radius) {
// early out
return;
}
if (s > separation) {
separation = s;
normalIndex = i;
}
}
// Vertices that subtend the incident face.
int vertIndex1 = normalIndex;
int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
Vec2 v1 = vertices[vertIndex1];
Vec2 v2 = vertices[vertIndex2];
// If the center is inside the polygon ...
if (separation < Settings.EPSILON) {
manifold.pointCount = 1;
manifold.type = ManifoldType.FACE_A;
manifold.localNormal.set(normals[normalIndex]);
manifold.localPoint.set(v1).addLocal(v2).mulLocal(.5f);
manifold.points[0].localPoint.set(circle.m_p);
manifold.points[0].id.zero();
return;
}
// Compute barycentric coordinates
temp.set(cLocal).subLocal(v1);
temp2.set(v2).subLocal(v1);
float u1 = Vec2.dot(temp, temp2);
temp.set(cLocal).subLocal(v2);
temp2.set(v1).subLocal(v2);
float u2 = Vec2.dot(temp, temp2);
if (u1 <= 0f) {
if (MathUtils.distanceSquared(cLocal, v1) > radius * radius) {
return;
}
manifold.pointCount = 1;
manifold.type = ManifoldType.FACE_A;
manifold.localNormal.set(cLocal).subLocal(v1);
manifold.localNormal.normalize();
manifold.localPoint.set(v1);
manifold.points[0].localPoint.set(circle.m_p);
manifold.points[0].id.zero();
} else if (u2 <= 0.0f) {
if (MathUtils.distanceSquared(cLocal, v2) > radius * radius) {
return;
}
manifold.pointCount = 1;
manifold.type = ManifoldType.FACE_A;
manifold.localNormal.set(cLocal).subLocal(v2);
manifold.localNormal.normalize();
manifold.localPoint.set(v2);
manifold.points[0].localPoint.set(circle.m_p);
manifold.points[0].id.zero();
} else {
// Vec2 faceCenter = 0.5f * (v1 + v2);
// (temp is faceCenter)
temp.set(v1).addLocal(v2).mulLocal(.5f);
temp2.set(cLocal).subLocal(temp);
separation = Vec2.dot(temp2, normals[vertIndex1]);
if (separation > radius) {
return;
}
manifold.pointCount = 1;
manifold.type = ManifoldType.FACE_A;
manifold.localNormal.set(normals[vertIndex1]);
manifold.localPoint.set(temp); // (faceCenter)
manifold.points[0].localPoint.set(circle.m_p);
manifold.points[0].id.zero();
}
}
private final void insertLeaf(int leaf_index) {
DynamicTreeNode leaf = m_nodes[leaf_index];
if (m_root == null) {
m_root = leaf;
m_root.parent = null;
return;
}
// find the best sibling
org.jbox2d.collision.AABB leafAABB = leaf.aabb;
DynamicTreeNode index = m_root;
while (index.child1 != null) {
final DynamicTreeNode node = index;
DynamicTreeNode child1 = node.child1;
DynamicTreeNode child2 = node.child2;
float area = node.aabb.getPerimeter();
combinedAABB.combine(node.aabb, leafAABB);
float combinedArea = combinedAABB.getPerimeter();
// Cost of creating a new parent for this node and the new leaf
float cost = 2.0f * combinedArea;
// Minimum cost of pushing the leaf further down the tree
float inheritanceCost = 2.0f * (combinedArea - area);
// Cost of descending into child1
float cost1;
if (child1.child1 == null) {
combinedAABB.combine(leafAABB, child1.aabb);
cost1 = combinedAABB.getPerimeter() + inheritanceCost;
} else {
combinedAABB.combine(leafAABB, child1.aabb);
float oldArea = child1.aabb.getPerimeter();
float newArea = combinedAABB.getPerimeter();
cost1 = (newArea - oldArea) + inheritanceCost;
}
// Cost of descending into child2
float cost2;
if (child2.child1 == null) {
combinedAABB.combine(leafAABB, child2.aabb);
cost2 = combinedAABB.getPerimeter() + inheritanceCost;
} else {
combinedAABB.combine(leafAABB, child2.aabb);
float oldArea = child2.aabb.getPerimeter();
float newArea = combinedAABB.getPerimeter();
cost2 = newArea - oldArea + inheritanceCost;
}
// Descend according to the minimum cost.
if (cost < cost1 && cost < cost2) {
break;
}
// Descend
if (cost1 < cost2) {
index = child1;
} else {
index = child2;
}
}
DynamicTreeNode sibling = index;
DynamicTreeNode oldParent = m_nodes[sibling.id].parent;
final DynamicTreeNode newParent = allocateNode();
newParent.parent = oldParent;
newParent.userData = null;
newParent.aabb.combine(leafAABB, sibling.aabb);
newParent.height = sibling.height + 1;
if (oldParent != null) {
// The sibling was not the root.
if (oldParent.child1 == sibling) {
oldParent.child1 = newParent;
} else {
oldParent.child2 = newParent;
}
newParent.child1 = sibling;
newParent.child2 = leaf;
sibling.parent = newParent;
leaf.parent = newParent;
} else {
// The sibling was the root.
newParent.child1 = sibling;
newParent.child2 = leaf;
sibling.parent = newParent;
leaf.parent = newParent;
m_root = newParent;
}
// Walk back up the tree fixing heights and AABBs
index = leaf.parent;
while (index != null) {
index = balance(index);
DynamicTreeNode child1 = index.child1;
DynamicTreeNode child2 = index.child2;
assert (child1 != null);
assert (child2 != null);
index.height = 1 + org.jbox2d.common.MathUtils.max(child1.height, child2.height);
index.aabb.combine(child1.aabb, child2.aabb);
index = index.parent;
}
// validate();
}
@Override
public void solveVelocityConstraints(final org.jbox2d.dynamics.SolverData data) {
Vec2 vA = data.velocities[m_indexA].v;
float wA = data.velocities[m_indexA].w;
Vec2 vB = data.velocities[m_indexB].v;
float wB = data.velocities[m_indexB].w;
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
boolean fixedRotation = (iA + iB == 0.0f);
// Solve motor constraint.
if (m_enableMotor && m_limitState != LimitState.EQUAL && fixedRotation == false) {
float Cdot = wB - wA - m_motorSpeed;
float impulse = -m_motorMass * Cdot;
float oldImpulse = m_motorImpulse;
float maxImpulse = data.step.dt * m_maxMotorTorque;
m_motorImpulse = MathUtils.clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_motorImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
final Vec2 temp = pool.popVec2();
// Solve limit constraint.
if (m_enableLimit && m_limitState != LimitState.INACTIVE && fixedRotation == false) {
final Vec2 Cdot1 = pool.popVec2();
final Vec3 Cdot = pool.popVec3();
// Solve point-to-point constraint
Vec2.crossToOutUnsafe(wA, m_rA, temp);
Vec2.crossToOutUnsafe(wB, m_rB, Cdot1);
Cdot1.addLocal(vB).subLocal(vA).subLocal(temp);
float Cdot2 = wB - wA;
Cdot.set(Cdot1.x, Cdot1.y, Cdot2);
Vec3 impulse = pool.popVec3();
m_mass.solve33ToOut(Cdot, impulse);
impulse.negateLocal();
if (m_limitState == LimitState.EQUAL) {
m_impulse.addLocal(impulse);
} else if (m_limitState == LimitState.AT_LOWER) {
float newImpulse = m_impulse.z + impulse.z;
if (newImpulse < 0.0f) {
final Vec2 rhs = pool.popVec2();
rhs.set(m_mass.ez.x, m_mass.ez.y).mulLocal(m_impulse.z).subLocal(Cdot1);
m_mass.solve22ToOut(rhs, temp);
impulse.x = temp.x;
impulse.y = temp.y;
impulse.z = -m_impulse.z;
m_impulse.x += temp.x;
m_impulse.y += temp.y;
m_impulse.z = 0.0f;
pool.pushVec2(1);
} else {
m_impulse.addLocal(impulse);
}
} else if (m_limitState == LimitState.AT_UPPER) {
float newImpulse = m_impulse.z + impulse.z;
if (newImpulse > 0.0f) {
final Vec2 rhs = pool.popVec2();
rhs.set(m_mass.ez.x, m_mass.ez.y).mulLocal(m_impulse.z).subLocal(Cdot1);
m_mass.solve22ToOut(rhs, temp);
impulse.x = temp.x;
impulse.y = temp.y;
impulse.z = -m_impulse.z;
m_impulse.x += temp.x;
m_impulse.y += temp.y;
m_impulse.z = 0.0f;
pool.pushVec2(1);
} else {
m_impulse.addLocal(impulse);
}
}
final Vec2 P = pool.popVec2();
P.set(impulse.x, impulse.y);
vA.x -= mA * P.x;
vA.y -= mA * P.y;
wA -= iA * (Vec2.cross(m_rA, P) + impulse.z);
vB.x += mB * P.x;
vB.y += mB * P.y;
wB += iB * (Vec2.cross(m_rB, P) + impulse.z);
pool.pushVec2(2);
pool.pushVec3(2);
} else {
// Solve point-to-point constraint
Vec2 Cdot = pool.popVec2();
Vec2 impulse = pool.popVec2();
Vec2.crossToOutUnsafe(wA, m_rA, temp);
Vec2.crossToOutUnsafe(wB, m_rB, Cdot);
Cdot.addLocal(vB).subLocal(vA).subLocal(temp);
m_mass.solve22ToOut(Cdot.negateLocal(), impulse); // just leave negated
m_impulse.x += impulse.x;
m_impulse.y += impulse.y;
vA.x -= mA * impulse.x;
vA.y -= mA * impulse.y;
wA -= iA * Vec2.cross(m_rA, impulse);
vB.x += mB * impulse.x;
vB.y += mB * impulse.y;
wB += iB * Vec2.cross(m_rB, impulse);
pool.pushVec2(2);
}
// data.velocities[m_indexA].v.set(vA);
data.velocities[m_indexA].w = wA;
// data.velocities[m_indexB].v.set(vB);
data.velocities[m_indexB].w = wB;
pool.pushVec2(1);
}