@Override
public final void computeAABB(final AABB aabb, final Transform xf, int childIndex) {
final Vec2 v = pool1;
final Vec2 lower = aabb.lowerBound;
final Vec2 upper = aabb.upperBound;
final Vec2 v1 = m_vertices[0];
lower.x = (xf.q.c * v1.x - xf.q.s * v1.y) + xf.p.x;
lower.y = (xf.q.s * v1.x + xf.q.c * v1.y) + xf.p.y;
upper.set(lower);
for (int i = 1; i < m_count; ++i) {
Vec2 v2 = m_vertices[i];
v.x = (xf.q.c * v2.x - xf.q.s * v2.y) + xf.p.x;
v.y = (xf.q.s * v2.x + xf.q.c * v2.y) + xf.p.y;
// Vec2 v = Mul(xf, m_vertices[i]);
Vec2.minToOut(lower, v, lower);
Vec2.maxToOut(upper, v, upper);
}
// Vec2 r(m_radius, m_radius);
// aabb.lowerBound = lower - r;
// aabb.upperBound = upper + r;
aabb.lowerBound.x -= m_radius;
aabb.lowerBound.y -= m_radius;
aabb.upperBound.x += m_radius;
aabb.upperBound.y += m_radius;
}
/**
* Internal method
*
* @param broadPhase
* @param xf1
* @param xf2
*/
protected void synchronize(
BroadPhase broadPhase, final Transform transform1, final Transform transform2) {
if (m_proxyCount == 0) {
return;
}
for (int i = 0; i < m_proxyCount; ++i) {
FixtureProxy proxy = m_proxies[i];
// Compute an AABB that covers the swept shape (may miss some rotation effect).
final AABB aabb1 = pool1;
final AABB aab = pool2;
m_shape.computeAABB(aabb1, transform1, proxy.childIndex);
m_shape.computeAABB(aab, transform2, proxy.childIndex);
proxy.aabb.lowerBound.x =
aabb1.lowerBound.x < aab.lowerBound.x ? aabb1.lowerBound.x : aab.lowerBound.x;
proxy.aabb.lowerBound.y =
aabb1.lowerBound.y < aab.lowerBound.y ? aabb1.lowerBound.y : aab.lowerBound.y;
proxy.aabb.upperBound.x =
aabb1.upperBound.x > aab.upperBound.x ? aabb1.upperBound.x : aab.upperBound.x;
proxy.aabb.upperBound.y =
aabb1.upperBound.y > aab.upperBound.y ? aabb1.upperBound.y : aab.upperBound.y;
displacement.x = transform2.p.x - transform1.p.x;
displacement.y = transform2.p.y - transform1.p.y;
broadPhase.moveProxy(proxy.proxyId, proxy.aabb, displacement);
}
}
public void jump() {
Vec2 linearVelocity = getBody().getLinearVelocity();
linearVelocity.y = JUMP_SPEED;
getBody().setLinearVelocity(linearVelocity);
SOUND_JUMP.play();
}
/**
* Set the linear velocity of this body
*
* @param xVelocity The x component of the velocity
* @param yVelocity The y component of the velocity
*/
public void setVelocity(float xVelocity, float yVelocity) {
checkBody();
Vec2 vel = jboxBody.getLinearVelocity();
vel.x = xVelocity;
vel.y = yVelocity;
jboxBody.setLinearVelocity(vel);
}
@Override
public final boolean moveProxy(
int proxyId, final org.jbox2d.collision.AABB aabb, Vec2 displacement) {
assert (aabb.isValid());
assert (0 <= proxyId && proxyId < m_nodeCapacity);
final DynamicTreeNode node = m_nodes[proxyId];
assert (node.child1 == null);
final org.jbox2d.collision.AABB nodeAABB = node.aabb;
// if (nodeAABB.contains(aabb)) {
if (nodeAABB.lowerBound.x <= aabb.lowerBound.x
&& nodeAABB.lowerBound.y <= aabb.lowerBound.y
&& aabb.upperBound.x <= nodeAABB.upperBound.x
&& aabb.upperBound.y <= nodeAABB.upperBound.y) {
return false;
}
removeLeaf(node);
// Extend AABB
final Vec2 lowerBound = nodeAABB.lowerBound;
final Vec2 upperBound = nodeAABB.upperBound;
lowerBound.x = aabb.lowerBound.x - Settings.aabbExtension;
lowerBound.y = aabb.lowerBound.y - Settings.aabbExtension;
upperBound.x = aabb.upperBound.x + Settings.aabbExtension;
upperBound.y = aabb.upperBound.y + Settings.aabbExtension;
// Predict AABB displacement.
final float dx = displacement.x * Settings.aabbMultiplier;
final float dy = displacement.y * Settings.aabbMultiplier;
if (dx < 0.0f) {
lowerBound.x += dx;
} else {
upperBound.x += dx;
}
if (dy < 0.0f) {
lowerBound.y += dy;
} else {
upperBound.y += dy;
}
insertLeaf(proxyId);
return true;
}
/**
* Get a direction vector from start point, within screens height and width
*
* @return
*/
public Vec2 getStartDirVec(float screenWidth, float screenHeight) {
Vec2 start = this.getBody().getWorldCenter().mul(Globals.PHYS_RATIO);
Vec2 dir = new Vec2();
// Find where the screen is, so Vec can go through it
if (start.x <= -screenWidth) {
dir.x = 1;
} else if (start.x >= screenWidth) {
dir.x = -1;
}
if (start.y <= -screenHeight) {
dir.y = 1;
} else if (start.y >= screenHeight) {
dir.y = -1;
}
return dir;
}
@Override
public void onCreate(Bundle savedInstanceState) {
super.onCreate(savedInstanceState);
view = new TestView(this);
setContentView(view);
// new World(new Vec2(0.0f, -10.0f), true);
gravity = getPreferences(Context.MODE_PRIVATE).getInt(PREF_GRAVITY, GRAVITY_ACCELEROMETER);
w = (PhysicsWorld) getLastNonConfigurationInstance();
if (w == null) {
Vec2 grav = new Vec2(0.0f, 0.0f);
if (gravity == GRAVITY_NORMAL) {
grav.y = -10.0f;
} else if (gravity == GRAVITY_MOON) {
grav.y = -1.67f;
}
w = new PhysicsWorld();
w.create(grav);
}
view.setModel(w);
if (savedInstanceState != null) {
defaultLandscape = savedInstanceState.getBoolean(KEY_DEFAULT_LANDSCAPE);
} else {
defaultLandscape =
getResources().getConfiguration().orientation == Configuration.ORIENTATION_LANDSCAPE;
rotation = getPreferences(Context.MODE_PRIVATE).getInt(PREF_ROTATION, Surface.ROTATION_0);
int orientation;
if (defaultLandscape && rotation == Surface.ROTATION_90
|| !defaultLandscape && rotation == Surface.ROTATION_180) {
orientation = 9;
} else if (defaultLandscape && rotation == Surface.ROTATION_180
|| !defaultLandscape && rotation == Surface.ROTATION_270) {
orientation = 8;
} else if (defaultLandscape && rotation == Surface.ROTATION_270
|| !defaultLandscape && rotation == Surface.ROTATION_0) {
orientation = ActivityInfo.SCREEN_ORIENTATION_PORTRAIT;
} else {
orientation = ActivityInfo.SCREEN_ORIENTATION_LANDSCAPE;
}
setRequestedOrientation(orientation);
}
}
@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 float computeSubmergedArea(final Vec2 normal, float offset, XForm xf, Vec2 c) {
final Vec2 v0 = tlV0.get();
final Vec2 v1 = tlV1.get();
final Vec2 v2 = tlV2.get();
final Vec2 temp = tlTemp.get();
// Note that v0 is independent of any details of the specific edge
// We are relying on v0 being consistent between multiple edges of the same body
v0.set(normal).mul(offset);
// b2Vec2 v0 = xf.position + (offset - b2Dot(normal, xf.position)) * normal;
XForm.mulToOut(xf, m_v1, v1);
XForm.mulToOut(xf, m_v2, v2);
float d1 = Vec2.dot(normal, v1) - offset;
float d2 = Vec2.dot(normal, v2) - offset;
if (d1 > 0.0f) {
if (d2 > 0.0f) {
return 0.0f;
} else {
temp.set(v2).mulLocal(d1 / (d1 - d2));
v1.mulLocal(-d2 / (d1 - d2)).addLocal(temp);
}
} else {
if (d2 > 0.0f) {
temp.set(v1).mulLocal(-d2 / (d1 - d2));
v2.mulLocal(d1 / (d1 - d2)).addLocal(temp);
} else {
// Nothing
}
}
final Vec2 e1 = tlE1.get();
final Vec2 e2 = tlE2.get();
// v0,v1,v2 represents a fully submerged triangle
float k_inv3 = 1.0f / 3.0f;
// Area weighted centroid
c.x = k_inv3 * (v0.x + v1.x + v2.x);
c.y = k_inv3 * (v0.y + v1.y + v2.y);
e1.set(v1).subLocal(v0);
e2.set(v2).subLocal(v0);
return 0.5f * Vec2.cross(e1, e2);
}
/**
* Apply a force at a world point. If the force is not applied at the center of mass, it will
* generate a torque and affect the angular velocity. This wakes up the body.
*
* @param force the world force vector, usually in Newtons (N).
* @param point the world position of the point of application.
*/
public final void applyForce(Vec2 force, Vec2 point) {
if (m_type != BodyType.DYNAMIC) {
return;
}
if (isAwake() == false) {
setAwake(true);
}
// m_force.addLocal(force);
// Vec2 temp = tltemp.get();
// temp.set(point).subLocal(m_sweep.c);
// m_torque += Vec2.cross(temp, force);
m_force.x += force.x;
m_force.y += force.y;
m_torque += (point.x - m_sweep.c.x) * force.y - (point.y - m_sweep.c.y) * force.x;
}
/**
* Apply an impulse at a point. This immediately modifies the velocity. It also modifies the
* angular velocity if the point of application is not at the center of mass. This wakes up the
* body.
*
* @param impulse the world impulse vector, usually in N-seconds or kg-m/s.
* @param point the world position of the point of application.
*/
public final void applyLinearImpulse(Vec2 impulse, Vec2 point) {
if (m_type != BodyType.DYNAMIC) {
return;
}
if (isAwake() == false) {
setAwake(true);
}
// Vec2 temp = tltemp.get();
// temp.set(impulse).mulLocal(m_invMass);
// m_linearVelocity.addLocal(temp);
//
// temp.set(point).subLocal(m_sweep.c);
// m_angularVelocity += m_invI * Vec2.cross(temp, impulse);
m_linearVelocity.x += impulse.x * m_invMass;
m_linearVelocity.y += impulse.y * m_invMass;
m_angularVelocity +=
m_invI * ((point.x - m_sweep.c.x) * impulse.y - (point.y - m_sweep.c.y) * impulse.x);
}
public PhysicsObject(float x, float y, int w, int h, BodyType t) {
pos.x = x;
pos.y = y;
height = h;
width = w;
shape = new PolygonShape();
shape.setAsBox(
(width / 2) / PhysicsInvaders.PTM_RATIO, (height / 2) / PhysicsInvaders.PTM_RATIO);
FixtureDef fd = new FixtureDef();
fd.shape = shape;
fd.density = 100;
fd.friction = 0.9f;
fd.restitution = 0.1f;
BodyDef bodyDef = new BodyDef();
bodyDef.position.set(pos.x / PhysicsInvaders.PTM_RATIO, pos.y / PhysicsInvaders.PTM_RATIO);
bodyDef.type = t;
body = PhysicsInvaders.world.createBody(bodyDef);
body.createFixture(fd);
}
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;
}
}
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));
}
@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;
}
}
}
@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;
}
@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);
}
@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);
}
