Java Code Examples for org.jbox2d.common.Settings#linearSlop()

/**
 * Create a loop. This automatically adjusts connectivity.
 * 
 * @param vertices an array of vertices, these are copied
 * @param count the vertex count
 */
public void createLoop(final Vec2[] vertices, int count) {
  assert (m_vertices == null && m_count == 0);
  assert (count >= 3);
  m_count = count + 1;
  m_vertices = new Vec2[m_count];
  for (int i = 1; i < count; i++) {
    Vec2 v1 = vertices[i - 1];
    Vec2 v2 = vertices[i];
    // If the code crashes here, it means your vertices are too close together.
    if (MathUtils.distanceSquared(v1, v2) < Settings.linearSlop * Settings.linearSlop) {
      throw new RuntimeException("Vertices of chain shape are too close together");
    }
  }
  for (int i = 0; i < count; i++) {
    m_vertices[i] = new Vec2(vertices[i]);
  }
  m_vertices[count] = new Vec2(m_vertices[0]);
  m_prevVertex.set(m_vertices[m_count - 2]);
  m_nextVertex.set(m_vertices[1]);
  m_hasPrevVertex = true;
  m_hasNextVertex = true;
}
 

/**
 * Create a chain with isolated end vertices.
 * 
 * @param vertices an array of vertices, these are copied
 * @param count the vertex count
 */
public void createChain(final Vec2 vertices[], int count) {
  assert (m_vertices == null && m_count == 0);
  assert (count >= 2);
  m_count = count;
  m_vertices = new Vec2[m_count];
  for (int i = 1; i < m_count; i++) {
    Vec2 v1 = vertices[i - 1];
    Vec2 v2 = vertices[i];
    // If the code crashes here, it means your vertices are too close together.
    if (MathUtils.distanceSquared(v1, v2) < Settings.linearSlop * Settings.linearSlop) {
      throw new RuntimeException("Vertices of chain shape are too close together");
    }
  }
  for (int i = 0; i < m_count; i++) {
    m_vertices[i] = new Vec2(vertices[i]);
  }
  m_hasPrevVertex = false;
  m_hasNextVertex = false;

  m_prevVertex.setZero();
  m_nextVertex.setZero();
}
 

@Override
public void preSolve(Contact contact, Manifold oldManifold) {
  super.preSolve(contact, oldManifold);

  Fixture fixtureA = contact.getFixtureA();
  Fixture fixtureB = contact.getFixtureB();

  if (fixtureA != m_platform && fixtureA != m_character) {
    return;
  }

  if (fixtureB != m_character && fixtureB != m_character) {
    return;
  }

  Vec2 position = m_character.getBody().getPosition();

  if (position.y < m_top + m_radius - 3.0f * Settings.linearSlop) {
    contact.setEnabled(false);
  }
}
 

@Override
public boolean reportFixture(Fixture fixture) {
  if (fixture.isSensor()) {
    return true;
  }
  final Shape shape = fixture.getShape();
  Body body = fixture.getBody();
  int childCount = shape.getChildCount();
  for (int childIndex = 0; childIndex < childCount; childIndex++) {
    AABB aabb = fixture.getAABB(childIndex);
    final float aabblowerBoundx = aabb.lowerBound.x - system.m_particleDiameter;
    final float aabblowerBoundy = aabb.lowerBound.y - system.m_particleDiameter;
    final float aabbupperBoundx = aabb.upperBound.x + system.m_particleDiameter;
    final float aabbupperBoundy = aabb.upperBound.y + system.m_particleDiameter;
    int firstProxy =
        lowerBound(
            system.m_proxyBuffer,
            system.m_proxyCount,
            computeTag(system.m_inverseDiameter * aabblowerBoundx, system.m_inverseDiameter
                * aabblowerBoundy));
    int lastProxy =
        upperBound(
            system.m_proxyBuffer,
            system.m_proxyCount,
            computeTag(system.m_inverseDiameter * aabbupperBoundx, system.m_inverseDiameter
                * aabbupperBoundy));

    for (int proxy = firstProxy; proxy != lastProxy; ++proxy) {
      int a = system.m_proxyBuffer[proxy].index;
      Vec2 ap = system.m_positionBuffer.data[a];
      if (aabblowerBoundx <= ap.x && ap.x <= aabbupperBoundx && aabblowerBoundy <= ap.y
          && ap.y <= aabbupperBoundy) {
        Vec2 av = system.m_velocityBuffer.data[a];
        final Vec2 temp = tempVec;
        Transform.mulTransToOutUnsafe(body.m_xf0, ap, temp);
        Transform.mulToOutUnsafe(body.m_xf, temp, input.p1);
        input.p2.x = ap.x + step.dt * av.x;
        input.p2.y = ap.y + step.dt * av.y;
        input.maxFraction = 1;
        if (fixture.raycast(output, input, childIndex)) {
          final Vec2 p = tempVec;
          p.x =
              (1 - output.fraction) * input.p1.x + output.fraction * input.p2.x
                  + Settings.linearSlop * output.normal.x;
          p.y =
              (1 - output.fraction) * input.p1.y + output.fraction * input.p2.y
                  + Settings.linearSlop * output.normal.y;

          final float vx = step.inv_dt * (p.x - ap.x);
          final float vy = step.inv_dt * (p.y - ap.y);
          av.x = vx;
          av.y = vy;
          final float particleMass = system.getParticleMass();
          final float ax = particleMass * (av.x - vx);
          final float ay = particleMass * (av.y - vy);
          Vec2 b = output.normal;
          final float fdn = ax * b.x + ay * b.y;
          final Vec2 f = tempVec2;
          f.x = fdn * b.x;
          f.y = fdn * b.y;
          body.applyLinearImpulse(f, p, true);
        }
      }
    }
  }
  return true;
}
 

