Esempio n. 1
0
  // djm pooling from above
  public final void findIncidentEdge(
      final ClipVertex[] c,
      final PolygonShape poly1,
      final Transform xf1,
      int edge1,
      final PolygonShape poly2,
      final Transform xf2) {
    int count1 = poly1.m_vertexCount;
    final Vec2[] normals1 = poly1.m_normals;

    int count2 = poly2.m_vertexCount;
    final Vec2[] vertices2 = poly2.m_vertices;
    final Vec2[] normals2 = poly2.m_normals;

    assert (0 <= edge1 && edge1 < count1);

    // Get the normal of the reference edge in poly2's frame.
    Mat22.mulToOut(xf1.R, normals1[edge1], normal1); // temporary
    // b2Vec2 normal1 = b2MulT(xf2.R, b2Mul(xf1.R, normals1[edge1]));
    Mat22.mulTransToOut(xf2.R, normal1, normal1);

    // Find the incident edge on poly2.
    int index = 0;
    float minDot = Float.MAX_VALUE;
    for (int i = 0; i < count2; ++i) {
      float dot = Vec2.dot(normal1, normals2[i]);
      if (dot < minDot) {
        minDot = dot;
        index = i;
      }
    }

    // Build the clip vertices for the incident edge.
    int i1 = index;
    int i2 = i1 + 1 < count2 ? i1 + 1 : 0;

    Transform.mulToOut(xf2, vertices2[i1], c[0].v); // = Mul(xf2, vertices2[i1]);
    c[0].id.features.referenceEdge = edge1;
    c[0].id.features.incidentEdge = i1;
    c[0].id.features.incidentVertex = 0;

    Transform.mulToOut(xf2, vertices2[i2], c[1].v); // = Mul(xf2, vertices2[i2]);
    c[1].id.features.referenceEdge = edge1;
    c[1].id.features.incidentEdge = i2;
    c[1].id.features.incidentVertex = 1;
  }
Esempio n. 2
0
  /**
   * Find the separation between poly1 and poly2 for a given edge normal on poly1.
   *
   * @param poly1
   * @param xf1
   * @param edge1
   * @param poly2
   * @param xf2
   */
  public final float edgeSeparation(
      final PolygonShape poly1,
      final Transform xf1,
      final int edge1,
      final PolygonShape poly2,
      final Transform xf2) {

    int count1 = poly1.m_vertexCount;
    final Vec2[] vertices1 = poly1.m_vertices;
    final Vec2[] normals1 = poly1.m_normals;

    int count2 = poly2.m_vertexCount;
    final Vec2[] vertices2 = poly2.m_vertices;

    assert (0 <= edge1 && edge1 < count1);

    // Convert normal from poly1's frame into poly2's frame.
    // Vec2 normal1World = Mul(xf1.R, normals1[edge1]);
    Mat22.mulToOut(xf1.R, normals1[edge1], normal1World);
    // Vec2 normal1 = MulT(xf2.R, normal1World);
    Mat22.mulTransToOut(xf2.R, normal1World, normal1);

    // Find support vertex on poly2 for -normal.
    int index = 0;
    float minDot = Float.MAX_VALUE;

    for (int i = 0; i < count2; ++i) {
      float dot = Vec2.dot(vertices2[i], normal1);
      if (dot < minDot) {
        minDot = dot;
        index = i;
      }
    }

    // Vec2 v1 = Mul(xf1, vertices1[edge1]);
    // Vec2 v2 = Mul(xf2, vertices2[index]);
    Transform.mulToOut(xf1, vertices1[edge1], v1);
    Transform.mulToOut(xf2, vertices2[index], v2);

    float separation = Vec2.dot(v2.subLocal(v1), normal1World);
    return separation;
  }
Esempio n. 3
0
  protected final void synchronizeFixtures() {
    final Transform xf1 = pxf;
    xf1.R.set(m_sweep.a0);
    // xf1.position = m_sweep.c0 - Mul(xf1.R, m_sweep.localCenter);
    Mat22.mulToOut(xf1.R, m_sweep.localCenter, xf1.position);
    xf1.position.mulLocal(-1).addLocal(m_sweep.c0);

    BroadPhase broadPhase = m_world.m_contactManager.m_broadPhase;
    for (Fixture f = m_fixtureList; f != null; f = f.m_next) {
      f.synchronize(broadPhase, xf1, m_xf);
    }
  }
