// djm pooling from above
public final void findIncidentEdge(
final ClipVertex[] c,
final PolygonShape poly1,
final Transform xf1,
int edge1,
final PolygonShape poly2,
final Transform xf2) {
int count1 = poly1.m_vertexCount;
final Vec2[] normals1 = poly1.m_normals;
int count2 = poly2.m_vertexCount;
final Vec2[] vertices2 = poly2.m_vertices;
final Vec2[] normals2 = poly2.m_normals;
assert (0 <= edge1 && edge1 < count1);
// Get the normal of the reference edge in poly2's frame.
Mat22.mulToOut(xf1.R, normals1[edge1], normal1); // temporary
// b2Vec2 normal1 = b2MulT(xf2.R, b2Mul(xf1.R, normals1[edge1]));
Mat22.mulTransToOut(xf2.R, normal1, normal1);
// Find the incident edge on poly2.
int index = 0;
float minDot = Float.MAX_VALUE;
for (int i = 0; i < count2; ++i) {
float dot = Vec2.dot(normal1, normals2[i]);
if (dot < minDot) {
minDot = dot;
index = i;
}
}
// Build the clip vertices for the incident edge.
int i1 = index;
int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
Transform.mulToOut(xf2, vertices2[i1], c[0].v); // = Mul(xf2, vertices2[i1]);
c[0].id.features.referenceEdge = edge1;
c[0].id.features.incidentEdge = i1;
c[0].id.features.incidentVertex = 0;
Transform.mulToOut(xf2, vertices2[i2], c[1].v); // = Mul(xf2, vertices2[i2]);
c[1].id.features.referenceEdge = edge1;
c[1].id.features.incidentEdge = i2;
c[1].id.features.incidentVertex = 1;
}
/**
* Find the separation between poly1 and poly2 for a given edge normal on poly1.
*
* @param poly1
* @param xf1
* @param edge1
* @param poly2
* @param xf2
*/
public final float edgeSeparation(
final PolygonShape poly1,
final Transform xf1,
final int edge1,
final PolygonShape poly2,
final Transform xf2) {
int count1 = poly1.m_vertexCount;
final Vec2[] vertices1 = poly1.m_vertices;
final Vec2[] normals1 = poly1.m_normals;
int count2 = poly2.m_vertexCount;
final Vec2[] vertices2 = poly2.m_vertices;
assert (0 <= edge1 && edge1 < count1);
// Convert normal from poly1's frame into poly2's frame.
// Vec2 normal1World = Mul(xf1.R, normals1[edge1]);
Mat22.mulToOut(xf1.R, normals1[edge1], normal1World);
// Vec2 normal1 = MulT(xf2.R, normal1World);
Mat22.mulTransToOut(xf2.R, normal1World, normal1);
// Find support vertex on poly2 for -normal.
int index = 0;
float minDot = Float.MAX_VALUE;
for (int i = 0; i < count2; ++i) {
float dot = Vec2.dot(vertices2[i], normal1);
if (dot < minDot) {
minDot = dot;
index = i;
}
}
// Vec2 v1 = Mul(xf1, vertices1[edge1]);
// Vec2 v2 = Mul(xf2, vertices2[index]);
Transform.mulToOut(xf1, vertices1[edge1], v1);
Transform.mulToOut(xf2, vertices2[index], v2);
float separation = Vec2.dot(v2.subLocal(v1), normal1World);
return separation;
}
protected final void synchronizeFixtures() {
final Transform xf1 = pxf;
xf1.R.set(m_sweep.a0);
// xf1.position = m_sweep.c0 - Mul(xf1.R, m_sweep.localCenter);
Mat22.mulToOut(xf1.R, m_sweep.localCenter, xf1.position);
xf1.position.mulLocal(-1).addLocal(m_sweep.c0);
BroadPhase broadPhase = m_world.m_contactManager.m_broadPhase;
for (Fixture f = m_fixtureList; f != null; f = f.m_next) {
f.synchronize(broadPhase, xf1, m_xf);
}
}
@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 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;
}
/**
* Find the max separation between poly1 and poly2 using edge normals from poly1.
*
* @param edgeIndex
* @param poly1
* @param xf1
* @param poly2
* @param xf2
* @return
*/
public final void findMaxSeparation(
EdgeResults results,
final PolygonShape poly1,
final Transform xf1,
final PolygonShape poly2,
final Transform xf2) {
int count1 = poly1.m_vertexCount;
final Vec2[] normals1 = poly1.m_normals;
// Vector pointing from the centroid of poly1 to the centroid of poly2.
Transform.mulToOut(xf2, poly2.m_centroid, d);
Transform.mulToOut(xf1, poly1.m_centroid, temp);
d.subLocal(temp);
Mat22.mulTransToOut(xf1.R, d, dLocal1);
// Find edge normal on poly1 that has the largest projection onto d.
int edge = 0;
float dot;
float maxDot = Float.MIN_VALUE;
for (int i = 0; i < count1; i++) {
dot = Vec2.dot(normals1[i], dLocal1);
if (dot > maxDot) {
maxDot = dot;
edge = i;
}
}
// Get the separation for the edge normal.
float s = edgeSeparation(poly1, xf1, edge, poly2, xf2);
// Check the separation for the previous edge normal.
int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
float sPrev = edgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
// Check the separation for the next edge normal.
int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
float sNext = edgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
// Find the best edge and the search direction.
int bestEdge;
float bestSeparation;
int increment;
if (sPrev > s && sPrev > sNext) {
increment = -1;
bestEdge = prevEdge;
bestSeparation = sPrev;
} else if (sNext > s) {
increment = 1;
bestEdge = nextEdge;
bestSeparation = sNext;
} else {
results.edgeIndex = edge;
results.separation = s;
return;
}
// Perform a local search for the best edge normal.
for (; ; ) {
if (increment == -1) {
edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
} else {
edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
}
s = edgeSeparation(poly1, xf1, edge, poly2, xf2);
if (s > bestSeparation) {
bestEdge = edge;
bestSeparation = s;
} else {
break;
}
}
results.edgeIndex = bestEdge;
results.separation = bestSeparation;
}
@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 initVelocityConstraints(TimeStep step) {
Body b = m_bodyB;
float mass = b.getMass();
// Frequency
float omega = 2.0f * MathUtils.PI * m_frequencyHz;
// Damping coefficient
float d = 2.0f * mass * m_dampingRatio * omega;
// Spring stiffness
float k = mass * (omega * omega);
// magic formulas
// gamma has units of inverse mass.
// beta has units of inverse time.
assert (d + step.dt * k > Settings.EPSILON);
m_gamma = step.dt * (d + step.dt * k);
if (m_gamma != 0.0f) {
m_gamma = 1.0f / m_gamma;
}
m_beta = step.dt * k * m_gamma;
Vec2 r = pool.popVec2();
// Compute the effective mass matrix.
// Vec2 r = Mul(b.getTransform().R, m_localAnchor - b.getLocalCenter());
r.set(m_localAnchor).subLocal(b.getLocalCenter());
Mat22.mulToOut(b.getTransform().R, r, r);
// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
// = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y
// -r1.x*r1.y]
// [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y
// r1.x*r1.x]
float invMass = b.m_invMass;
float invI = b.m_invI;
Mat22 K1 = pool.popMat22();
K1.m11 = invMass;
K1.m21 = 0.0f;
K1.m12 = 0.0f;
K1.m22 = invMass;
Mat22 K2 = pool.popMat22();
K2.m11 = invI * r.y * r.y;
K2.m21 = -invI * r.x * r.y;
K2.m12 = -invI * r.x * r.y;
K2.m22 = invI * r.x * r.x;
Mat22 K = pool.popMat22();
K.set(K1).addLocal(K2);
K.m11 += m_gamma;
K.m22 += m_gamma;
K.invertToOut(m_mass);
m_C.set(b.m_sweep.c).addLocal(r).subLocal(m_target);
// Cheat with some damping
b.m_angularVelocity *= 0.98f;
// Warm starting.
m_impulse.mulLocal(step.dtRatio);
// pool
Vec2 temp = pool.popVec2();
temp.set(m_impulse).mulLocal(invMass);
b.m_linearVelocity.addLocal(temp);
b.m_angularVelocity += invI * Vec2.cross(r, m_impulse);
pool.pushVec2(2);
pool.pushMat22(3);
}
public final void getLocalVectorToOut(Vec2 worldVector, Vec2 out) {
Mat22.mulTransToOut(m_xf.R, worldVector, out);
}
public final void getWorldVectorToOut(Vec2 localVector, Vec2 out) {
Mat22.mulToOut(m_xf.R, localVector, out);
}
