/** @see Shape#testSegment(XForm, RaycastResult, Segment, float) */
  @Override
  public SegmentCollide testSegment(
      final XForm xf, final RaycastResult out, final Segment segment, final float maxLambda) {
    final Vec2 r = tlR.get();
    final Vec2 v1 = tlV1.get();
    final Vec2 d = tlD.get();
    final Vec2 n = tlN.get();
    final Vec2 b = tlB.get();

    r.set(segment.p2).subLocal(segment.p1);
    XForm.mulToOut(xf, m_v1, v1);
    XForm.mulToOut(xf, m_v2, d);
    d.subLocal(v1);
    Vec2.crossToOut(d, 1.0f, n);

    final float k_slop = 100.0f * Settings.EPSILON;
    final float denom = -Vec2.dot(r, n);

    // Cull back facing collision and ignore parallel segments.
    if (denom > k_slop) {
      // Does the segment intersect the infinite line associated with this segment?
      b.set(segment.p1).subLocal(v1);
      float a = Vec2.dot(b, n);

      if (0.0f <= a && a <= maxLambda * denom) {
        final float mu2 = -r.x * b.y + r.y * b.x;

        // Does the segment intersect this segment?
        if (-k_slop * denom <= mu2 && mu2 <= denom * (1.0f + k_slop)) {
          a /= denom;
          n.normalize();
          out.lambda = a;
          out.normal.set(n);
          return SegmentCollide.HIT_COLLIDE;
        }
      }
    }

    return SegmentCollide.MISS_COLLIDE;
  }
  /**
   * Don't use this. Instead create using {@link Body#createShape(ShapeDef)} with an {@link
   * EdgeChainDef}, not the constructor here.
   *
   * @see Body#createShape(ShapeDef)
   * @see EdgeChainDef
   * @param v1
   * @param v2
   * @param def
   */
  public EdgeShape(final Vec2 v1, final Vec2 v2, final ShapeDef def) {
    super(def);
    assert (def.type == ShapeType.EDGE_SHAPE);

    m_type = ShapeType.EDGE_SHAPE;

    m_prevEdge = null;
    m_nextEdge = null;

    m_v1 = v1;
    m_v2 = v2;

    m_direction = m_v2.sub(m_v1);
    m_length = m_direction.normalize();
    m_normal = new Vec2(m_direction.y, -m_direction.x);

    // djm they are new objects after that first math call
    m_coreV1 = (m_normal.sub(m_direction)).mulLocal(-Settings.toiSlop).addLocal(m_v1);
    m_coreV2 = (m_normal.add(m_direction)).mulLocal(-Settings.toiSlop).addLocal(m_v2);

    m_cornerDir1 = m_normal.clone();
    m_cornerDir2 = m_normal.mul(-1.0f);
  }
Пример #3
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;
  }