Esempio n. 4
0
  @Override
  public void solveVelocityConstraints(TimeStep step) {
    Body b = m_bodyB;

    Vec2 r = pool.popVec2();

    r.set(m_localAnchor).subLocal(b.getLocalCenter());
    Mat22.mulToOut(b.getTransform().R, r, r);

    // Cdot = v + cross(w, r)
    Vec2 Cdot = pool.popVec2();
    Vec2.crossToOut(b.m_angularVelocity, r, Cdot);
    Cdot.addLocal(b.m_linearVelocity);

    Vec2 impulse = pool.popVec2();
    Vec2 temp = pool.popVec2();

    // Mul(m_mass, -(Cdot + m_beta * m_C + m_gamma * m_impulse));
    impulse.set(m_C).mulLocal(m_beta);
    temp.set(m_impulse).mulLocal(m_gamma);
    temp.addLocal(impulse).addLocal(Cdot).mulLocal(-1);
    Mat22.mulToOut(m_mass, temp, impulse);

    Vec2 oldImpulse = temp;
    oldImpulse.set(m_impulse);
    m_impulse.addLocal(impulse);
    float maxImpulse = step.dt * m_maxForce;
    if (m_impulse.lengthSquared() > maxImpulse * maxImpulse) {
      m_impulse.mulLocal(maxImpulse / m_impulse.length());
    }
    impulse.set(m_impulse).subLocal(oldImpulse);

    // pooling
    oldImpulse.set(impulse).mulLocal(b.m_invMass);
    b.m_linearVelocity.addLocal(oldImpulse);
    b.m_angularVelocity += b.m_invI * Vec2.cross(r, impulse);

    pool.pushVec2(4);
  }
Esempio n. 5
0
  /**
   * Compute the collision manifold between two polygons.
   *
   * @param manifold
   * @param polygon1
   * @param xf1
   * @param polygon2
   * @param xf2
   */
  public final void collidePolygons(
      Manifold manifold,
      final PolygonShape polyA,
      final Transform xfA,
      final PolygonShape polyB,
      final Transform xfB) {
    // Find edge normal of max separation on A - return if separating axis is found
    // Find edge normal of max separation on B - return if separation axis is found
    // Choose reference edge as min(minA, minB)
    // Find incident edge
    // Clip

    // The normal points from 1 to 2

    manifold.pointCount = 0;
    float totalRadius = polyA.m_radius + polyB.m_radius;

    findMaxSeparation(results1, polyA, xfA, polyB, xfB);
    if (results1.separation > totalRadius) {
      return;
    }

    findMaxSeparation(results2, polyB, xfB, polyA, xfA);
    if (results2.separation > totalRadius) {
      return;
    }

    final PolygonShape poly1; // reference polygon
    final PolygonShape poly2; // incident polygon
    Transform xf1, xf2;
    int edge1; // reference edge
    int flip;
    final float k_relativeTol = 0.98f;
    final float k_absoluteTol = 0.001f;

    if (results2.separation > k_relativeTol * results1.separation + k_absoluteTol) {
      poly1 = polyB;
      poly2 = polyA;
      xf1 = xfB;
      xf2 = xfA;
      edge1 = results2.edgeIndex;
      manifold.type = ManifoldType.FACE_B;
      flip = 1;
    } else {
      poly1 = polyA;
      poly2 = polyB;
      xf1 = xfA;
      xf2 = xfB;
      edge1 = results1.edgeIndex;
      manifold.type = ManifoldType.FACE_A;
      flip = 0;
    }

    findIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);

    int count1 = poly1.m_vertexCount;
    final Vec2[] vertices1 = poly1.m_vertices;

    v11.set(vertices1[edge1]);
    v12.set(edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0]);

    localTangent.set(v12).subLocal(v11);
    localTangent.normalize();

    Vec2.crossToOut(localTangent, 1f, localNormal); // Vec2 localNormal = Cross(dv,
    // 1.0f);

    planePoint.set(v11).addLocal(v12).mulLocal(.5f); // Vec2 planePoint = 0.5f * (v11
    // + v12);

    Mat22.mulToOut(xf1.R, localTangent, tangent); // Vec2 sideNormal = Mul(xf1.R, v12
    // - v11);
    Vec2.crossToOut(tangent, 1f, normal); // Vec2 frontNormal = Cross(sideNormal,
    // 1.0f);

    Transform.mulToOut(xf1, v11, v11);
    Transform.mulToOut(xf1, v12, v12);
    // v11 = Mul(xf1, v11);
    // v12 = Mul(xf1, v12);

    // Face offset
    float frontOffset = Vec2.dot(normal, v11);

    // Side offsets, extended by polytope skin thickness.
    float sideOffset1 = -Vec2.dot(tangent, v11) + totalRadius;
    float sideOffset2 = Vec2.dot(tangent, v12) + totalRadius;

    // Clip incident edge against extruded edge1 side edges.
    // ClipVertex clipPoints1[2];
    // ClipVertex clipPoints2[2];
    int np;

    // Clip to box side 1
    // np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1);
    tangent.negateLocal();
    np = clipSegmentToLine(clipPoints1, incidentEdge, tangent, sideOffset1);
    tangent.negateLocal();

    if (np < 2) {
      return;
    }

    // Clip to negative box side 1
    np = clipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2);

    if (np < 2) {
      return;
    }

    // Now clipPoints2 contains the clipped points.
    manifold.localNormal.set(localNormal);
    manifold.localPoint.set(planePoint);

    int pointCount = 0;
    for (int i = 0; i < Settings.maxManifoldPoints; ++i) {
      float separation = Vec2.dot(normal, clipPoints2[i].v) - frontOffset;

      if (separation <= totalRadius) {
        ManifoldPoint cp = manifold.points[pointCount];
        Transform.mulTransToOut(xf2, clipPoints2[i].v, cp.localPoint);
        // cp.m_localPoint = MulT(xf2, clipPoints2[i].v);
        cp.id.set(clipPoints2[i].id);
        cp.id.features.flip = flip;
        ++pointCount;
      }
    }

    manifold.pointCount = pointCount;
  }
