Java Code Examples for org.jbox2d.common.Vec2#cross()

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

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;
  }
}
 

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

@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 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;
  }
}
 

@Override
  public void solveVelocityConstraints(final SolverData data) {

    Vec2 vB = data.velocities[m_indexB].v;
    float wB = data.velocities[m_indexB].w;

    // Cdot = v + cross(w, r)
    final Vec2 Cdot = pool.popVec2();
    Vec2.crossToOutUnsafe(wB, m_rB, Cdot);
    Cdot.addLocal(vB);

    final Vec2 impulse = pool.popVec2();
    final Vec2 temp = pool.popVec2();

    temp.set(m_impulse).mulLocal(m_gamma).addLocal(m_C).addLocal(Cdot).negateLocal();
    Mat22.mulToOutUnsafe(m_mass, temp, impulse);

    Vec2 oldImpulse = temp;
    oldImpulse.set(m_impulse);
    m_impulse.addLocal(impulse);
    float maxImpulse = data.step.dt * m_maxForce;
    if (m_impulse.lengthSquared() > maxImpulse * maxImpulse) {
      m_impulse.mulLocal(maxImpulse / m_impulse.length());
    }
    impulse.set(m_impulse).subLocal(oldImpulse);

    vB.x += m_invMassB * impulse.x;
    vB.y += m_invMassB * impulse.y;
    wB += m_invIB * Vec2.cross(m_rB, impulse);

//    data.velocities[m_indexB].v.set(vB);
    data.velocities[m_indexB].w = wB;
    
    pool.pushVec2(3);
  }
 

@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;
  }
 

public final void complexlogic(final Vec2[] verts, final int num, final Vec2Array vecPool,
                      final IntArray intPool) {

    int i0 = 1;
    int[] hull = new int[Settings.maxPolygonVertices];

    int m = 0;
    int ih = i0;

    int counter = 0;
    while (true) {

        counter++;
        if (counter>5) {
            break;
        }

        hull[m] = ih;

        int ie = 0;
        for (int j = 1; j < num; ++j) {

            if (ie == ih) {
                ie = j;
                continue;
            }

            Vec2 r = pool1.set(verts[ie]).subLocal(verts[hull[m]]);
            Vec2 v = pool2.set(verts[j]).subLocal(verts[hull[m]]);

            float c = Vec2.cross(r, v);
            if (c < 0.0f) {
                ie = j;
            }

            if (c == 0.0f && v.lengthSquared() > r.lengthSquared()) {
                ie = j;
            }
        }

        ++m;
        ih = ie;

        System.out.println("End of loop");
        System.out.println(ie);
        System.out.println(i0);
        if (ie == i0) {
            break;
        }
    }
    System.out.println("Finished");
}
 

@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);
  }
 

/**
   * @see org.jbox2d.dynamics.joints.Joint#initVelocityConstraints(org.jbox2d.dynamics.TimeStep)
   */
  @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;

    float aA = data.positions[m_indexA].a;
    Vec2 vA = data.velocities[m_indexA].v;
    float wA = data.velocities[m_indexA].w;

    float aB = data.positions[m_indexB].a;
    Vec2 vB = data.velocities[m_indexB].v;
    float wB = data.velocities[m_indexB].w;


    final Vec2 temp = pool.popVec2();
    final Rot qA = pool.popRot();
    final Rot qB = pool.popRot();

    qA.set(aA);
    qB.set(aB);

    // Compute the effective mass matrix.
    Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), m_rA);
    Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), m_rB);

    // J = [-I -r1_skew I r2_skew]
    // [ 0 -1 0 1]
    // r_skew = [-ry; rx]

    // Matlab
    // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
    // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
    // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]

    float mA = m_invMassA, mB = m_invMassB;
    float iA = m_invIA, iB = m_invIB;

    final Mat22 K = pool.popMat22();
    K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y;
    K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y;
    K.ey.x = K.ex.y;
    K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x;

    K.invertToOut(m_linearMass);

    m_angularMass = iA + iB;
    if (m_angularMass > 0.0f) {
      m_angularMass = 1.0f / m_angularMass;
    }

    if (data.step.warmStarting) {
      // Scale impulses to support a variable time step.
      m_linearImpulse.mulLocal(data.step.dtRatio);
      m_angularImpulse *= data.step.dtRatio;

      final Vec2 P = pool.popVec2();
      P.set(m_linearImpulse);

      temp.set(P).mulLocal(mA);
      vA.subLocal(temp);
      wA -= iA * (Vec2.cross(m_rA, P) + m_angularImpulse);

      temp.set(P).mulLocal(mB);
      vB.addLocal(temp);
      wB += iB * (Vec2.cross(m_rB, P) + m_angularImpulse);

      pool.pushVec2(1);
    } else {
      m_linearImpulse.setZero();
      m_angularImpulse = 0.0f;
    }