Пример #4
0
  public final void initialize(
      final Manifold manifold,
      final Transform xfA,
      float radiusA,
      final Transform xfB,
      float radiusB) {
    if (manifold.pointCount == 0) {
      return;
    }

    switch (manifold.type) {
      case CIRCLES:
        {
          //				final Vec2 pointA = pool3;
          //				final Vec2 pointB = pool4;
          //
          //				normal.set(1, 0);
          //				Transform.mulToOut(xfA, manifold.localPoint, pointA);
          //				Transform.mulToOut(xfB, manifold.points[0].localPoint, pointB);
          //
          //				if (MathUtils.distanceSquared(pointA, pointB) > Settings.EPSILON * Settings.EPSILON)
          // {
          //					normal.set(pointB).subLocal(pointA);
          //					normal.normalize();
          //				}
          //
          //				cA.set(normal).mulLocal(radiusA).addLocal(pointA);
          //				cB.set(normal).mulLocal(radiusB).subLocal(pointB).negateLocal();
          //				points[0].set(cA).addLocal(cB).mulLocal(0.5f);
          final Vec2 pointA = pool3;
          final Vec2 pointB = pool4;

          normal.x = 1;
          normal.y = 0;
          pointA.x =
              xfA.position.x
                  + xfA.R.col1.x * manifold.localPoint.x
                  + xfA.R.col2.x * manifold.localPoint.y;
          pointA.y =
              xfA.position.y
                  + xfA.R.col1.y * manifold.localPoint.x
                  + xfA.R.col2.y * manifold.localPoint.y;
          pointB.x =
              xfB.position.x
                  + xfB.R.col1.x * manifold.points[0].localPoint.x
                  + xfB.R.col2.x * manifold.points[0].localPoint.y;
          pointB.y =
              xfB.position.y
                  + xfB.R.col1.y * manifold.points[0].localPoint.x
                  + xfB.R.col2.y * manifold.points[0].localPoint.y;

          if (MathUtils.distanceSquared(pointA, pointB) > Settings.EPSILON * Settings.EPSILON) {
            normal.x = pointB.x - pointA.x;
            normal.y = pointB.y - pointA.y;
            normal.normalize();
          }

          final float cAx = normal.x * radiusA + pointA.x;
          final float cAy = normal.y * radiusA + pointA.y;

          final float cBx = -normal.x * radiusB + pointB.x;
          final float cBy = -normal.y * radiusB + pointB.y;

          points[0].x = (cAx + cBx) * .5f;
          points[0].y = (cAy + cBy) * .5f;
        }
        break;
      case FACE_A:
        {
          //				final Vec2 planePoint = pool3;
          //
          //				Mat22.mulToOut(xfA.R, manifold.localNormal, normal);
          //				Transform.mulToOut(xfA, manifold.localPoint, planePoint);
          //
          //				final Vec2 clipPoint = pool4;
          //
          //				for (int i = 0; i < manifold.pointCount; i++) {
          //					// b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint);
          //					// b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint,
          //					// normal)) * normal;
          //					// b2Vec2 cB = clipPoint - radiusB * normal;
          //					// points[i] = 0.5f * (cA + cB);
          //					Transform.mulToOut(xfB, manifold.points[i].localPoint, clipPoint);
          //					// use cA as temporary for now
          //					cA.set(clipPoint).subLocal(planePoint);
          //					float scalar = radiusA - Vec2.dot(cA, normal);
          //					cA.set(normal).mulLocal(scalar).addLocal(clipPoint);
          //					cB.set(normal).mulLocal(radiusB).subLocal(clipPoint).negateLocal();
          //					points[i].set(cA).addLocal(cB).mulLocal(0.5f);
          //				}
          final Vec2 planePoint = pool3;

          normal.x = xfA.R.col1.x * manifold.localNormal.x + xfA.R.col2.x * manifold.localNormal.y;
          normal.y = xfA.R.col1.y * manifold.localNormal.x + xfA.R.col2.y * manifold.localNormal.y;
          planePoint.x =
              xfA.position.x
                  + xfA.R.col1.x * manifold.localPoint.x
                  + xfA.R.col2.x * manifold.localPoint.y;
          planePoint.y =
              xfA.position.y
                  + xfA.R.col1.y * manifold.localPoint.x
                  + xfA.R.col2.y * manifold.localPoint.y;

          final Vec2 clipPoint = pool4;

          for (int i = 0; i < manifold.pointCount; i++) {
            // b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint);
            // b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint,
            // normal)) * normal;
            // b2Vec2 cB = clipPoint - radiusB * normal;
            // points[i] = 0.5f * (cA + cB);

            clipPoint.x =
                xfB.position.x
                    + xfB.R.col1.x * manifold.points[i].localPoint.x
                    + xfB.R.col2.x * manifold.points[i].localPoint.y;
            clipPoint.y =
                xfB.position.y
                    + xfB.R.col1.y * manifold.points[i].localPoint.x
                    + xfB.R.col2.y * manifold.points[i].localPoint.y;

            final float scalar =
                radiusA
                    - ((clipPoint.x - planePoint.x) * normal.x
                        + (clipPoint.y - planePoint.y) * normal.y);

            final float cAx = normal.x * scalar + clipPoint.x;
            final float cAy = normal.y * scalar + clipPoint.y;

            final float cBx = -normal.x * radiusB + clipPoint.x;
            final float cBy = -normal.y * radiusB + clipPoint.y;

            points[i].x = (cAx + cBx) * .5f;
            points[i].y = (cAy + cBy) * .5f;
          }
        }
        break;
      case FACE_B:
        final Vec2 planePoint = pool3;

        final Mat22 R = xfB.R;
        normal.x = R.col1.x * manifold.localNormal.x + R.col2.x * manifold.localNormal.y;
        normal.y = R.col1.y * manifold.localNormal.x + R.col2.y * manifold.localNormal.y;
        final Vec2 v = manifold.localPoint;
        planePoint.x = xfB.position.x + xfB.R.col1.x * v.x + xfB.R.col2.x * v.y;
        planePoint.y = xfB.position.y + xfB.R.col1.y * v.x + xfB.R.col2.y * v.y;

        final Vec2 clipPoint = pool4;

        for (int i = 0; i < manifold.pointCount; i++) {
          // b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint);
          // b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint,
          // normal)) * normal;
          // b2Vec2 cA = clipPoint - radiusA * normal;
          // points[i] = 0.5f * (cA + cB);

          //					Transform.mulToOut(xfA, manifold.points[i].localPoint, clipPoint);
          //					cB.set(clipPoint).subLocal(planePoint);
          //					float scalar = radiusB - Vec2.dot(cB, normal);
          //					cB.set(normal).mulLocal(scalar).addLocal(clipPoint);
          //					cA.set(normal).mulLocal(radiusA).subLocal(clipPoint).negateLocal();
          //					points[i].set(cA).addLocal(cB).mulLocal(0.5f);

          // points[i] = 0.5f * (cA + cB);

          clipPoint.x =
              xfA.position.x
                  + xfA.R.col1.x * manifold.points[i].localPoint.x
                  + xfA.R.col2.x * manifold.points[i].localPoint.y;
          clipPoint.y =
              xfA.position.y
                  + xfA.R.col1.y * manifold.points[i].localPoint.x
                  + xfA.R.col2.y * manifold.points[i].localPoint.y;

          final float scalar =
              radiusB
                  - ((clipPoint.x - planePoint.x) * normal.x
                      + (clipPoint.y - planePoint.y) * normal.y);

          final float cBx = normal.x * scalar + clipPoint.x;
          final float cBy = normal.y * scalar + clipPoint.y;

          final float cAx = -normal.x * radiusA + clipPoint.x;
          final float cAy = -normal.y * radiusA + clipPoint.y;

          points[i].x = (cAx + cBx) * .5f;
          points[i].y = (cAy + cBy) * .5f;
        }
        // Ensure normal points from A to B.
        normal.x = -normal.x;
        normal.y = -normal.y;
        break;
    }
  }
