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

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

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

    // Cdot = dot(u, v + cross(w, r))
    Vec2.crossToOutUnsafe(wA, m_rA, vpA);
    vpA.addLocal(vA);
    Vec2.crossToOutUnsafe(wB, m_rB, vpB);
    vpB.addLocal(vB);
    float Cdot = Vec2.dot(m_u, vpB.subLocal(vpA));

    float impulse = -m_mass * (Cdot + m_bias + m_gamma * m_impulse);
    m_impulse += impulse;


    float Px = impulse * m_u.x;
    float Py = 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);

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

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

private void launchBomb(Vec2 position, Vec2 velocity) {
  if (bomb != null) {
    m_world.destroyBody(bomb);
    bomb = null;
  }
  // todo optimize this
  BodyDef bd = new BodyDef();
  bd.type = BodyType.DYNAMIC;
  bd.position.set(position);
  bd.bullet = true;
  bomb = m_world.createBody(bd);
  bomb.setLinearVelocity(velocity);

  CircleShape circle = new CircleShape();
  circle.m_radius = 0.3f;

  FixtureDef fd = new FixtureDef();
  fd.shape = circle;
  fd.density = 20f;
  fd.restitution = 0;

  Vec2 minV = new Vec2(position);
  Vec2 maxV = new Vec2(position);

  minV.subLocal(new Vec2(.3f, .3f));
  maxV.addLocal(new Vec2(.3f, .3f));

  aabb.lowerBound.set(minV);
  aabb.upperBound.set(maxV);

  bomb.createFixture(fd);
}
 

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

  // Cdot = dot(u, v + cross(w, r))
  Vec2 vpA = pool.popVec2();
  Vec2 vpB = pool.popVec2();
  Vec2 temp = pool.popVec2();

  Vec2.crossToOutUnsafe(wA, m_rA, vpA);
  vpA.addLocal(vA);
  Vec2.crossToOutUnsafe(wB, m_rB, vpB);
  vpB.addLocal(vB);

  float C = m_length - m_maxLength;
  float Cdot = Vec2.dot(m_u, temp.set(vpB).subLocal(vpA));

  // Predictive constraint.
  if (C < 0.0f) {
    Cdot += data.step.inv_dt * C;
  }

  float impulse = -m_mass * Cdot;
  float oldImpulse = m_impulse;
  m_impulse = MathUtils.min(0.0f, m_impulse + impulse);
  impulse = m_impulse - oldImpulse;

  float Px = impulse * m_u.x;
  float Py = 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);

  pool.pushVec2(3);

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

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