Esempio n. 6
0
  /**
   * Find the max separation between poly1 and poly2 using edge normals from poly1.
   *
   * @param edgeIndex
   * @param poly1
   * @param xf1
   * @param poly2
   * @param xf2
   * @return
   */
  public final void findMaxSeparation(
      EdgeResults results,
      final PolygonShape poly1,
      final Transform xf1,
      final PolygonShape poly2,
      final Transform xf2) {
    int count1 = poly1.m_vertexCount;
    final Vec2[] normals1 = poly1.m_normals;

    // Vector pointing from the centroid of poly1 to the centroid of poly2.
    Transform.mulToOut(xf2, poly2.m_centroid, d);
    Transform.mulToOut(xf1, poly1.m_centroid, temp);
    d.subLocal(temp);

    Mat22.mulTransToOut(xf1.R, d, dLocal1);

    // Find edge normal on poly1 that has the largest projection onto d.
    int edge = 0;
    float dot;
    float maxDot = Float.MIN_VALUE;
    for (int i = 0; i < count1; i++) {
      dot = Vec2.dot(normals1[i], dLocal1);
      if (dot > maxDot) {
        maxDot = dot;
        edge = i;
      }
    }

    // Get the separation for the edge normal.
    float s = edgeSeparation(poly1, xf1, edge, poly2, xf2);

    // Check the separation for the previous edge normal.
    int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
    float sPrev = edgeSeparation(poly1, xf1, prevEdge, poly2, xf2);

    // Check the separation for the next edge normal.
    int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
    float sNext = edgeSeparation(poly1, xf1, nextEdge, poly2, xf2);

    // Find the best edge and the search direction.
    int bestEdge;
    float bestSeparation;
    int increment;
    if (sPrev > s && sPrev > sNext) {
      increment = -1;
      bestEdge = prevEdge;
      bestSeparation = sPrev;
    } else if (sNext > s) {
      increment = 1;
      bestEdge = nextEdge;
      bestSeparation = sNext;
    } else {
      results.edgeIndex = edge;
      results.separation = s;
      return;
    }

    // Perform a local search for the best edge normal.
    for (; ; ) {
      if (increment == -1) {
        edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
      } else {
        edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
      }

      s = edgeSeparation(poly1, xf1, edge, poly2, xf2);

      if (s > bestSeparation) {
        bestEdge = edge;
        bestSeparation = s;
      } else {
        break;
      }
    }

    results.edgeIndex = bestEdge;
    results.separation = bestSeparation;
  }
