org.jbox2d.common.Settings Java Examples

@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 final int createProxy(final AABB aabb, Object userData) {
  assert(aabb.isValid());
  final DynamicTreeNode node = allocateNode();
  int proxyId = node.id;
  // Fatten the aabb
  final AABB nodeAABB = node.aabb;
  nodeAABB.lowerBound.x = aabb.lowerBound.x - Settings.aabbExtension;
  nodeAABB.lowerBound.y = aabb.lowerBound.y - Settings.aabbExtension;
  nodeAABB.upperBound.x = aabb.upperBound.x + Settings.aabbExtension;
  nodeAABB.upperBound.y = aabb.upperBound.y + Settings.aabbExtension;
  node.userData = userData;

  insertLeaf(proxyId);

  return proxyId;
}
 

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

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

@Override
public void step(TestbedSettings settings) {
  // TODO Auto-generated method stub
  super.step(settings);

  // Traverse the contact results. Apply a force on shapes
  // that overlap the sensor.
  for (int i = 0; i < e_count; ++i) {
    if (m_touching[i].tf == false) {
      continue;
    }

    Body body = m_bodies[i];
    Body ground = m_sensor.getBody();

    CircleShape circle = (CircleShape) m_sensor.getShape();
    Vec2 center = ground.getWorldPoint(circle.m_p);

    Vec2 position = body.getPosition();

    Vec2 d = center.sub(position);
    if (d.lengthSquared() < Settings.EPSILON * Settings.EPSILON) {
      continue;
    }

    d.normalize();
    Vec2 F = d.mulLocal(100f);
    body.applyForce(F, position);
  }
}
 

/**
 * Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors.
 */
public void initialize(Body b1, Body b2, Vec2 ga1, Vec2 ga2, Vec2 anchor1, Vec2 anchor2, float r) {
  bodyA = b1;
  bodyB = b2;
  groundAnchorA = ga1;
  groundAnchorB = ga2;
  localAnchorA = bodyA.getLocalPoint(anchor1);
  localAnchorB = bodyB.getLocalPoint(anchor2);
  Vec2 d1 = anchor1.sub(ga1);
  lengthA = d1.length();
  Vec2 d2 = anchor2.sub(ga2);
  lengthB = d2.length();
  ratio = r;
  assert (ratio > Settings.EPSILON);
}
 

public TestShape() {

            m_count = 0;
            m_vertices = new Vec2[Settings.maxPolygonVertices];
            for (int i = 0; i < m_vertices.length; i++) {
                m_vertices[i] = new Vec2();
            }
            m_normals = new Vec2[Settings.maxPolygonVertices];
            for (int i = 0; i < m_normals.length; i++) {
                m_normals[i] = new Vec2();
            }
            m_centroid.setZero();
        }
 

public void addContact(int a, int b) {
    assert(a != b);
    Vec2 pa = m_positionBuffer.data[a];
    Vec2 pb = m_positionBuffer.data[b];
    float dx = pb.x - pa.x;
    float dy = pb.y - pa.y;
    float d2 = dx * dx + dy * dy;
//    assert(d2 != 0);
    if (d2 < m_squaredDiameter) {
      if (m_contactCount >= m_contactCapacity) {
        int oldCapacity = m_contactCapacity;
        int newCapacity =
            m_contactCount != 0 ? 2 * m_contactCount : Settings.minParticleBufferCapacity;
        m_contactBuffer =
            BufferUtils.reallocateBuffer(ParticleContact.class, m_contactBuffer, oldCapacity,
                newCapacity);
        m_contactCapacity = newCapacity;
      }
      float invD = d2 != 0 ? MathUtils.sqrt(1 / d2) : Float.MAX_VALUE;
      ParticleContact contact = m_contactBuffer[m_contactCount];
      contact.indexA = a;
      contact.indexB = b;
      contact.flags = m_flagsBuffer.data[a] | m_flagsBuffer.data[b];
      contact.weight = 1 - d2 * invD * m_inverseDiameter;
      contact.normal.x = invD * dx;
      contact.normal.y = invD * dy;
      m_contactCount++;
    }
  }
 

/**
 * Creates this manifold as a copy of the other
 * 
 * @param other
 */
public Manifold(Manifold other) {
  points = new ManifoldPoint[Settings.maxManifoldPoints];
  localNormal = other.localNormal.clone();
  localPoint = other.localPoint.clone();
  pointCount = other.pointCount;
  type = other.type;
  // djm: this is correct now
  for (int i = 0; i < Settings.maxManifoldPoints; i++) {
    points[i] = new ManifoldPoint(other.points[i]);
  }
}
 

/**
 * creates a manifold with 0 points, with it's points array full of instantiated ManifoldPoints.
 */
public Manifold() {
  points = new ManifoldPoint[Settings.maxManifoldPoints];
  for (int i = 0; i < Settings.maxManifoldPoints; i++) {
    points[i] = new ManifoldPoint();
  }
  localNormal = new Vec2();
  localPoint = new Vec2();
  pointCount = 0;
}
 

public WorldManifold() {
  normal = new Vec2();
  points = new Vec2[Settings.maxManifoldPoints];
  separations = new float[Settings.maxManifoldPoints];
  for (int i = 0; i < Settings.maxManifoldPoints; i++) {
    points[i] = new Vec2();
  }
}
 

@Override
public final int createProxy(final AABB aabb, Object userData) {
  final int node = allocateNode();
  // Fatten the aabb
  final AABB nodeAABB = m_aabb[node];
  nodeAABB.lowerBound.x = aabb.lowerBound.x - Settings.aabbExtension;
  nodeAABB.lowerBound.y = aabb.lowerBound.y - Settings.aabbExtension;
  nodeAABB.upperBound.x = aabb.upperBound.x + Settings.aabbExtension;
  nodeAABB.upperBound.y = aabb.upperBound.y + Settings.aabbExtension;
  m_userData[node] = userData;

  insertLeaf(node);

  return node;
}
 

