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; } }
/** * 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; }
wB += m_invIB * Vec2.cross(m_rB, impulse);
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; } }
@Override public void solveVelocityConstraints(final SolverData step) { float crossMassSum = 0.0f; float dotMassSum = 0.0f; Velocity[] velocities = step.velocities; Position[] positions = step.positions; final Vec2 d[] = pool.getVec2Array(bodies.length); for (int i = 0; i < bodies.length; ++i) { final int prev = (i == 0) ? bodies.length - 1 : i - 1; final int next = (i == bodies.length - 1) ? 0 : i + 1; d[i].set(positions[bodies[next].m_islandIndex].c); d[i].subLocal(positions[bodies[prev].m_islandIndex].c); dotMassSum += (d[i].lengthSquared()) / bodies[i].getMass(); crossMassSum += Vec2.cross(velocities[bodies[i].m_islandIndex].v, d[i]); } float lambda = -2.0f * crossMassSum / dotMassSum; // System.out.println(crossMassSum + " " +dotMassSum); // lambda = MathUtils.clamp(lambda, -Settings.maxLinearCorrection, // Settings.maxLinearCorrection); m_impulse += lambda; // System.out.println(m_impulse); for (int i = 0; i < bodies.length; ++i) { velocities[bodies[i].m_islandIndex].v.x += bodies[i].m_invMass * d[i].y * .5f * lambda; velocities[bodies[i].m_islandIndex].v.y += bodies[i].m_invMass * -d[i].x * .5f * lambda; } }
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); }
e2.set(s).negateLocal().addLocal(i + 1 < m_count ? m_vertices[i + 1] : m_vertices[0]); final float D = Vec2.cross(e1, e2);
vB.x += m_invMassB * m_impulse.x; vB.y += m_invMassB * m_impulse.y; wB += m_invIB * Vec2.cross(m_rB, m_impulse); } else { m_impulse.setZero();