Esempio n. 7
0
  @Override
  public boolean solvePositionConstraints(final org.jbox2d.dynamics.SolverData data) {
    final Rot qA = pool.popRot();
    final Rot qB = pool.popRot();
    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);

    float angularError = 0.0f;
    float positionError = 0.0f;

    boolean fixedRotation = (m_invIA + m_invIB == 0.0f);

    // Solve angular limit constraint.
    if (m_enableLimit && m_limitState != LimitState.INACTIVE && fixedRotation == false) {
      float angle = aB - aA - m_referenceAngle;
      float limitImpulse = 0.0f;

      if (m_limitState == LimitState.EQUAL) {
        // Prevent large angular corrections
        float C =
            MathUtils.clamp(
                angle - m_lowerAngle,
                -Settings.maxAngularCorrection,
                Settings.maxAngularCorrection);
        limitImpulse = -m_motorMass * C;
        angularError = MathUtils.abs(C);
      } else if (m_limitState == LimitState.AT_LOWER) {
        float C = angle - m_lowerAngle;
        angularError = -C;

        // Prevent large angular corrections and allow some slop.
        C = MathUtils.clamp(C + Settings.angularSlop, -Settings.maxAngularCorrection, 0.0f);
        limitImpulse = -m_motorMass * C;
      } else if (m_limitState == LimitState.AT_UPPER) {
        float C = angle - m_upperAngle;
        angularError = C;

        // Prevent large angular corrections and allow some slop.
        C = MathUtils.clamp(C - Settings.angularSlop, 0.0f, Settings.maxAngularCorrection);
        limitImpulse = -m_motorMass * C;
      }

      aA -= m_invIA * limitImpulse;
      aB += m_invIB * limitImpulse;
    }
    // Solve point-to-point constraint.
    {
      qA.set(aA);
      qB.set(aB);

      final Vec2 rA = pool.popVec2();
      final Vec2 rB = pool.popVec2();
      final Vec2 C = pool.popVec2();
      final Vec2 impulse = pool.popVec2();

      Rot.mulToOutUnsafe(qA, C.set(m_localAnchorA).subLocal(m_localCenterA), rA);
      Rot.mulToOutUnsafe(qB, C.set(m_localAnchorB).subLocal(m_localCenterB), rB);
      C.set(cB).addLocal(rB).subLocal(cA).subLocal(rA);
      positionError = C.length();

      float mA = m_invMassA, mB = m_invMassB;
      float iA = m_invIA, iB = m_invIB;

      final org.jbox2d.common.Mat22 K = pool.popMat22();
      K.ex.x = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y;
      K.ex.y = -iA * rA.x * rA.y - iB * rB.x * rB.y;
      K.ey.x = K.ex.y;
      K.ey.y = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x;
      K.solveToOut(C, impulse);
      impulse.negateLocal();

      cA.x -= mA * impulse.x;
      cA.y -= mA * impulse.y;
      aA -= iA * Vec2.cross(rA, impulse);

      cB.x += mB * impulse.x;
      cB.y += mB * impulse.y;
      aB += iB * Vec2.cross(rB, impulse);

      pool.pushVec2(4);
      pool.pushMat22(1);
    }
    // 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);

    return positionError <= Settings.linearSlop && angularError <= Settings.angularSlop;
  }
Esempio n. 8
0
  @Override
  public void initVelocityConstraints(TimeStep step) {
    Body b = m_bodyB;

    float mass = b.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.
    assert (d + step.dt * k > Settings.EPSILON);
    m_gamma = step.dt * (d + step.dt * k);
    if (m_gamma != 0.0f) {
      m_gamma = 1.0f / m_gamma;
    }
    m_beta = step.dt * k * m_gamma;

    Vec2 r = pool.popVec2();

    // Compute the effective mass matrix.
    // Vec2 r = Mul(b.getTransform().R, m_localAnchor - b.getLocalCenter());
    r.set(m_localAnchor).subLocal(b.getLocalCenter());
    Mat22.mulToOut(b.getTransform().R, r, r);

    // 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]
    float invMass = b.m_invMass;
    float invI = b.m_invI;

    Mat22 K1 = pool.popMat22();
    K1.m11 = invMass;
    K1.m21 = 0.0f;
    K1.m12 = 0.0f;
    K1.m22 = invMass;

    Mat22 K2 = pool.popMat22();
    K2.m11 = invI * r.y * r.y;
    K2.m21 = -invI * r.x * r.y;
    K2.m12 = -invI * r.x * r.y;
    K2.m22 = invI * r.x * r.x;

    Mat22 K = pool.popMat22();
    K.set(K1).addLocal(K2);
    K.m11 += m_gamma;
    K.m22 += m_gamma;

    K.invertToOut(m_mass);

    m_C.set(b.m_sweep.c).addLocal(r).subLocal(m_target);

    // Cheat with some damping
    b.m_angularVelocity *= 0.98f;

    // Warm starting.
    m_impulse.mulLocal(step.dtRatio);
    // pool
    Vec2 temp = pool.popVec2();
    temp.set(m_impulse).mulLocal(invMass);
    b.m_linearVelocity.addLocal(temp);
    b.m_angularVelocity += invI * Vec2.cross(r, m_impulse);

    pool.pushVec2(2);
    pool.pushMat22(3);
  }
Esempio n. 9
0
 public final void getLocalVectorToOut(Vec2 worldVector, Vec2 out) {
   Mat22.mulTransToOut(m_xf.R, worldVector, out);
 }
Esempio n. 10
0
 public final void getWorldVectorToOut(Vec2 localVector, Vec2 out) {
   Mat22.mulToOut(m_xf.R, localVector, out);
 }