@Override
public void preStep(int testNum) {
  Settings.FAST_ABS = testNum == 1;
  Settings.FAST_ATAN2 = testNum == 2;
  Settings.FAST_CEIL = testNum == 3;
  Settings.FAST_FLOOR = testNum == 4;
  Settings.FAST_ROUND = testNum == 5;
  Settings.SINCOS_LUT_ENABLED = testNum == 6;

  if (testNum == 7) {
    Settings.FAST_ABS = true;
    Settings.FAST_ATAN2 = true;
    Settings.FAST_CEIL = true;
    Settings.FAST_FLOOR = true;
    Settings.FAST_ROUND = true;
    Settings.SINCOS_LUT_ENABLED = true;
  }

  if (testNum > 7) {
    Settings.FAST_ABS = testNum != 8;
    Settings.FAST_ATAN2 = testNum != 9;
    Settings.FAST_CEIL = testNum != 10;
    Settings.FAST_FLOOR = testNum != 11;
    Settings.FAST_ROUND = testNum != 12;
    Settings.SINCOS_LUT_ENABLED = testNum != 13;
  }
}
 

@Override
public final void computeMass(final MassData massData, final float density) {
  massData.mass = density * Settings.PI * m_radius * m_radius;
  massData.center.x = m_p.x;
  massData.center.y = m_p.y;

  // inertia about the local origin
  // massData.I = massData.mass * (0.5f * m_radius * m_radius + Vec2.dot(m_p, m_p));
  massData.I = massData.mass * (0.5f * m_radius * m_radius + (m_p.x * m_p.x + m_p.y * m_p.y));
}
 

void DrawFixture(Fixture fixture) {
  Color3f color = new Color3f(0.95f, 0.95f, 0.6f);
  final Transform xf = fixture.getBody().getTransform();

  switch (fixture.getType()) {
    case CIRCLE: {
      CircleShape circle = (CircleShape) fixture.getShape();

      Vec2 center = Transform.mul(xf, circle.m_p);
      float radius = circle.m_radius;

      debugDraw.drawCircle(center, radius, color);
    }
      break;

    case POLYGON: {
      PolygonShape poly = (PolygonShape) fixture.getShape();
      int vertexCount = poly.m_count;
      assert (vertexCount <= Settings.maxPolygonVertices);
      Vec2 vertices[] = new Vec2[Settings.maxPolygonVertices];

      for (int i = 0; i < vertexCount; ++i) {
        vertices[i] = Transform.mul(xf, poly.m_vertices[i]);
      }

      debugDraw.drawPolygon(vertices, vertexCount, color);
    }
      break;
    default:
      break;
  }
}
 

public void RayCast() {
	m_rayActor = null;

	RayCastInput input = new RayCastInput();
	input.set(m_rayCastInput);

	// Ray cast against the dynamic tree.
	m_tree.raycast(this, input);

	// Brute force ray cast.
	Actor bruteActor = null;
	RayCastOutput bruteOutput = new RayCastOutput();
	for (int i = 0; i < e_actorCount; ++i) {
		if (m_actors[i].proxyId == -1) {
			continue;
		}

		RayCastOutput output = new RayCastOutput();
		boolean hit = m_actors[i].aabb.raycast(output, input,
				getWorld().getPool());
		if (hit) {
			bruteActor = m_actors[i];
			bruteOutput = output;
			input.maxFraction = output.fraction;
		}
	}

	if (bruteActor != null) {
	  if(MathUtils.abs(bruteOutput.fraction
                   - m_rayCastOutput.fraction) > Settings.EPSILON) {
	    System.out.println("wrong!");
	    assert (MathUtils.abs(bruteOutput.fraction
             - m_rayCastOutput.fraction) <= 20 * Settings.EPSILON);
	  }
		
	}
}
 

/**
 * Determine if two generic shapes overlap.
 * 
 * @param shapeA
 * @param shapeB
 * @param xfA
 * @param xfB
 * @return
 */
public final boolean testOverlap(Shape shapeA, int indexA, Shape shapeB, int indexB,
    Transform xfA, Transform xfB) {
  input.proxyA.set(shapeA, indexA);
  input.proxyB.set(shapeB, indexB);
  input.transformA.set(xfA);
  input.transformB.set(xfB);
  input.useRadii = true;

  cache.count = 0;

  pool.getDistance().distance(output, cache, input);
  // djm note: anything significant about 10.0f?
  return output.distance < 10.0f * Settings.EPSILON;
}
 

public PolygonShape() {
  super(ShapeType.POLYGON);

  m_count = 0;
  m_vertices = new Vec2[Settings.maxPolygonVertices];
  for (int i = 0; i < m_vertices.length; i++) {
    m_vertices[i] = new Vec2();
  }
  m_normals = new Vec2[Settings.maxPolygonVertices];
  for (int i = 0; i < m_normals.length; i++) {
    m_normals[i] = new Vec2();
  }
  setRadius(Settings.polygonRadius);
  m_centroid.setZero();
}
 

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

public DistanceProxy() {
  m_vertices = new Vec2[Settings.maxPolygonVertices];
  for (int i = 0; i < m_vertices.length; i++) {
    m_vertices[i] = new Vec2();
  }
  m_buffer = new Vec2[2];
  m_count = 0;
  m_radius = 0f;
}
 

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

float getParticleStride() {
  return Settings.particleStride * m_particleDiameter;
}
 

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

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