Java Vec2.cross方法代碼示例

import org.jbox2d.common.Vec2; //導入方法依賴的package包/類
/**
 * Validate convexity. This is a very time consuming operation.
 * 
 * @return
 */
public boolean validate() {
  for (int i = 0; i < m_count; ++i) {
    int i1 = i;
    int i2 = i < m_count - 1 ? i1 + 1 : 0;
    Vec2 p = m_vertices[i1];
    Vec2 e = pool1.set(m_vertices[i2]).subLocal(p);

    for (int j = 0; j < m_count; ++j) {
      if (j == i1 || j == i2) {
        continue;
      }

      Vec2 v = pool2.set(m_vertices[j]).subLocal(p);
      float c = Vec2.cross(e, v);
      if (c < 0.0f) {
        return false;
      }
    }
  }

  return true;
}
 

import org.jbox2d.common.Vec2; //導入方法依賴的package包/類
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;
  }
}
 

import org.jbox2d.common.Vec2; //導入方法依賴的package包/類
public float getMetric() {
  switch (m_count) {
    case 0:
      assert (false);
      return 0.0f;

    case 1:
      return 0.0f;

    case 2:
      return MathUtils.distance(m_v1.w, m_v2.w);

    case 3:
      case3.set(m_v2.w).subLocal(m_v1.w);
      case33.set(m_v3.w).subLocal(m_v1.w);
      // return Vec2.cross(m_v2.w - m_v1.w, m_v3.w - m_v1.w);
      return Vec2.cross(case3, case33);

    default:
      assert (false);
      return 0.0f;
  }
}
 

import org.jbox2d.common.Vec2; //導入方法依賴的package包/類
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);
}
 

import org.jbox2d.common.Vec2; //導入方法依賴的package包/類
@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);
  }
 

import org.jbox2d.common.Vec2; //導入方法依賴的package包/類
@Override
public void solveVelocityConstraints(final SolverData step) {
  float crossMassSum = 0.0f;
  float dotMassSum = 0.0f;

  Velocity[] velocities = step.velocities;
  Position[] positions = step.positions;
  final Vec2 d[] = pool.getVec2Array(bodies.length);

  for (int i = 0; i < bodies.length; ++i) {
    final int prev = (i == 0) ? bodies.length - 1 : i - 1;
    final int next = (i == bodies.length - 1) ? 0 : i + 1;
    d[i].set(positions[bodies[next].m_islandIndex].c);
    d[i].subLocal(positions[bodies[prev].m_islandIndex].c);
    dotMassSum += (d[i].lengthSquared()) / bodies[i].getMass();
    crossMassSum += Vec2.cross(velocities[bodies[i].m_islandIndex].v, d[i]);
  }
  float lambda = -2.0f * crossMassSum / dotMassSum;
  // System.out.println(crossMassSum + " " +dotMassSum);
  // lambda = MathUtils.clamp(lambda, -Settings.maxLinearCorrection,
  // Settings.maxLinearCorrection);
  m_impulse += lambda;
  // System.out.println(m_impulse);
  for (int i = 0; i < bodies.length; ++i) {
    velocities[bodies[i].m_islandIndex].v.x += bodies[i].m_invMass * d[i].y * .5f * lambda;
    velocities[bodies[i].m_islandIndex].v.y += bodies[i].m_invMass * -d[i].x * .5f * lambda;
  }
}
 

import org.jbox2d.common.Vec2; //導入方法依賴的package包/類
@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);
  }
 

import org.jbox2d.common.Vec2; //導入方法依賴的package包/類
public void callback(int a, int b, int c) {
  final Vec2 pa = system.m_positionBuffer.data[a];
  final Vec2 pb = system.m_positionBuffer.data[b];
  final Vec2 pc = system.m_positionBuffer.data[c];
  final float dabx = pa.x - pb.x;
  final float daby = pa.y - pb.y;
  final float dbcx = pb.x - pc.x;
  final float dbcy = pb.y - pc.y;
  final float dcax = pc.x - pa.x;
  final float dcay = pc.y - pa.y;
  float maxDistanceSquared = Settings.maxTriadDistanceSquared * system.m_squaredDiameter;
  if (dabx * dabx + daby * daby < maxDistanceSquared
      && dbcx * dbcx + dbcy * dbcy < maxDistanceSquared
      && dcax * dcax + dcay * dcay < maxDistanceSquared) {
    if (system.m_triadCount >= system.m_triadCapacity) {
      int oldCapacity = system.m_triadCapacity;
      int newCapacity =
          system.m_triadCount != 0
              ? 2 * system.m_triadCount
              : Settings.minParticleBufferCapacity;
      system.m_triadBuffer =
          BufferUtils.reallocateBuffer(Triad.class, system.m_triadBuffer, oldCapacity,
              newCapacity);
      system.m_triadCapacity = newCapacity;
    }
    Triad triad = system.m_triadBuffer[system.m_triadCount];
    triad.indexA = a;
    triad.indexB = b;
    triad.indexC = c;
    triad.flags =
        system.m_flagsBuffer.data[a] | system.m_flagsBuffer.data[b]
            | system.m_flagsBuffer.data[c];
    triad.strength = def.strength;
    final float midPointx = (float) 1 / 3 * (pa.x + pb.x + pc.x);
    final float midPointy = (float) 1 / 3 * (pa.y + pb.y + pc.y);
    triad.pa.x = pa.x - midPointx;
    triad.pa.y = pa.y - midPointy;
    triad.pb.x = pb.x - midPointx;
    triad.pb.y = pb.y - midPointy;
    triad.pc.x = pc.x - midPointx;
    triad.pc.y = pc.y - midPointy;
    triad.ka = -(dcax * dabx + dcay * daby);
    triad.kb = -(dabx * dbcx + daby * dbcy);
    triad.kc = -(dbcx * dcax + dbcy * dcay);
    triad.s = Vec2.cross(pa, pb) + Vec2.cross(pb, pc) + Vec2.cross(pc, pa);
    system.m_triadCount++;
  }
}
 

import org.jbox2d.common.Vec2; //導入方法依賴的package包/類
@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;
}
 

import org.jbox2d.common.Vec2; //導入方法依賴的package包/類
@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);
  }
 

import org.jbox2d.common.Vec2; //導入方法依賴的package包/類
@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;
  }
 

import org.jbox2d.common.Vec2; //導入方法依賴的package包/類
public final void complexlogic(final Vec2[] verts, final int num, final Vec2Array vecPool,
                      final IntArray intPool) {

    int i0 = 1;
    int[] hull = new int[Settings.maxPolygonVertices];

    int m = 0;
    int ih = i0;

    int counter = 0;
    while (true) {

        counter++;
        if (counter>5) {
            break;
        }

        hull[m] = ih;

        int ie = 0;
        for (int j = 1; j < num; ++j) {

            if (ie == ih) {
                ie = j;
                continue;
            }

            Vec2 r = pool1.set(verts[ie]).subLocal(verts[hull[m]]);
            Vec2 v = pool2.set(verts[j]).subLocal(verts[hull[m]]);

            float c = Vec2.cross(r, v);
            if (c < 0.0f) {
                ie = j;
            }

            if (c == 0.0f && v.lengthSquared() > r.lengthSquared()) {
                ie = j;
            }
        }

        ++m;
        ih = ie;

        System.out.println("End of loop");
        System.out.println(ie);
        System.out.println(i0);
        if (ie == i0) {
            break;
        }
    }
    System.out.println("Finished");
}