Пример #5
0
  @Override
  public void raycast(
      org.jbox2d.callbacks.TreeRayCastCallback callback, org.jbox2d.collision.RayCastInput input) {
    final Vec2 p1 = input.p1;
    final Vec2 p2 = input.p2;
    float p1x = p1.x, p2x = p2.x, p1y = p1.y, p2y = p2.y;
    float vx, vy;
    float rx, ry;
    float absVx, absVy;
    float cx, cy;
    float hx, hy;
    float tempx, tempy;
    r.x = p2x - p1x;
    r.y = p2y - p1y;
    assert ((r.x * r.x + r.y * r.y) > 0f);
    r.normalize();
    rx = r.x;
    ry = r.y;

    // v is perpendicular to the segment.
    vx = -1f * ry;
    vy = 1f * rx;
    absVx = org.jbox2d.common.MathUtils.abs(vx);
    absVy = org.jbox2d.common.MathUtils.abs(vy);

    // Separating axis for segment (Gino, p80).
    // |dot(v, p1 - c)| > dot(|v|, h)

    float maxFraction = input.maxFraction;

    // Build a bounding box for the segment.
    final org.jbox2d.collision.AABB segAABB = aabb;
    // Vec2 t = p1 + maxFraction * (p2 - p1);
    // before inline
    // temp.set(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1);
    // Vec2.minToOut(p1, temp, segAABB.lowerBound);
    // Vec2.maxToOut(p1, temp, segAABB.upperBound);
    tempx = (p2x - p1x) * maxFraction + p1x;
    tempy = (p2y - p1y) * maxFraction + p1y;
    segAABB.lowerBound.x = p1x < tempx ? p1x : tempx;
    segAABB.lowerBound.y = p1y < tempy ? p1y : tempy;
    segAABB.upperBound.x = p1x > tempx ? p1x : tempx;
    segAABB.upperBound.y = p1y > tempy ? p1y : tempy;
    // end inline

    nodeStackIndex = 0;
    nodeStack[nodeStackIndex++] = m_root;
    while (nodeStackIndex > 0) {
      final DynamicTreeNode node = nodeStack[--nodeStackIndex];
      if (node == null) {
        continue;
      }

      final org.jbox2d.collision.AABB nodeAABB = node.aabb;
      if (!org.jbox2d.collision.AABB.testOverlap(nodeAABB, segAABB)) {
        continue;
      }

      // Separating axis for segment (Gino, p80).
      // |dot(v, p1 - c)| > dot(|v|, h)
      // node.aabb.getCenterToOut(c);
      // node.aabb.getExtentsToOut(h);
      cx = (nodeAABB.lowerBound.x + nodeAABB.upperBound.x) * .5f;
      cy = (nodeAABB.lowerBound.y + nodeAABB.upperBound.y) * .5f;
      hx = (nodeAABB.upperBound.x - nodeAABB.lowerBound.x) * .5f;
      hy = (nodeAABB.upperBound.y - nodeAABB.lowerBound.y) * .5f;
      tempx = p1x - cx;
      tempy = p1y - cy;
      float separation =
          org.jbox2d.common.MathUtils.abs(vx * tempx + vy * tempy) - (absVx * hx + absVy * hy);
      if (separation > 0.0f) {
        continue;
      }

      if (node.child1 == null) {
        subInput.p1.x = p1x;
        subInput.p1.y = p1y;
        subInput.p2.x = p2x;
        subInput.p2.y = p2y;
        subInput.maxFraction = maxFraction;

        float value = callback.raycastCallback(subInput, node.id);

        if (value == 0.0f) {
          // The client has terminated the ray cast.
          return;
        }

        if (value > 0.0f) {
          // Update segment bounding box.
          maxFraction = value;
          // temp.set(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1);
          // Vec2.minToOut(p1, temp, segAABB.lowerBound);
          // Vec2.maxToOut(p1, temp, segAABB.upperBound);
          tempx = (p2x - p1x) * maxFraction + p1x;
          tempy = (p2y - p1y) * maxFraction + p1y;
          segAABB.lowerBound.x = p1x < tempx ? p1x : tempx;
          segAABB.lowerBound.y = p1y < tempy ? p1y : tempy;
          segAABB.upperBound.x = p1x > tempx ? p1x : tempx;
          segAABB.upperBound.y = p1y > tempy ? p1y : tempy;
        }
      } else {
        if (nodeStack.length - nodeStackIndex - 2 <= 0) {
          DynamicTreeNode[] newBuffer = new DynamicTreeNode[nodeStack.length * 2];
          System.arraycopy(nodeStack, 0, newBuffer, 0, nodeStack.length);
          nodeStack = newBuffer;
        }
        nodeStack[nodeStackIndex++] = node.child1;
        nodeStack[nodeStackIndex++] = node.child2;
      }
    }
  }