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

public float getJointTranslation() {
  Body b1 = m_bodyA;
  Body b2 = m_bodyB;

  Vec2 p1 = pool.popVec2();
  Vec2 p2 = pool.popVec2();
  Vec2 axis = pool.popVec2();
  b1.getWorldPointToOut(m_localAnchorA, p1);
  b2.getWorldPointToOut(m_localAnchorA, p2);
  p2.subLocal(p1);
  b1.getWorldVectorToOut(m_localXAxisA, axis);

  float translation = Vec2.dot(p2, axis);
  pool.pushVec2(3);
  return translation;
}
 

/**
 * 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 getCurrentLengthA() {
  final Vec2 p = pool.popVec2();
  m_bodyA.getWorldPointToOut(m_localAnchorA, p);
  p.subLocal(m_groundAnchorA);
  float length = p.length();
  pool.pushVec2(1);
  return length;
}
 

public float getCurrentLengthB() {
  final Vec2 p = pool.popVec2();
  m_bodyB.getWorldPointToOut(m_localAnchorB, p);
  p.subLocal(m_groundAnchorB);
  float length = p.length();
  pool.pushVec2(1);
  return length;
}
 

public float getLength1() {
  final Vec2 p = pool.popVec2();
  m_bodyA.getWorldPointToOut(m_localAnchorA, p);
  p.subLocal(m_groundAnchorA);

  float len = p.length();
  pool.pushVec2(1);
  return len;
}
 

public float getLength2() {
  final Vec2 p = pool.popVec2();
  m_bodyB.getWorldPointToOut(m_localAnchorB, p);
  p.subLocal(m_groundAnchorB);

  float len = p.length();
  pool.pushVec2(1);
  return len;
}
 

public void testVec2() {
  Vec2 v = new Vec2();
  v.x = 0;
  v.y = 1;
  v.subLocal(new Vec2(10, 10));
  assertEquals(-10f, v.x);
  assertEquals(-9f, v.y);

  Vec2 v2 = v.add(new Vec2(1, 1));
  assertEquals(-9f, v2.x);
  assertEquals(-8f, v2.y);
  assertFalse(v.equals(v2));
}
 

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

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