public float getCurrentLengthA() {
final Vec2 p = pool.popVec2();
m_bodyA.getWorldPointToOut(m_localAnchorA, p);
p.subLocal(m_groundAnchorA);
float length = p.length();
pool.pushVec2(1);
return length;
}
public void set(SimplexVertex sv) {
wA.set(sv.wA);
wB.set(sv.wB);
w.set(sv.w);
a = sv.a;
indexA = sv.indexA;
indexB = sv.indexB;
}
public float getLength2() {
final Vec2 p = pool.popVec2();
m_bodyB.getWorldPointToOut(m_localAnchorB, p);
p.subLocal(m_groundAnchorB);
float len = p.length();
pool.pushVec2(1);
return len;
}
/** Solve a line segment using barycentric coordinates. */
public void solve2() {
// Solve a line segment using barycentric coordinates.
//
// p = a1 * w1 + a2 * w2
// a1 + a2 = 1
//
// The vector from the origin to the closest point on the line is
// perpendicular to the line.
// e12 = w2 - w1
// dot(p, e) = 0
// a1 * dot(w1, e) + a2 * dot(w2, e) = 0
//
// 2-by-2 linear system
// [1 1 ][a1] = [1]
// [w1.e12 w2.e12][a2] = [0]
//
// Define
// d12_1 = dot(w2, e12)
// d12_2 = -dot(w1, e12)
// d12 = d12_1 + d12_2
//
// Solution
// a1 = d12_1 / d12
// a2 = d12_2 / d12
final Vec2 w1 = m_v1.w;
final Vec2 w2 = m_v2.w;
e12.set(w2).subLocal(w1);
// w1 region
float d12_2 = -Vec2.dot(w1, e12);
if (d12_2 <= 0.0f) {
// a2 <= 0, so we clamp it to 0
m_v1.a = 1.0f;
m_count = 1;
return;
}
// w2 region
float d12_1 = Vec2.dot(w2, e12);
if (d12_1 <= 0.0f) {
// a1 <= 0, so we clamp it to 0
m_v2.a = 1.0f;
m_count = 1;
m_v1.set(m_v2);
return;
}
// Must be in e12 region.
float inv_d12 = 1.0f / (d12_1 + d12_2);
m_v1.a = d12_1 * inv_d12;
m_v2.a = d12_2 * inv_d12;
m_count = 2;
}
/**
* Get the supporting vertex in the given direction.
*
* @param d
* @return
*/
public final Vec2 getSupportVertex(final Vec2 d) {
int bestIndex = 0;
float bestValue = Vec2.dot(m_vertices[0], d);
for (int i = 1; i < m_count; i++) {
float value = Vec2.dot(m_vertices[i], d);
if (value > bestValue) {
bestIndex = i;
bestValue = value;
}
}
return m_vertices[bestIndex];
}
protected PulleyJoint(IWorldPool argWorldPool, PulleyJointDef def) {
super(argWorldPool, def);
m_groundAnchorA.set(def.groundAnchorA);
m_groundAnchorB.set(def.groundAnchorB);
m_localAnchorA.set(def.localAnchorA);
m_localAnchorB.set(def.localAnchorB);
assert (def.ratio != 0.0f);
m_ratio = def.ratio;
m_lengthA = def.lengthA;
m_lengthB = def.lengthB;
m_constant = def.lengthA + m_ratio * def.lengthB;
m_impulse = 0.0f;
}
public final void getSearchDirection(final Vec2 out) {
switch (m_count) {
case 1:
out.set(m_v1.w).negateLocal();
return;
case 2:
e12.set(m_v2.w).subLocal(m_v1.w);
// use out for a temp variable real quick
out.set(m_v1.w).negateLocal();
float sgn = Vec2.cross(e12, out);
if (sgn > 0f) {
// Origin is left of e12.
Vec2.crossToOutUnsafe(1f, e12, out);
return;
} else {
// Origin is right of e12.
Vec2.crossToOutUnsafe(e12, 1f, out);
return;
}
default:
assert (false);
out.setZero();
return;
}
}
// djm pooled, from above
public float getMetric() {
switch (m_count) {
case 0:
assert (false);
return 0.0f;
case 1:
return 0.0f;
case 2:
return MathUtils.distance(m_v1.w, m_v2.w);
case 3:
case3.set(m_v2.w).subLocal(m_v1.w);
case33.set(m_v3.w).subLocal(m_v1.w);
// return Vec2.cross(m_v2.w - m_v1.w, m_v3.w - m_v1.w);
return Vec2.cross(case3, case33);
default:
assert (false);
return 0.0f;
}
}
/**
* this returns pooled objects. don't keep or modify them
*
* @return
*/
public void getClosestPoint(final Vec2 out) {
switch (m_count) {
case 0:
assert (false);
out.setZero();
return;
case 1:
out.set(m_v1.w);
return;
case 2:
case22.set(m_v2.w).mulLocal(m_v2.a);
case2.set(m_v1.w).mulLocal(m_v1.a).addLocal(case22);
out.set(case2);
return;
case 3:
out.setZero();
return;
default:
assert (false);
out.setZero();
return;
}
}
@Override
public boolean solvePositionConstraints(final SolverData data) {
final Rot qA = pool.popRot();
final Rot qB = pool.popRot();
final Vec2 rA = pool.popVec2();
final Vec2 rB = pool.popVec2();
final Vec2 uA = pool.popVec2();
final Vec2 uB = pool.popVec2();
final Vec2 temp = pool.popVec2();
final Vec2 PA = pool.popVec2();
final Vec2 PB = pool.popVec2();
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);
Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA);
Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB);
uA.set(cA).addLocal(rA).subLocal(m_groundAnchorA);
uB.set(cB).addLocal(rB).subLocal(m_groundAnchorB);
float lengthA = uA.length();
float lengthB = uB.length();
if (lengthA > 10.0f * Settings.linearSlop) {
uA.mulLocal(1.0f / lengthA);
} else {
uA.setZero();
}
if (lengthB > 10.0f * Settings.linearSlop) {
uB.mulLocal(1.0f / lengthB);
} else {
uB.setZero();
}
// Compute effective mass.
float ruA = Vec2.cross(rA, uA);
float ruB = Vec2.cross(rB, uB);
float mA = m_invMassA + m_invIA * ruA * ruA;
float mB = m_invMassB + m_invIB * ruB * ruB;
float mass = mA + m_ratio * m_ratio * mB;
if (mass > 0.0f) {
mass = 1.0f / mass;
}
float C = m_constant - lengthA - m_ratio * lengthB;
float linearError = MathUtils.abs(C);
float impulse = -mass * C;
PA.set(uA).mulLocal(-impulse);
PB.set(uB).mulLocal(-m_ratio * impulse);
cA.x += m_invMassA * PA.x;
cA.y += m_invMassA * PA.y;
aA += m_invIA * Vec2.cross(rA, PA);
cB.x += m_invMassB * PB.x;
cB.y += m_invMassB * PB.y;
aB += m_invIB * Vec2.cross(rB, PB);
// 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);
pool.pushVec2(7);
return linearError < Settings.linearSlop;
}
@Override
public void solveVelocityConstraints(final 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;
final Vec2 vpA = pool.popVec2();
final Vec2 vpB = pool.popVec2();
final Vec2 PA = pool.popVec2();
final Vec2 PB = pool.popVec2();
Vec2.crossToOutUnsafe(wA, m_rA, vpA);
vpA.addLocal(vA);
Vec2.crossToOutUnsafe(wB, m_rB, vpB);
vpB.addLocal(vB);
float Cdot = -Vec2.dot(m_uA, vpA) - m_ratio * Vec2.dot(m_uB, vpB);
float impulse = -m_mass * Cdot;
m_impulse += impulse;
PA.set(m_uA).mulLocal(-impulse);
PB.set(m_uB).mulLocal(-m_ratio * impulse);
vA.x += m_invMassA * PA.x;
vA.y += m_invMassA * PA.y;
wA += m_invIA * Vec2.cross(m_rA, PA);
vB.x += m_invMassB * PB.x;
vB.y += m_invMassB * PB.y;
wB += m_invIB * Vec2.cross(m_rB, PB);
// 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(4);
}
@Override
public void initVelocityConstraints(final 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);
m_uA.set(cA).addLocal(m_rA).subLocal(m_groundAnchorA);
m_uB.set(cB).addLocal(m_rB).subLocal(m_groundAnchorB);
float lengthA = m_uA.length();
float lengthB = m_uB.length();
if (lengthA > 10f * Settings.linearSlop) {
m_uA.mulLocal(1.0f / lengthA);
} else {
m_uA.setZero();
}
if (lengthB > 10f * Settings.linearSlop) {
m_uB.mulLocal(1.0f / lengthB);
} else {
m_uB.setZero();
}
// Compute effective mass.
float ruA = Vec2.cross(m_rA, m_uA);
float ruB = Vec2.cross(m_rB, m_uB);
float mA = m_invMassA + m_invIA * ruA * ruA;
float mB = m_invMassB + m_invIB * ruB * ruB;
m_mass = mA + m_ratio * m_ratio * mB;
if (m_mass > 0.0f) {
m_mass = 1.0f / m_mass;
}
if (data.step.warmStarting) {
// Scale impulses to support variable time steps.
m_impulse *= data.step.dtRatio;
// Warm starting.
final Vec2 PA = pool.popVec2();
final Vec2 PB = pool.popVec2();
PA.set(m_uA).mulLocal(-m_impulse);
PB.set(m_uB).mulLocal(-m_ratio * m_impulse);
vA.x += m_invMassA * PA.x;
vA.y += m_invMassA * PA.y;
wA += m_invIA * Vec2.cross(m_rA, PA);
vB.x += m_invMassB * PB.x;
vB.y += m_invMassB * PB.y;
wB += m_invIB * Vec2.cross(m_rB, PB);
pool.pushVec2(2);
} else {
m_impulse = 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);
}
@Override
public void getReactionForce(float inv_dt, Vec2 argOut) {
argOut.set(m_uB).mulLocal(m_impulse).mulLocal(inv_dt);
}
/**
* Compute the closest points between two shapes. Supports any combination of: CircleShape and
* PolygonShape. The simplex cache is input/output. On the first call set SimplexCache.count to
* zero.
*
* @param output
* @param cache
* @param input
*/
public final void distance(
final DistanceOutput output, final SimplexCache cache, final DistanceInput input) {
GJK_CALLS++;
final DistanceProxy proxyA = input.proxyA;
final DistanceProxy proxyB = input.proxyB;
Transform transformA = input.transformA;
Transform transformB = input.transformB;
// Initialize the simplex.
simplex.readCache(cache, proxyA, transformA, proxyB, transformB);
// Get simplex vertices as an array.
SimplexVertex[] vertices = simplex.vertices;
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
// (pooled above)
int saveCount = 0;
simplex.getClosestPoint(closestPoint);
float distanceSqr1 = closestPoint.lengthSquared();
float distanceSqr2 = distanceSqr1;
// Main iteration loop
int iter = 0;
while (iter < MAX_ITERS) {
// Copy simplex so we can identify duplicates.
saveCount = simplex.m_count;
for (int i = 0; i < saveCount; i++) {
saveA[i] = vertices[i].indexA;
saveB[i] = vertices[i].indexB;
}
switch (simplex.m_count) {
case 1:
break;
case 2:
simplex.solve2();
break;
case 3:
simplex.solve3();
break;
default:
assert (false);
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex.m_count == 3) {
break;
}
// Compute closest point.
simplex.getClosestPoint(closestPoint);
distanceSqr2 = closestPoint.lengthSquared();
// ensure progress
if (distanceSqr2 >= distanceSqr1) {
// break;
}
distanceSqr1 = distanceSqr2;
// get search direction;
simplex.getSearchDirection(d);
// Ensure the search direction is numerically fit.
if (d.lengthSquared() < Settings.EPSILON * Settings.EPSILON) {
// The origin is probably contained by a line segment
// or triangle. Thus the shapes are overlapped.
// We can't return zero here even though there may be overlap.
// In case the simplex is a point, segment, or triangle it is difficult
// to determine if the origin is contained in the CSO or very close to it.
break;
}
/*
* SimplexVertex* vertex = vertices + simplex.m_count; vertex.indexA =
* proxyA.GetSupport(MulT(transformA.R, -d)); vertex.wA = Mul(transformA,
* proxyA.GetVertex(vertex.indexA)); Vec2 wBLocal; vertex.indexB =
* proxyB.GetSupport(MulT(transformB.R, d)); vertex.wB = Mul(transformB,
* proxyB.GetVertex(vertex.indexB)); vertex.w = vertex.wB - vertex.wA;
*/
// Compute a tentative new simplex vertex using support points.
SimplexVertex vertex = vertices[simplex.m_count];
Rot.mulTransUnsafe(transformA.q, d.negateLocal(), temp);
vertex.indexA = proxyA.getSupport(temp);
Transform.mulToOutUnsafe(transformA, proxyA.getVertex(vertex.indexA), vertex.wA);
// Vec2 wBLocal;
Rot.mulTransUnsafe(transformB.q, d.negateLocal(), temp);
vertex.indexB = proxyB.getSupport(temp);
Transform.mulToOutUnsafe(transformB, proxyB.getVertex(vertex.indexB), vertex.wB);
vertex.w.set(vertex.wB).subLocal(vertex.wA);
// Iteration count is equated to the number of support point calls.
++iter;
++GJK_ITERS;
// Check for duplicate support points. This is the main termination criteria.
boolean duplicate = false;
for (int i = 0; i < saveCount; ++i) {
if (vertex.indexA == saveA[i] && vertex.indexB == saveB[i]) {
duplicate = true;
break;
}
}
// If we found a duplicate support point we must exit to avoid cycling.
if (duplicate) {
break;
}
// New vertex is ok and needed.
++simplex.m_count;
}
GJK_MAX_ITERS = MathUtils.max(GJK_MAX_ITERS, iter);
// Prepare output.
simplex.getWitnessPoints(output.pointA, output.pointB);
output.distance = MathUtils.distance(output.pointA, output.pointB);
output.iterations = iter;
// Cache the simplex.
simplex.writeCache(cache);
// Apply radii if requested.
if (input.useRadii) {
float rA = proxyA.m_radius;
float rB = proxyB.m_radius;
if (output.distance > rA + rB && output.distance > Settings.EPSILON) {
// Shapes are still no overlapped.
// Move the witness points to the outer surface.
output.distance -= rA + rB;
normal.set(output.pointB).subLocal(output.pointA);
normal.normalize();
temp.set(normal).mulLocal(rA);
output.pointA.addLocal(temp);
temp.set(normal).mulLocal(rB);
output.pointB.subLocal(temp);
} else {
// Shapes are overlapped when radii are considered.
// Move the witness points to the middle.
// Vec2 p = 0.5f * (output.pointA + output.pointB);
output.pointA.addLocal(output.pointB).mulLocal(.5f);
output.pointB.set(output.pointA);
output.distance = 0.0f;
}
}
}
/**
* Solve a line segment using barycentric coordinates.<br>
* Possible regions:<br>
* - points[2]<br>
* - edge points[0]-points[2]<br>
* - edge points[1]-points[2]<br>
* - inside the triangle
*/
public void solve3() {
w1.set(m_v1.w);
w2.set(m_v2.w);
w3.set(m_v3.w);
// Edge12
// [1 1 ][a1] = [1]
// [w1.e12 w2.e12][a2] = [0]
// a3 = 0
e12.set(w2).subLocal(w1);
float w1e12 = Vec2.dot(w1, e12);
float w2e12 = Vec2.dot(w2, e12);
float d12_1 = w2e12;
float d12_2 = -w1e12;
// Edge13
// [1 1 ][a1] = [1]
// [w1.e13 w3.e13][a3] = [0]
// a2 = 0
e13.set(w3).subLocal(w1);
float w1e13 = Vec2.dot(w1, e13);
float w3e13 = Vec2.dot(w3, e13);
float d13_1 = w3e13;
float d13_2 = -w1e13;
// Edge23
// [1 1 ][a2] = [1]
// [w2.e23 w3.e23][a3] = [0]
// a1 = 0
e23.set(w3).subLocal(w2);
float w2e23 = Vec2.dot(w2, e23);
float w3e23 = Vec2.dot(w3, e23);
float d23_1 = w3e23;
float d23_2 = -w2e23;
// Triangle123
float n123 = Vec2.cross(e12, e13);
float d123_1 = n123 * Vec2.cross(w2, w3);
float d123_2 = n123 * Vec2.cross(w3, w1);
float d123_3 = n123 * Vec2.cross(w1, w2);
// w1 region
if (d12_2 <= 0.0f && d13_2 <= 0.0f) {
m_v1.a = 1.0f;
m_count = 1;
return;
}
// e12
if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f) {
float inv_d12 = 1.0f / (d12_1 + d12_2);
m_v1.a = d12_1 * inv_d12;
m_v2.a = d12_2 * inv_d12;
m_count = 2;
return;
}
// e13
if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f) {
float inv_d13 = 1.0f / (d13_1 + d13_2);
m_v1.a = d13_1 * inv_d13;
m_v3.a = d13_2 * inv_d13;
m_count = 2;
m_v2.set(m_v3);
return;
}
// w2 region
if (d12_1 <= 0.0f && d23_2 <= 0.0f) {
m_v2.a = 1.0f;
m_count = 1;
m_v1.set(m_v2);
return;
}
// w3 region
if (d13_1 <= 0.0f && d23_1 <= 0.0f) {
m_v3.a = 1.0f;
m_count = 1;
m_v1.set(m_v3);
return;
}
// e23
if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f) {
float inv_d23 = 1.0f / (d23_1 + d23_2);
m_v2.a = d23_1 * inv_d23;
m_v3.a = d23_2 * inv_d23;
m_count = 2;
m_v1.set(m_v3);
return;
}
// Must be in triangle123
float inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);
m_v1.a = d123_1 * inv_d123;
m_v2.a = d123_2 * inv_d123;
m_v3.a = d123_3 * inv_d123;
m_count = 3;
}
public void getWitnessPoints(Vec2 pA, Vec2 pB) {
switch (m_count) {
case 0:
assert (false);
break;
case 1:
pA.set(m_v1.wA);
pB.set(m_v1.wB);
break;
case 2:
case2.set(m_v1.wA).mulLocal(m_v1.a);
pA.set(m_v2.wA).mulLocal(m_v2.a).addLocal(case2);
// m_v1.a * m_v1.wA + m_v2.a * m_v2.wA;
// *pB = m_v1.a * m_v1.wB + m_v2.a * m_v2.wB;
case2.set(m_v1.wB).mulLocal(m_v1.a);
pB.set(m_v2.wB).mulLocal(m_v2.a).addLocal(case2);
break;
case 3:
pA.set(m_v1.wA).mulLocal(m_v1.a);
case3.set(m_v2.wA).mulLocal(m_v2.a);
case33.set(m_v3.wA).mulLocal(m_v3.a);
pA.addLocal(case3).addLocal(case33);
pB.set(pA);
// *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA + m_v3.a * m_v3.wA;
// *pB = *pA;
break;
default:
assert (false);
break;
}
}
