public void setPrevEdge(
final EdgeShape edge, final Vec2 core, final Vec2 cornerDir, final boolean convex) {
m_prevEdge = edge;
m_coreV1.set(core);
m_cornerDir1.set(cornerDir);
m_cornerConvex1 = convex;
}
@Override
public final boolean testPoint(final Transform xf, final Vec2 p) {
final Vec2 pLocal = pool1;
final Vec2 temp = pool2;
pLocal.set(p).subLocal(xf.p);
Rot.mulTransUnsafe(xf.q, pLocal, temp);
pLocal.set(temp);
if (m_debug) {
System.out.println("--testPoint debug--");
System.out.println("Vertices: ");
for (int i = 0; i < m_count; ++i) {
System.out.println(m_vertices[i]);
}
System.out.println("pLocal: " + pLocal);
}
for (int i = 0; i < m_count; ++i) {
temp.set(pLocal).subLocal(m_vertices[i]);
final float dot = Vec2.dot(m_normals[i], temp);
if (dot > 0.0f) {
return false;
}
}
return true;
}
public void setNextEdge(
final EdgeShape edge, final Vec2 core, final Vec2 cornerDir, final boolean convex) {
// djm note: the vec2s are probably pooled, don't use them
m_nextEdge = edge;
m_coreV2.set(core);
m_cornerDir2.set(cornerDir);
m_cornerConvex2 = convex;
}
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);
}
/**
* Construct a world object.
*
* @param gravity the world gravity vector.
* @param doSleep improve performance by not simulating inactive bodies.
*/
public World(Vec2 gravity, IWorldPool argPool) {
pool = argPool;
m_destructionListener = null;
m_debugDraw = null;
m_bodyList = null;
m_jointList = null;
m_bodyCount = 0;
m_jointCount = 0;
m_warmStarting = true;
m_continuousPhysics = true;
m_subStepping = false;
m_stepComplete = true;
m_allowSleep = true;
m_gravity.set(gravity);
m_flags = CLEAR_FORCES;
m_inv_dt0 = 0f;
m_contactManager = new ContactManager(this);
m_profile = new Profile();
initializeRegisters();
}
protected RevoluteJoint(IWorldPool argWorld, RevoluteJointDef def) {
super(argWorld, def);
m_localAnchorA.set(def.localAnchorA);
m_localAnchorB.set(def.localAnchorB);
m_referenceAngle = def.referenceAngle;
m_motorImpulse = 0;
m_lowerAngle = def.lowerAngle;
m_upperAngle = def.upperAngle;
m_maxMotorTorque = def.maxMotorTorque;
m_motorSpeed = def.motorSpeed;
m_enableLimit = def.enableLimit;
m_enableMotor = def.enableMotor;
m_limitState = LimitState.INACTIVE;
}
@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;
}
public Vec2
getProjectileSpawnPoint() { // TODO move to an interface like IProjectileParent or something
mProjectileSpawnPoint.set(
mPhysics.getX() + 3,
mPhysics.getY()); // TODO change arguments to mPhysics.getProjectileSpawn
return mProjectileSpawnPoint;
}
/**
* Compute the collision manifold between two circles.
*
* @param manifold
* @param circle1
* @param xfA
* @param circle2
* @param xfB
*/
public final void collideCircles(
Manifold manifold,
final CircleShape circle1,
final Transform xfA,
final CircleShape circle2,
final Transform xfB) {
manifold.pointCount = 0;
Transform.mulToOut(xfA, circle1.m_p, pA);
Transform.mulToOut(xfB, circle2.m_p, pB);
d.set(pB).subLocal(pA);
float distSqr = Vec2.dot(d, d);
float radius = circle1.m_radius + circle2.m_radius;
if (distSqr > radius * radius) {
return;
}
manifold.type = ManifoldType.CIRCLES;
manifold.localPoint.set(circle1.m_p);
manifold.localNormal.setZero();
manifold.pointCount = 1;
manifold.points[0].localPoint.set(circle2.m_p);
manifold.points[0].id.zero();
}
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;
}
/** @see SupportsGenericDistance#support(Vec2, XForm, Vec2) */
public void support(final Vec2 dest, final XForm xf, final Vec2 d) {
final Vec2 supportV1 = tlSupportV1.get();
final Vec2 supportV2 = tlSupportV2.get();
XForm.mulToOut(xf, m_coreV1, supportV1);
XForm.mulToOut(xf, m_coreV2, supportV2);
dest.set(Vec2.dot(supportV1, d) > Vec2.dot(supportV2, d) ? supportV1 : supportV2);
}
/**
* Set the mass properties to override the mass properties of the fixtures. Note that this changes
* the center of mass position. Note that creating or destroying fixtures can also alter the mass.
* This function has no effect if the body isn't dynamic.
*
* @param massData the mass properties.
*/
public final void setMassData(MassData massData) {
// TODO_ERIN adjust linear velocity and torque to account for movement of center.
assert (m_world.isLocked() == false);
if (m_world.isLocked() == true) {
return;
}
if (m_type != BodyType.DYNAMIC) {
return;
}
m_invMass = 0.0f;
m_I = 0.0f;
m_invI = 0.0f;
m_mass = massData.mass;
if (m_mass <= 0.0f) {
m_mass = 1f;
}
m_invMass = 1.0f / m_mass;
if (massData.I > 0.0f && (m_flags & e_fixedRotationFlag) == 0) {
m_I = massData.I - m_mass * Vec2.dot(massData.center, massData.center);
assert (m_I > 0.0f);
m_invI = 1.0f / m_I;
}
final Vec2 oldCenter = m_world.getPool().popVec2();
// Move center of mass.
oldCenter.set(m_sweep.c);
m_sweep.localCenter.set(massData.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);
final Vec2 temp = m_world.getPool().popVec2();
temp.set(m_sweep.c).subLocal(oldCenter);
Vec2.crossToOut(m_angularVelocity, temp, temp);
m_linearVelocity.addLocal(temp);
m_world.getPool().pushVec2(2);
}
/**
* Set the linear velocity of the center of mass.
*
* @param v the new linear velocity of the center of mass.
*/
public final void setLinearVelocity(Vec2 v) {
if (m_type == BodyType.STATIC) {
return;
}
if (Vec2.dot(v, v) > 0.0f) {
setAwake(true);
}
m_linearVelocity.set(v);
}
/**
* Set this as a single edge.
*
* @param v1
* @param v2
* @deprecated
*/
public final void setAsEdge(final Vec2 v1, final Vec2 v2) {
m_count = 2;
m_vertices[0].set(v1);
m_vertices[1].set(v2);
m_centroid.set(v1).addLocal(v2).mulLocal(0.5f);
// = 0.5f * (v1 + v2);
m_normals[0].set(v2).subLocal(v1);
Vec2.crossToOut(m_normals[0], 1f, m_normals[0]);
// m_normals[0] = Cross(v2 - v1, 1.0f);
m_normals[0].normalize();
m_normals[1].set(m_normals[0]).negateLocal();
}
@Override
public void move(Body body) {
Objects.requireNonNull(body);
Vec2 v2 = new Vec2(2f, 0f);
switch (nbCall) {
case -1:
nbCall++;
break;
case 0:
if (body.getPosition().y * EscapeWorld.SCALE > (FrontApplication.WIDTH / 3 - 120)) {
return;
}
v2.set(2f, 0f);
break;
case 1:
if (body.getPosition().x * EscapeWorld.SCALE < (FrontApplication.WIDTH / 3)) {
return;
}
v2.set(0f, 2f);
break;
case 2:
if (body.getPosition().y * EscapeWorld.SCALE < (FrontApplication.WIDTH / 3)) {
return;
}
v2.set(-2f, 0f);
break;
case 3:
if (body.getPosition().x * EscapeWorld.SCALE > (FrontApplication.WIDTH / 3 - 120)) {
return;
}
v2.set(0f, -2f);
break;
default:
throw new AssertionError();
}
body.setLinearVelocity(v2);
nbCall = (nbCall + 1) % 4;
}
/** @see Shape#testSegment(XForm, RaycastResult, Segment, float) */
@Override
public SegmentCollide testSegment(
final XForm xf, final RaycastResult out, final Segment segment, final float maxLambda) {
final Vec2 r = tlR.get();
final Vec2 v1 = tlV1.get();
final Vec2 d = tlD.get();
final Vec2 n = tlN.get();
final Vec2 b = tlB.get();
r.set(segment.p2).subLocal(segment.p1);
XForm.mulToOut(xf, m_v1, v1);
XForm.mulToOut(xf, m_v2, d);
d.subLocal(v1);
Vec2.crossToOut(d, 1.0f, n);
final float k_slop = 100.0f * Settings.EPSILON;
final float denom = -Vec2.dot(r, n);
// Cull back facing collision and ignore parallel segments.
if (denom > k_slop) {
// Does the segment intersect the infinite line associated with this segment?
b.set(segment.p1).subLocal(v1);
float a = Vec2.dot(b, n);
if (0.0f <= a && a <= maxLambda * denom) {
final float mu2 = -r.x * b.y + r.y * b.x;
// Does the segment intersect this segment?
if (-k_slop * denom <= mu2 && mu2 <= denom * (1.0f + k_slop)) {
a /= denom;
n.normalize();
out.lambda = a;
out.normal.set(n);
return SegmentCollide.HIT_COLLIDE;
}
}
}
return SegmentCollide.MISS_COLLIDE;
}
/**
* Validate convexity. This is a very time consuming operation.
*
* @return
*/
public boolean validate() {
for (int i = 0; i < m_count; ++i) {
int i1 = i;
int i2 = i < m_count - 1 ? i1 + 1 : 0;
Vec2 p = m_vertices[i1];
Vec2 e = pool1.set(m_vertices[i2]).subLocal(p);
for (int j = 0; j < m_count; ++j) {
if (j == i1 || j == i2) {
continue;
}
Vec2 v = pool2.set(m_vertices[j]).subLocal(p);
float c = Vec2.cross(e, v);
if (c < 0.0f) {
return false;
}
}
}
return true;
}
public final void computeCentroidToOut(final Vec2[] vs, final int count, final Vec2 out) {
assert (count >= 3);
out.set(0.0f, 0.0f);
float area = 0.0f;
// pRef is the reference point for forming triangles.
// It's location doesn't change the result (except for rounding error).
final Vec2 pRef = pool1;
pRef.setZero();
final Vec2 e1 = pool2;
final Vec2 e2 = pool3;
final float inv3 = 1.0f / 3.0f;
for (int i = 0; i < count; ++i) {
// Triangle vertices.
final Vec2 p1 = pRef;
final Vec2 p2 = vs[i];
final Vec2 p3 = i + 1 < count ? vs[i + 1] : vs[0];
e1.set(p2).subLocal(p1);
e2.set(p3).subLocal(p1);
final float D = Vec2.cross(e1, e2);
final float triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
e1.set(p1).addLocal(p2).addLocal(p3).mulLocal(triangleArea * inv3);
out.addLocal(e1);
}
// Centroid
assert (area > Settings.EPSILON);
out.mulLocal(1.0f / area);
}
protected MouseJoint(IWorldPool argWorld, MouseJointDef def) {
super(argWorld, def);
assert (def.target.isValid());
assert (def.maxForce >= 0);
assert (def.frequencyHz >= 0);
assert (def.dampingRatio >= 0);
m_target.set(def.target);
Transform.mulTransToOut(m_bodyB.getTransform(), m_target, m_localAnchor);
m_maxForce = def.maxForce;
m_impulse.setZero();
m_frequencyHz = def.frequencyHz;
m_dampingRatio = def.dampingRatio;
m_beta = 0;
m_gamma = 0;
}
/**
* Build vertices to represent an oriented box.
*
* @param hx the half-width.
* @param hy the half-height.
* @param center the center of the box in local coordinates.
* @param angle the rotation of the box in local coordinates.
*/
public final void setAsBox(final float hx, final float hy, final Vec2 center, final float angle) {
m_count = 4;
m_vertices[0].set(-hx, -hy);
m_vertices[1].set(hx, -hy);
m_vertices[2].set(hx, hy);
m_vertices[3].set(-hx, hy);
m_normals[0].set(0.0f, -1.0f);
m_normals[1].set(1.0f, 0.0f);
m_normals[2].set(0.0f, 1.0f);
m_normals[3].set(-1.0f, 0.0f);
m_centroid.set(center);
final Transform xf = poolt1;
xf.p.set(center);
xf.q.set(angle);
// Transform vertices and normals.
for (int i = 0; i < m_count; ++i) {
Transform.mulToOut(xf, m_vertices[i], m_vertices[i]);
Rot.mulToOut(xf.q, m_normals[i], m_normals[i]);
}
}
@Override
public void solveVelocityConstraints(TimeStep step) {
Body b = m_bodyB;
Vec2 r = pool.popVec2();
r.set(m_localAnchor).subLocal(b.getLocalCenter());
Mat22.mulToOut(b.getTransform().R, r, r);
// Cdot = v + cross(w, r)
Vec2 Cdot = pool.popVec2();
Vec2.crossToOut(b.m_angularVelocity, r, Cdot);
Cdot.addLocal(b.m_linearVelocity);
Vec2 impulse = pool.popVec2();
Vec2 temp = pool.popVec2();
// Mul(m_mass, -(Cdot + m_beta * m_C + m_gamma * m_impulse));
impulse.set(m_C).mulLocal(m_beta);
temp.set(m_impulse).mulLocal(m_gamma);
temp.addLocal(impulse).addLocal(Cdot).mulLocal(-1);
Mat22.mulToOut(m_mass, temp, impulse);
Vec2 oldImpulse = temp;
oldImpulse.set(m_impulse);
m_impulse.addLocal(impulse);
float maxImpulse = step.dt * m_maxForce;
if (m_impulse.lengthSquared() > maxImpulse * maxImpulse) {
m_impulse.mulLocal(maxImpulse / m_impulse.length());
}
impulse.set(m_impulse).subLocal(oldImpulse);
// pooling
oldImpulse.set(impulse).mulLocal(b.m_invMass);
b.m_linearVelocity.addLocal(oldImpulse);
b.m_angularVelocity += b.m_invI * Vec2.cross(r, impulse);
pool.pushVec2(4);
}
/**
* 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();
}
}
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));
}
/**
* Change the global gravity vector.
*
* @param gravity
*/
public void setGravity(Vec2 gravity) {
m_gravity.set(gravity);
}
@Override
public void getReactionForce(float inv_dt, Vec2 argOut) {
argOut.set(m_impulse.x, m_impulse.y).mulLocal(inv_dt);
}
/**
* 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);
}
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;
}
}
/**
* Compute the collision manifold between two polygons.
*
* @param manifold
* @param polygon1
* @param xf1
* @param polygon2
* @param xf2
*/
public final void collidePolygons(
Manifold manifold,
final PolygonShape polyA,
final Transform xfA,
final PolygonShape polyB,
final Transform xfB) {
// Find edge normal of max separation on A - return if separating axis is found
// Find edge normal of max separation on B - return if separation axis is found
// Choose reference edge as min(minA, minB)
// Find incident edge
// Clip
// The normal points from 1 to 2
manifold.pointCount = 0;
float totalRadius = polyA.m_radius + polyB.m_radius;
findMaxSeparation(results1, polyA, xfA, polyB, xfB);
if (results1.separation > totalRadius) {
return;
}
findMaxSeparation(results2, polyB, xfB, polyA, xfA);
if (results2.separation > totalRadius) {
return;
}
final PolygonShape poly1; // reference polygon
final PolygonShape poly2; // incident polygon
Transform xf1, xf2;
int edge1; // reference edge
int flip;
final float k_relativeTol = 0.98f;
final float k_absoluteTol = 0.001f;
if (results2.separation > k_relativeTol * results1.separation + k_absoluteTol) {
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = results2.edgeIndex;
manifold.type = ManifoldType.FACE_B;
flip = 1;
} else {
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = results1.edgeIndex;
manifold.type = ManifoldType.FACE_A;
flip = 0;
}
findIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
int count1 = poly1.m_vertexCount;
final Vec2[] vertices1 = poly1.m_vertices;
v11.set(vertices1[edge1]);
v12.set(edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0]);
localTangent.set(v12).subLocal(v11);
localTangent.normalize();
Vec2.crossToOut(localTangent, 1f, localNormal); // Vec2 localNormal = Cross(dv,
// 1.0f);
planePoint.set(v11).addLocal(v12).mulLocal(.5f); // Vec2 planePoint = 0.5f * (v11
// + v12);
Mat22.mulToOut(xf1.R, localTangent, tangent); // Vec2 sideNormal = Mul(xf1.R, v12
// - v11);
Vec2.crossToOut(tangent, 1f, normal); // Vec2 frontNormal = Cross(sideNormal,
// 1.0f);
Transform.mulToOut(xf1, v11, v11);
Transform.mulToOut(xf1, v12, v12);
// v11 = Mul(xf1, v11);
// v12 = Mul(xf1, v12);
// Face offset
float frontOffset = Vec2.dot(normal, v11);
// Side offsets, extended by polytope skin thickness.
float sideOffset1 = -Vec2.dot(tangent, v11) + totalRadius;
float sideOffset2 = Vec2.dot(tangent, v12) + totalRadius;
// Clip incident edge against extruded edge1 side edges.
// ClipVertex clipPoints1[2];
// ClipVertex clipPoints2[2];
int np;
// Clip to box side 1
// np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1);
tangent.negateLocal();
np = clipSegmentToLine(clipPoints1, incidentEdge, tangent, sideOffset1);
tangent.negateLocal();
if (np < 2) {
return;
}
// Clip to negative box side 1
np = clipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2);
if (np < 2) {
return;
}
// Now clipPoints2 contains the clipped points.
manifold.localNormal.set(localNormal);
manifold.localPoint.set(planePoint);
int pointCount = 0;
for (int i = 0; i < Settings.maxManifoldPoints; ++i) {
float separation = Vec2.dot(normal, clipPoints2[i].v) - frontOffset;
if (separation <= totalRadius) {
ManifoldPoint cp = manifold.points[pointCount];
Transform.mulTransToOut(xf2, clipPoints2[i].v, cp.localPoint);
// cp.m_localPoint = MulT(xf2, clipPoints2[i].v);
cp.id.set(clipPoints2[i].id);
cp.id.features.flip = flip;
++pointCount;
}
}
manifold.pointCount = pointCount;
}
public void set(final ClipVertex cv) {
v.set(cv.v);
id.set(cv.id);
}
@Override
public final boolean raycast(
RayCastOutput output, RayCastInput input, Transform xf, int childIndex) {
final Vec2 p1 = pool1;
final Vec2 p2 = pool2;
final Vec2 d = pool3;
final Vec2 temp = pool4;
p1.set(input.p1).subLocal(xf.p);
Rot.mulTrans(xf.q, p1, p1);
p2.set(input.p2).subLocal(xf.p);
Rot.mulTrans(xf.q, p2, p2);
d.set(p2).subLocal(p1);
// if (count == 2) {
// } else {
float lower = 0, upper = input.maxFraction;
int index = -1;
for (int i = 0; i < m_count; ++i) {
// p = p1 + a * d
// dot(normal, p - v) = 0
// dot(normal, p1 - v) + a * dot(normal, d) = 0
temp.set(m_vertices[i]).subLocal(p1);
final float numerator = Vec2.dot(m_normals[i], temp);
final float denominator = Vec2.dot(m_normals[i], d);
if (denominator == 0.0f) {
if (numerator < 0.0f) {
return false;
}
} else {
// Note: we want this predicate without division:
// lower < numerator / denominator, where denominator < 0
// Since denominator < 0, we have to flip the inequality:
// lower < numerator / denominator <==> denominator * lower >
// numerator.
if (denominator < 0.0f && numerator < lower * denominator) {
// Increase lower.
// The segment enters this half-space.
lower = numerator / denominator;
index = i;
} else if (denominator > 0.0f && numerator < upper * denominator) {
// Decrease upper.
// The segment exits this half-space.
upper = numerator / denominator;
}
}
if (upper < lower) {
return false;
}
}
assert (0.0f <= lower && lower <= input.maxFraction);
if (index >= 0) {
output.fraction = lower;
Rot.mulToOutUnsafe(xf.q, m_normals[index], output.normal);
// normal = Mul(xf.R, m_normals[index]);
return true;
}
return false;
}