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

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 supporting vertex index in the given direction.
 * 
 * @param d
 * @return
 */
public final int getSupport(final Vec2 d) {
  int bestIndex = 0;
  float bestValue = Vec2.dot(m_vertices[0], d);
  for (int i = 1; i < m_count; i++) {
    float value = Vec2.dot(m_vertices[i], d);
    if (value > bestValue) {
      bestIndex = i;
      bestValue = value;
    }
  }

  return bestIndex;
}
 

/**
 * Get the supporting vertex in the given direction.
 * 
 * @param d
 * @return
 */
public final Vec2 getSupportVertex(final Vec2 d) {
  int bestIndex = 0;
  float bestValue = Vec2.dot(m_vertices[0], d);
  for (int i = 1; i < m_count; i++) {
    float value = Vec2.dot(m_vertices[i], d);
    if (value > bestValue) {
      bestIndex = i;
      bestValue = value;
    }
  }

  return m_vertices[bestIndex];
}
 

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

public float getJointTranslation() {
  Vec2 pA = pool.popVec2(), pB = pool.popVec2(), axis = pool.popVec2();
  m_bodyA.getWorldPointToOut(m_localAnchorA, pA);
  m_bodyB.getWorldPointToOut(m_localAnchorB, pB);
  m_bodyA.getWorldVectorToOutUnsafe(m_localXAxisA, axis);
  pB.subLocal(pA);
  float translation = Vec2.dot(pB, axis);
  pool.pushVec2(3);
  return translation;
}
 

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

/**
 * Set the linear velocity of the center of mass.
 * 
 * @param v the new linear velocity of the center of mass.
 */
public final void setLinearVelocity(Vec2 v) {
  if (m_type == BodyType.STATIC) {
    return;
  }

  if (Vec2.dot(v, v) > 0.0f) {
    setAwake(true);
  }

  m_linearVelocity.set(v);
}
 

/**
 * Clipping for contact manifolds. Sutherland-Hodgman clipping.
 * 
 * @param vOut
 * @param vIn
 * @param normal
 * @param offset
 * @return
 */
public static final int clipSegmentToLine(final ClipVertex[] vOut, final ClipVertex[] vIn,
    final Vec2 normal, float offset, int vertexIndexA) {

  // Start with no output points
  int numOut = 0;
  final ClipVertex vIn0 = vIn[0];
  final ClipVertex vIn1 = vIn[1];
  final Vec2 vIn0v = vIn0.v;
  final Vec2 vIn1v = vIn1.v;

  // Calculate the distance of end points to the line
  float distance0 = Vec2.dot(normal, vIn0v) - offset;
  float distance1 = Vec2.dot(normal, vIn1v) - offset;

  // If the points are behind the plane
  if (distance0 <= 0.0f) {
    vOut[numOut++].set(vIn0);
  }
  if (distance1 <= 0.0f) {
    vOut[numOut++].set(vIn1);
  }

  // If the points are on different sides of the plane
  if (distance0 * distance1 < 0.0f) {
    // Find intersection point of edge and plane
    float interp = distance0 / (distance0 - distance1);

    ClipVertex vOutNO = vOut[numOut];
    // vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
    vOutNO.v.x = vIn0v.x + interp * (vIn1v.x - vIn0v.x);
    vOutNO.v.y = vIn0v.y + interp * (vIn1v.y - vIn0v.y);

    // VertexA is hitting edgeB.
    vOutNO.id.indexA = (byte) vertexIndexA;
    vOutNO.id.indexB = vIn0.id.indexB;
    vOutNO.id.typeA = (byte) ContactID.Type.VERTEX.ordinal();
    vOutNO.id.typeB = (byte) ContactID.Type.FACE.ordinal();
    ++numOut;
  }

  return numOut;
}
 

public void computePolygonSeparation(EPAxis axis) {
  axis.type = EPAxis.Type.UNKNOWN;
  axis.index = -1;
  axis.separation = -Float.MAX_VALUE;

  perp.x = -m_normal.y;
  perp.y = m_normal.x;

  for (int i = 0; i < m_polygonB.count; ++i) {
    Vec2 normalB = m_polygonB.normals[i];
    Vec2 vB = m_polygonB.vertices[i];
    n.x = -normalB.x;
    n.y = -normalB.y;

    // float s1 = Vec2.dot(n, temp.set(vB).subLocal(m_v1));
    // float s2 = Vec2.dot(n, temp.set(vB).subLocal(m_v2));
    float tempx = vB.x - m_v1.x;
    float tempy = vB.y - m_v1.y;
    float s1 = n.x * tempx + n.y * tempy;
    tempx = vB.x - m_v2.x;
    tempy = vB.y - m_v2.y;
    float s2 = n.x * tempx + n.y * tempy;
    float s = MathUtils.min(s1, s2);

    if (s > m_radius) {
      // No collision
      axis.type = EPAxis.Type.EDGE_B;
      axis.index = i;
      axis.separation = s;
      return;
    }

    // Adjacency
    if (n.x * perp.x + n.y * perp.y >= 0.0f) {
      if (Vec2.dot(temp.set(n).subLocal(m_upperLimit), m_normal) < -Settings.angularSlop) {
        continue;
      }
    } else {
      if (Vec2.dot(temp.set(n).subLocal(m_lowerLimit), m_normal) < -Settings.angularSlop) {
        continue;
      }
    }

    if (s > axis.separation) {
      axis.type = EPAxis.Type.EDGE_B;
      axis.index = i;
      axis.separation = s;
    }
  }
}
 

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

/**
 * Solve a line segment using barycentric coordinates.
 */
