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

public final void getSearchDirection(final Vec2 out) {
  switch (m_count) {
    case 1:
      out.set(m_v1.w).negateLocal();
      return;
    case 2:
      e12.set(m_v2.w).subLocal(m_v1.w);
      // use out for a temp variable real quick
      out.set(m_v1.w).negateLocal();
      float sgn = Vec2.cross(e12, out);

      if (sgn > 0f) {
        // Origin is left of e12.
        Vec2.crossToOutUnsafe(1f, e12, out);
        return;
      } else {
        // Origin is right of e12.
        Vec2.crossToOutUnsafe(e12, 1f, out);
        return;
      }
    default:
      assert (false);
      out.setZero();
      return;
  }
}
 

protected WheelJoint(IWorldPool argPool, WheelJointDef def) {
  super(argPool, def);
  m_localAnchorA.set(def.localAnchorA);
  m_localAnchorB.set(def.localAnchorB);
  m_localXAxisA.set(def.localAxisA);
  Vec2.crossToOutUnsafe(1.0f, m_localXAxisA, m_localYAxisA);


  m_motorMass = 0.0f;
  m_motorImpulse = 0.0f;

  m_maxMotorTorque = def.maxMotorTorque;
  m_motorSpeed = def.motorSpeed;
  m_enableMotor = def.enableMotor;

  m_frequencyHz = def.frequencyHz;
  m_dampingRatio = def.dampingRatio;
}
 

protected PrismaticJoint(IWorldPool argWorld, PrismaticJointDef def) {
  super(argWorld, def);
  m_localAnchorA = new Vec2(def.localAnchorA);
  m_localAnchorB = new Vec2(def.localAnchorB);
  m_localXAxisA = new Vec2(def.localAxisA);
  m_localXAxisA.normalize();
  m_localYAxisA = new Vec2();
  Vec2.crossToOutUnsafe(1f, m_localXAxisA, m_localYAxisA);
  m_referenceAngle = def.referenceAngle;

  m_impulse = new Vec3();
  m_motorMass = 0.0f;
  m_motorImpulse = 0.0f;

  m_lowerTranslation = def.lowerTranslation;
  m_upperTranslation = def.upperTranslation;
  m_maxMotorForce = def.maxMotorForce;
  m_motorSpeed = def.motorSpeed;
  m_enableLimit = def.enableLimit;
  m_enableMotor = def.enableMotor;
  m_limitState = LimitState.INACTIVE;

  m_K = new Mat33();
  m_axis = new Vec2();
  m_perp = new Vec2();
}
 

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

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

/**
 * Get the current joint translation, usually in meters.
 */
public float getJointSpeed() {
  Body bA = m_bodyA;
  Body bB = m_bodyB;

  Vec2 temp = pool.popVec2();
  Vec2 rA = pool.popVec2();
  Vec2 rB = pool.popVec2();
  Vec2 p1 = pool.popVec2();
  Vec2 p2 = pool.popVec2();
  Vec2 d = pool.popVec2();
  Vec2 axis = pool.popVec2();
  Vec2 temp2 = pool.popVec2();
  Vec2 temp3 = pool.popVec2();

  temp.set(m_localAnchorA).subLocal(bA.m_sweep.localCenter);
  Rot.mulToOutUnsafe(bA.m_xf.q, temp, rA);

  temp.set(m_localAnchorB).subLocal(bB.m_sweep.localCenter);
  Rot.mulToOutUnsafe(bB.m_xf.q, temp, rB);

  p1.set(bA.m_sweep.c).addLocal(rA);
  p2.set(bB.m_sweep.c).addLocal(rB);

  d.set(p2).subLocal(p1);
  Rot.mulToOutUnsafe(bA.m_xf.q, m_localXAxisA, axis);

  Vec2 vA = bA.m_linearVelocity;
  Vec2 vB = bB.m_linearVelocity;
  float wA = bA.m_angularVelocity;
  float wB = bB.m_angularVelocity;


  Vec2.crossToOutUnsafe(wA, axis, temp);
  Vec2.crossToOutUnsafe(wB, rB, temp2);
  Vec2.crossToOutUnsafe(wA, rA, temp3);

  temp2.addLocal(vB).subLocal(vA).subLocal(temp3);
  float speed = Vec2.dot(d, temp) + Vec2.dot(axis, temp2);

  pool.pushVec2(9);

  return speed;
}