//    data.velocities[m_indexA].v.set(vA);
    if( data.velocities[m_indexA].w != wA) {
      assert(data.velocities[m_indexA].w != wA);
    }
    data.velocities[m_indexA].w = wA;
//    data.velocities[m_indexB].v.set(vB);
    data.velocities[m_indexB].w = wB;

    pool.pushRot(2);
    pool.pushVec2(1);
    pool.pushMat22(1);
  }
 

@Override
  public void initVelocityConstraints(final SolverData data) {
    m_indexB = m_bodyB.m_islandIndex;
    m_localCenterB.set(m_bodyB.m_sweep.localCenter);
    m_invMassB = m_bodyB.m_invMass;
    m_invIB = m_bodyB.m_invI;

    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 qB = pool.popRot();

    qB.set(aB);

    float mass = m_bodyB.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.
    float h = data.step.dt;
    assert (d + h * k > Settings.EPSILON);
    m_gamma = h * (d + h * k);
    if (m_gamma != 0.0f) {
      m_gamma = 1.0f / m_gamma;
    }
    m_beta = h * k * m_gamma;

    Vec2 temp = pool.popVec2();

    // Compute the effective mass matrix.
    Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), m_rB);

    // 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]
    final Mat22 K = pool.popMat22();
    K.ex.x = m_invMassB + m_invIB * m_rB.y * m_rB.y + m_gamma;
    K.ex.y = -m_invIB * m_rB.x * m_rB.y;
    K.ey.x = K.ex.y;
    K.ey.y = m_invMassB + m_invIB * m_rB.x * m_rB.x + m_gamma;

    K.invertToOut(m_mass);

    m_C.set(cB).addLocal(m_rB).subLocal(m_targetA);
    m_C.mulLocal(m_beta);

    // Cheat with some damping
    wB *= 0.98f;

    if (data.step.warmStarting) {
      m_impulse.mulLocal(data.step.dtRatio);
      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();
    }

//    data.velocities[m_indexB].v.set(vB);
    data.velocities[m_indexB].w = wB;

    pool.pushVec2(1);
    pool.pushMat22(1);
    pool.pushRot(1);
  }
 

@Override
  public boolean solvePositionConstraints(final SolverData data) {
    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;
    final Rot qA = pool.popRot();
    final Rot qB = pool.popRot();
    final Vec2 temp = pool.popVec2();
    final Vec2 rA = pool.popVec2();
    final Vec2 rB = pool.popVec2();

    qA.set(aA);
    qB.set(aB);

    float mA = m_invMassA, mB = m_invMassB;
    float iA = m_invIA, iB = m_invIB;

    Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA);
    Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB);
    float positionError, angularError;

    final Mat33 K = pool.popMat33();
    final Vec2 C1 = pool.popVec2();
    final Vec2 P = pool.popVec2();

    K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
    K.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
    K.ez.x = -rA.y * iA - rB.y * iB;
    K.ex.y = K.ey.x;
    K.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
    K.ez.y = rA.x * iA + rB.x * iB;
    K.ex.z = K.ez.x;
    K.ey.z = K.ez.y;
    K.ez.z = iA + iB;
    if (m_frequencyHz > 0.0f) {
      C1.set(cB).addLocal(rB).subLocal(cA).subLocal(rA);

      positionError = C1.length();
      angularError = 0.0f;

      K.solve22ToOut(C1, P);
      P.negateLocal();

      cA.x -= mA * P.x;
      cA.y -= mA * P.y;
      aA -= iA * Vec2.cross(rA, P);

      cB.x += mB * P.x;
      cB.y += mB * P.y;
      aB += iB * Vec2.cross(rB, P);
    } else {
      C1.set(cB).addLocal(rB).subLocal(cA).subLocal(rA);
      float C2 = aB - aA - m_referenceAngle;

      positionError = C1.length();
      angularError = MathUtils.abs(C2);

      final Vec3 C = pool.popVec3();
      final Vec3 impulse = pool.popVec3();
      C.set(C1.x, C1.y, C2);

      K.solve33ToOut(C, impulse);
      impulse.negateLocal();
      P.set(impulse.x, impulse.y);

      cA.x -= mA * P.x;
      cA.y -= mA * P.y;
      aA -= iA * (Vec2.cross(rA, P) + impulse.z);

      cB.x += mB * P.x;
      cB.y += mB * P.y;
      aB += iB * (Vec2.cross(rB, P) + impulse.z);
      pool.pushVec3(2);
    }