/**
 * Sequential solver.
 */
public final boolean solvePositionConstraints() {
  float minSeparation = 0.0f;

  for (int i = 0; i < m_count; ++i) {
    ContactPositionConstraint pc = m_positionConstraints[i];

    int indexA = pc.indexA;
    int indexB = pc.indexB;

    float mA = pc.invMassA;
    float iA = pc.invIA;
    Vec2 localCenterA = pc.localCenterA;
    final float localCenterAx = localCenterA.x;
    final float localCenterAy = localCenterA.y;
    float mB = pc.invMassB;
    float iB = pc.invIB;
    Vec2 localCenterB = pc.localCenterB;
    final float localCenterBx = localCenterB.x;
    final float localCenterBy = localCenterB.y;
    int pointCount = pc.pointCount;

    Vec2 cA = m_positions[indexA].c;
    float aA = m_positions[indexA].a;
    Vec2 cB = m_positions[indexB].c;
    float aB = m_positions[indexB].a;

    // Solve normal constraints
    for (int j = 0; j < pointCount; ++j) {
      final Rot xfAq = xfA.q;
      final Rot xfBq = xfB.q;
      xfAq.set(aA);
      xfBq.set(aB);
      xfA.p.x = cA.x - xfAq.c * localCenterAx + xfAq.s * localCenterAy;
      xfA.p.y = cA.y - xfAq.s * localCenterAx - xfAq.c * localCenterAy;
      xfB.p.x = cB.x - xfBq.c * localCenterBx + xfBq.s * localCenterBy;
      xfB.p.y = cB.y - xfBq.s * localCenterBx - xfBq.c * localCenterBy;

      final PositionSolverManifold psm = psolver;
      psm.initialize(pc, xfA, xfB, j);
      final Vec2 normal = psm.normal;
      final Vec2 point = psm.point;
      final float separation = psm.separation;

      float rAx = point.x - cA.x;
      float rAy = point.y - cA.y;
      float rBx = point.x - cB.x;
      float rBy = point.y - cB.y;

      // Track max constraint error.
      minSeparation = MathUtils.min(minSeparation, separation);

      // Prevent large corrections and allow slop.
      final float C =
          MathUtils.clamp(Settings.baumgarte * (separation + Settings.linearSlop),
              -Settings.maxLinearCorrection, 0.0f);

      // Compute the effective mass.
      final float rnA = rAx * normal.y - rAy * normal.x;
      final float rnB = rBx * normal.y - rBy * normal.x;
      final float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;

      // Compute normal impulse
      final float impulse = K > 0.0f ? -C / K : 0.0f;

      float Px = normal.x * impulse;
      float Py = normal.y * impulse;
      
      cA.x -= Px * mA;
      cA.y -= Py * mA;
      aA -= iA * (rAx * Py - rAy * Px);

      cB.x += Px * mB;
      cB.y += Py * mB;
      aB += iB * (rBx * Py - rBy * Px);
    }

    // m_positions[indexA].c.set(cA);
    m_positions[indexA].a = aA;

    // m_positions[indexB].c.set(cB);
    m_positions[indexB].a = aB;
  }

  // We can't expect minSpeparation >= -linearSlop because we don't
  // push the separation above -linearSlop.
  return minSeparation >= -3.0f * Settings.linearSlop;
}
 