public void solve2() {
  // Solve a line segment using barycentric coordinates.
  //
  // p = a1 * w1 + a2 * w2
  // a1 + a2 = 1
  //
  // The vector from the origin to the closest point on the line is
  // perpendicular to the line.
  // e12 = w2 - w1
  // dot(p, e) = 0
  // a1 * dot(w1, e) + a2 * dot(w2, e) = 0
  //
  // 2-by-2 linear system
  // [1 1 ][a1] = [1]
  // [w1.e12 w2.e12][a2] = [0]
  //
  // Define
  // d12_1 = dot(w2, e12)
  // d12_2 = -dot(w1, e12)
  // d12 = d12_1 + d12_2
  //
  // Solution
  // a1 = d12_1 / d12
  // a2 = d12_2 / d12
  final Vec2 w1 = m_v1.w;
  final Vec2 w2 = m_v2.w;
  e12.set(w2).subLocal(w1);

  // w1 region
  float d12_2 = -Vec2.dot(w1, e12);
  if (d12_2 <= 0.0f) {
    // a2 <= 0, so we clamp it to 0
    m_v1.a = 1.0f;
    m_count = 1;
    return;
  }

  // w2 region
  float d12_1 = Vec2.dot(w2, e12);
  if (d12_1 <= 0.0f) {
    // a1 <= 0, so we clamp it to 0
    m_v2.a = 1.0f;
    m_count = 1;
    m_v1.set(m_v2);
    return;
  }

  // Must be in e12 region.
  float inv_d12 = 1.0f / (d12_1 + d12_2);
  m_v1.a = d12_1 * inv_d12;
  m_v2.a = d12_2 * inv_d12;
  m_count = 2;
}
 

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

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

@Override
public void solveVelocityConstraints(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;
  Vec2 vC = data.velocities[m_indexC].v;
  float wC = data.velocities[m_indexC].w;
  Vec2 vD = data.velocities[m_indexD].v;
  float wD = data.velocities[m_indexD].w;

  Vec2 temp1 = pool.popVec2();
  Vec2 temp2 = pool.popVec2();
  float Cdot =
      Vec2.dot(m_JvAC, temp1.set(vA).subLocal(vC)) + Vec2.dot(m_JvBD, temp2.set(vB).subLocal(vD));
  Cdot += (m_JwA * wA - m_JwC * wC) + (m_JwB * wB - m_JwD * wD);
  pool.pushVec2(2);

  float impulse = -m_mass * Cdot;
  m_impulse += impulse;

  vA.x += (m_mA * impulse) * m_JvAC.x;
  vA.y += (m_mA * impulse) * m_JvAC.y;
  wA += m_iA * impulse * m_JwA;

  vB.x += (m_mB * impulse) * m_JvBD.x;
  vB.y += (m_mB * impulse) * m_JvBD.y;
  wB += m_iB * impulse * m_JwB;

  vC.x -= (m_mC * impulse) * m_JvAC.x;
  vC.y -= (m_mC * impulse) * m_JvAC.y;
  wC -= m_iC * impulse * m_JwC;

  vD.x -= (m_mD * impulse) * m_JvBD.x;
  vD.y -= (m_mD * impulse) * m_JvBD.y;
  wD -= m_iD * impulse * m_JwD;


  // data.velocities[m_indexA].v = vA;
  data.velocities[m_indexA].w = wA;
  // data.velocities[m_indexB].v = vB;
  data.velocities[m_indexB].w = wB;
  // data.velocities[m_indexC].v = vC;
  data.velocities[m_indexC].w = wC;
  // data.velocities[m_indexD].v = vD;
  data.velocities[m_indexD].w = wD;
}
 

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

/**
 * Set the mass properties to override the mass properties of the fixtures. Note that this changes
 * the center of mass position. Note that creating or destroying fixtures can also alter the mass.
 * This function has no effect if the body isn't dynamic.
 * 
 * @param massData the mass properties.
 */
public final void setMassData(MassData massData) {
  // TODO_ERIN adjust linear velocity and torque to account for movement of center.
  assert (m_world.isLocked() == false);
  if (m_world.isLocked() == true) {
    return;
  }

  if (m_type != BodyType.DYNAMIC) {
    return;
  }

  m_invMass = 0.0f;
  m_I = 0.0f;
  m_invI = 0.0f;

  m_mass = massData.mass;
  if (m_mass <= 0.0f) {
    m_mass = 1f;
  }

  m_invMass = 1.0f / m_mass;

  if (massData.I > 0.0f && (m_flags & e_fixedRotationFlag) == 0) {
    m_I = massData.I - m_mass * Vec2.dot(massData.center, massData.center);
    assert (m_I > 0.0f);
    m_invI = 1.0f / m_I;
  }

  final Vec2 oldCenter = m_world.getPool().popVec2();
  // Move center of mass.
  oldCenter.set(m_sweep.c);
  m_sweep.localCenter.set(massData.center);
  // m_sweep.c0 = m_sweep.c = Mul(m_xf, m_sweep.localCenter);
  Transform.mulToOutUnsafe(m_xf, m_sweep.localCenter, m_sweep.c0);
  m_sweep.c.set(m_sweep.c0);

  // Update center of mass velocity.
  // m_linearVelocity += Cross(m_angularVelocity, m_sweep.c - oldCenter);
  final Vec2 temp = m_world.getPool().popVec2();
  temp.set(m_sweep.c).subLocal(oldCenter);
  Vec2.crossToOut(m_angularVelocity, temp, temp);
  m_linearVelocity.addLocal(temp);

  m_world.getPool().pushVec2(2);
}