//    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.pushVec2(5);
    pool.pushRot(2);
    pool.pushMat33(1);

    return positionError <= Settings.linearSlop && angularError <= Settings.angularSlop;
  }
 

@Override
public boolean solvePositionConstraints(SolverData data) {
  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;

  final Rot qA = pool.popRot();
  final Rot qB = pool.popRot();
  final Vec2 temp = pool.popVec2();

  qA.set(aA);
  qB.set(aB);

  Rot.mulToOut(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA);
  Rot.mulToOut(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB);
  d.set(cB).subLocal(cA).addLocal(rB).subLocal(rA);

  Vec2 ay = pool.popVec2();
  Rot.mulToOut(qA, m_localYAxisA, ay);

  float sAy = Vec2.cross(temp.set(d).addLocal(rA), ay);
  float sBy = Vec2.cross(rB, ay);

  float C = Vec2.dot(d, ay);

  float k = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy;

  float impulse;
  if (k != 0.0f) {
    impulse = -C / k;
  } else {
    impulse = 0.0f;
  }

  final Vec2 P = pool.popVec2();
  P.x = impulse * ay.x;
  P.y = impulse * ay.y;
  float LA = impulse * sAy;
  float LB = impulse * sBy;

  cA.x -= m_invMassA * P.x;
  cA.y -= m_invMassA * P.y;
  aA -= m_invIA * LA;
  cB.x += m_invMassB * P.x;
  cB.y += m_invMassB * P.y;
  aB += m_invIB * LB;

  pool.pushVec2(3);
  pool.pushRot(2);
  // data.positions[m_indexA].c = cA;
  data.positions[m_indexA].a = aA;
  // data.positions[m_indexB].c = cB;
  data.positions[m_indexB].a = aB;

  return MathUtils.abs(C) <= Settings.linearSlop;
}
 

@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_u.set(cB).addLocal(m_rB).subLocal(cA).subLocal(m_rA);

  m_length = m_u.length();

  float C = m_length - m_maxLength;
  if (C > 0.0f) {
    m_state = LimitState.AT_UPPER;
  } else {
    m_state = LimitState.INACTIVE;
  }

  if (m_length > Settings.linearSlop) {
    m_u.mulLocal(1.0f / m_length);
  } else {
    m_u.setZero();
    m_mass = 0.0f;
    m_impulse = 0.0f;
    pool.pushRot(2);
    pool.pushVec2(1);
    return;
  }

  // Compute effective mass.
  float crA = Vec2.cross(m_rA, m_u);
  float crB = Vec2.cross(m_rB, m_u);
  float invMass = m_invMassA + m_invIA * crA * crA + m_invMassB + m_invIB * crB * crB;

  m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;

  if (data.step.warmStarting) {
    // Scale the impulse to support a variable time step.
    m_impulse *= data.step.dtRatio;

    float Px = m_impulse * m_u.x;
    float Py = m_impulse * m_u.y;
    vA.x -= m_invMassA * Px;
    vA.y -= m_invMassA * Py;
    wA -= m_invIA * (m_rA.x * Py - m_rA.y * Px);

    vB.x += m_invMassB * Px;
    vB.y += m_invMassB * Py;
    wB += m_invIB * (m_rB.x * Py - m_rB.y * Px);
  } else {
    m_impulse = 0.0f;
  }

  pool.pushRot(2);
  pool.pushVec2(1);

  // data.velocities[m_indexA].v = vA;
  data.velocities[m_indexA].w = wA;
  // data.velocities[m_indexB].v = vB;
  data.velocities[m_indexB].w = wB;
}
 