public boolean solveTOIPositionConstraints(int toiIndexA, int toiIndexB) {
  float minSeparation = 0.0f;

  for (int i = 0; i < m_count; ++i) {
    ContactPositionConstraint pc = m_positionConstraints[i];

    int indexA = pc.indexA;
    int indexB = pc.indexB;
    Vec2 localCenterA = pc.localCenterA;
    Vec2 localCenterB = pc.localCenterB;
    final float localCenterAx = localCenterA.x;
    final float localCenterAy = localCenterA.y;
    final float localCenterBx = localCenterB.x;
    final float localCenterBy = localCenterB.y;
    int pointCount = pc.pointCount;

    float mA = 0.0f;
    float iA = 0.0f;
    if (indexA == toiIndexA || indexA == toiIndexB) {
      mA = pc.invMassA;
      iA = pc.invIA;
    }

    float mB = 0f;
    float iB = 0f;
    if (indexB == toiIndexA || indexB == toiIndexB) {
      mB = pc.invMassB;
      iB = pc.invIB;
    }

    Vec2 cA = m_positions[indexA].c;
    float aA = m_positions[indexA].a;

    Vec2 cB = m_positions[indexB].c;
    float aB = m_positions[indexB].a;

    // Solve normal constraints
    for (int j = 0; j < pointCount; ++j) {
      final Rot xfAq = xfA.q;
      final Rot xfBq = xfB.q;
      xfAq.set(aA);
      xfBq.set(aB);
      xfA.p.x = cA.x - xfAq.c * localCenterAx + xfAq.s * localCenterAy;
      xfA.p.y = cA.y - xfAq.s * localCenterAx - xfAq.c * localCenterAy;
      xfB.p.x = cB.x - xfBq.c * localCenterBx + xfBq.s * localCenterBy;
      xfB.p.y = cB.y - xfBq.s * localCenterBx - xfBq.c * localCenterBy;

      final PositionSolverManifold psm = psolver;
      psm.initialize(pc, xfA, xfB, j);
      Vec2 normal = psm.normal;

      Vec2 point = psm.point;
      float separation = psm.separation;

      float rAx = point.x - cA.x;
      float rAy = point.y - cA.y;
      float rBx = point.x - cB.x;
      float rBy = point.y - cB.y;

      // Track max constraint error.
      minSeparation = MathUtils.min(minSeparation, separation);

      // Prevent large corrections and allow slop.
      float C =
          MathUtils.clamp(Settings.toiBaugarte * (separation + Settings.linearSlop),
              -Settings.maxLinearCorrection, 0.0f);

      // Compute the effective mass.
      float rnA = rAx * normal.y - rAy * normal.x;
      float rnB = rBx * normal.y - rBy * normal.x;
      float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;

      // Compute normal impulse
      float impulse = K > 0.0f ? -C / K : 0.0f;

      float Px = normal.x * impulse;
      float Py = normal.y * impulse;
      
      cA.x -= Px * mA;
      cA.y -= Py * mA;
      aA -= iA * (rAx * Py - rAy * Px);

      cB.x += Px * mB;
      cB.y += Py * mB;
      aB += iB * (rBx * Py - rBy * Px);
    }

    // m_positions[indexA].c.set(cA);
    m_positions[indexA].a = aA;

    // m_positions[indexB].c.set(cB);
    m_positions[indexB].a = aB;
  }

  // We can't expect minSpeparation >= -_linearSlop because we don't
  // push the separation above -_linearSlop.
  return minSeparation >= -1.5f * 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();

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

@Override
  public boolean solvePositionConstraints(final SolverData data) {
    if (m_frequencyHz > 0.0f) {
      return true;
    }
    final Rot qA = pool.popRot();
    final Rot qB = pool.popRot();
    final Vec2 rA = pool.popVec2();
    final Vec2 rB = pool.popVec2();
    final Vec2 u = 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, u.set(m_localAnchorA).subLocal(m_localCenterA), rA);
    Rot.mulToOutUnsafe(qB, u.set(m_localAnchorB).subLocal(m_localCenterB), rB);
    u.set(cB).addLocal(rB).subLocal(cA).subLocal(rA);


    float length = u.normalize();
    float C = length - m_length;
    C = MathUtils.clamp(C, -Settings.maxLinearCorrection, Settings.maxLinearCorrection);

    float impulse = -m_mass * C;
    float Px = impulse * u.x;
    float Py = impulse * u.y;

    cA.x -= m_invMassA * Px;
    cA.y -= m_invMassA * Py;
    aA -= m_invIA * (rA.x * Py - rA.y * Px);
    cB.x += m_invMassB * Px;
    cB.y += m_invMassB * Py;
    aB += m_invIB * (rB.x * Py - rB.y * Px);

//    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(3);
    pool.pushRot(2);

    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 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 u = pool.popVec2();
  final Vec2 rA = pool.popVec2();
  final Vec2 rB = pool.popVec2();
  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), rA);
  Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB);
  u.set(cB).addLocal(rB).subLocal(cA).subLocal(rA);

  float length = u.normalize();
  float C = length - m_maxLength;

  C = MathUtils.clamp(C, 0.0f, Settings.maxLinearCorrection);

  float impulse = -m_mass * C;
  float Px = impulse * u.x;
  float Py = impulse * u.y;

  cA.x -= m_invMassA * Px;
  cA.y -= m_invMassA * Py;
  aA -= m_invIA * (rA.x * Py - rA.y * Px);
  cB.x += m_invMassB * Px;
  cB.y += m_invMassB * Py;
  aB += m_invIB * (rB.x * Py - rB.y * Px);

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

  // 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 length - m_maxLength < Settings.linearSlop;
}
 

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