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

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

/**
 * Get the center of the AABB
 * 
 * @return
 */
public final Vec2 getCenter() {
  final Vec2 center = new Vec2(lowerBound);
  center.addLocal(upperBound);
  center.mulLocal(.5f);
  return center;
}
 

/**
 * Get the extents of the AABB (half-widths).
 * 
 * @return
 */
public final Vec2 getExtents() {
  final Vec2 center = new Vec2(upperBound);
  center.subLocal(lowerBound);
  center.mulLocal(.5f);
  return center;
}
 

public float op(Vec2 argVec) {
  argVec.set(MathUtils.randomFloat(-100, 100), MathUtils.randomFloat(-100, 100));
  argVec.mulLocal(3.2f);
  float s = argVec.length();
  argVec.normalize();
  return s;
}
 

public float op(Vec2 argVec) {
  argVec.set(MathUtils.randomFloat(-100, 100), MathUtils.randomFloat(-100, 100));
  argVec.mulLocal(3.2f);
  float s = argVec.length();
  argVec.normalize();
  return s;
}
 

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

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

@Override
public void getReactionForce(float inv_dt, Vec2 argOut) {
  argOut.set(m_JvAC).mulLocal(m_impulse);
  argOut.mulLocal(inv_dt);
}
 

@Override
public void getReactionForce(float inv_dt, Vec2 argOut) {
  argOut.set(m_impulse.x, m_impulse.y);
  argOut.mulLocal(inv_dt);
}
 

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