@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();

    qA.set(aA);
    qB.set(aB);

    // use m_u as temporary variable
    Rot.mulToOutUnsafe(qA, m_u.set(m_localAnchorA).subLocal(m_localCenterA), m_rA);
    Rot.mulToOutUnsafe(qB, m_u.set(m_localAnchorB).subLocal(m_localCenterB), m_rB);
    m_u.set(cB).addLocal(m_rB).subLocal(cA).subLocal(m_rA);

    pool.pushRot(2);

    // Handle singularity.
    float length = m_u.length();
    if (length > Settings.linearSlop) {
      m_u.x *= 1.0f / length;
      m_u.y *= 1.0f / length;
    } else {
      m_u.set(0.0f, 0.0f);
    }


    float crAu = Vec2.cross(m_rA, m_u);
    float crBu = Vec2.cross(m_rB, m_u);
    float invMass = m_invMassA + m_invIA * crAu * crAu + m_invMassB + m_invIB * crBu * crBu;

    // Compute the effective mass matrix.
    m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;

    if (m_frequencyHz > 0.0f) {
      float C = length - m_length;

      // Frequency
      float omega = 2.0f * MathUtils.PI * m_frequencyHz;

      // Damping coefficient
      float d = 2.0f * m_mass * m_dampingRatio * omega;

      // Spring stiffness
      float k = m_mass * omega * omega;

      // magic formulas
      float h = data.step.dt;
      m_gamma = h * (d + h * k);
      m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;
      m_bias = C * h * k * m_gamma;

      invMass += m_gamma;
      m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
    } else {
      m_gamma = 0.0f;
      m_bias = 0.0f;
    }
    if (data.step.warmStarting) {

      // Scale the impulse to support a variable time step.
      m_impulse *= data.step.dtRatio;

      Vec2 P = pool.popVec2();
      P.set(m_u).mulLocal(m_impulse);

      vA.x -= m_invMassA * P.x;
      vA.y -= m_invMassA * P.y;
      wA -= m_invIA * Vec2.cross(m_rA, P);

      vB.x += m_invMassB * P.x;
      vB.y += m_invMassB * P.y;
      wB += m_invIB * Vec2.cross(m_rB, P);

      pool.pushVec2(1);
    } 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;
  }
 

