/**
* 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 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);
}