public void callback(int a, int b, int c) {
  // Create a triad if it will contain particles from both groups.
  int countA =
      ((a < groupB.m_firstIndex) ? 1 : 0) + ((b < groupB.m_firstIndex) ? 1 : 0)
          + ((c < groupB.m_firstIndex) ? 1 : 0);
  if (countA > 0 && countA < 3) {
    int af = system.m_flagsBuffer.data[a];
    int bf = system.m_flagsBuffer.data[b];
    int cf = system.m_flagsBuffer.data[c];
    if ((af & bf & cf & k_triadFlags) != 0) {
      final Vec2 pa = system.m_positionBuffer.data[a];
      final Vec2 pb = system.m_positionBuffer.data[b];
      final Vec2 pc = system.m_positionBuffer.data[c];
      final float dabx = pa.x - pb.x;
      final float daby = pa.y - pb.y;
      final float dbcx = pb.x - pc.x;
      final float dbcy = pb.y - pc.y;
      final float dcax = pc.x - pa.x;
      final float dcay = pc.y - pa.y;
      float maxDistanceSquared = Settings.maxTriadDistanceSquared * system.m_squaredDiameter;
      if (dabx * dabx + daby * daby < maxDistanceSquared
          && dbcx * dbcx + dbcy * dbcy < maxDistanceSquared
          && dcax * dcax + dcay * dcay < maxDistanceSquared) {
        if (system.m_triadCount >= system.m_triadCapacity) {
          int oldCapacity = system.m_triadCapacity;
          int newCapacity =
              system.m_triadCount != 0
                  ? 2 * system.m_triadCount
                  : Settings.minParticleBufferCapacity;
          system.m_triadBuffer =
              BufferUtils.reallocateBuffer(Triad.class, system.m_triadBuffer, oldCapacity,
                  newCapacity);
          system.m_triadCapacity = newCapacity;
        }
        Triad triad = system.m_triadBuffer[system.m_triadCount];
        triad.indexA = a;
        triad.indexB = b;
        triad.indexC = c;
        triad.flags = af | bf | cf;
        triad.strength = MathUtils.min(groupA.m_strength, groupB.m_strength);
        final float midPointx = (float) 1 / 3 * (pa.x + pb.x + pc.x);
        final float midPointy = (float) 1 / 3 * (pa.y + pb.y + pc.y);
        triad.pa.x = pa.x - midPointx;
        triad.pa.y = pa.y - midPointy;
        triad.pb.x = pb.x - midPointx;
        triad.pb.y = pb.y - midPointy;
        triad.pc.x = pc.x - midPointx;
        triad.pc.y = pc.y - midPointy;
        triad.ka = -(dcax * dabx + dcay * daby);
        triad.kb = -(dabx * dbcx + daby * dbcy);
        triad.kc = -(dbcx * dcax + dbcy * dcay);
        triad.s = Vec2.cross(pa, pb) + Vec2.cross(pb, pc) + Vec2.cross(pc, pa);
        system.m_triadCount++;
      }
    }
  }
}
 

public void callback(int a, int b, int c) {
  final Vec2 pa = system.m_positionBuffer.data[a];
  final Vec2 pb = system.m_positionBuffer.data[b];
  final Vec2 pc = system.m_positionBuffer.data[c];
  final float dabx = pa.x - pb.x;
  final float daby = pa.y - pb.y;
  final float dbcx = pb.x - pc.x;
  final float dbcy = pb.y - pc.y;
  final float dcax = pc.x - pa.x;
  final float dcay = pc.y - pa.y;
  float maxDistanceSquared = Settings.maxTriadDistanceSquared * system.m_squaredDiameter;
  if (dabx * dabx + daby * daby < maxDistanceSquared
      && dbcx * dbcx + dbcy * dbcy < maxDistanceSquared
      && dcax * dcax + dcay * dcay < maxDistanceSquared) {
    if (system.m_triadCount >= system.m_triadCapacity) {
      int oldCapacity = system.m_triadCapacity;
      int newCapacity =
          system.m_triadCount != 0
              ? 2 * system.m_triadCount
              : Settings.minParticleBufferCapacity;
      system.m_triadBuffer =
          BufferUtils.reallocateBuffer(Triad.class, system.m_triadBuffer, oldCapacity,
              newCapacity);
      system.m_triadCapacity = newCapacity;
    }
    Triad triad = system.m_triadBuffer[system.m_triadCount];
    triad.indexA = a;
    triad.indexB = b;
    triad.indexC = c;
    triad.flags =
        system.m_flagsBuffer.data[a] | system.m_flagsBuffer.data[b]
            | system.m_flagsBuffer.data[c];
    triad.strength = def.strength;
    final float midPointx = (float) 1 / 3 * (pa.x + pb.x + pc.x);
    final float midPointy = (float) 1 / 3 * (pa.y + pb.y + pc.y);
    triad.pa.x = pa.x - midPointx;
    triad.pa.y = pa.y - midPointy;
    triad.pb.x = pb.x - midPointx;
    triad.pb.y = pb.y - midPointy;
    triad.pc.x = pc.x - midPointx;
    triad.pc.y = pc.y - midPointy;
    triad.ka = -(dcax * dabx + dcay * daby);
    triad.kb = -(dabx * dbcx + daby * dbcy);
    triad.kc = -(dbcx * dcax + dbcy * dcay);
    triad.s = Vec2.cross(pa, pb) + Vec2.cross(pb, pc) + Vec2.cross(pc, pa);
    system.m_triadCount++;
  }
}
 

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