Example #1
0
  /**
   * Compute the collision manifold between a polygon and a circle.
   *
   * @param manifold
   * @param polygon
   * @param xfA
   * @param circle
   * @param xfB
   */
  public final void collidePolygonAndCircle(
      Manifold manifold,
      final PolygonShape polygon,
      final Transform xfA,
      final CircleShape circle,
      final Transform xfB) {
    manifold.pointCount = 0;

    // Compute circle position in the frame of the polygon.
    Transform.mulToOut(xfB, circle.m_p, c);
    Transform.mulTransToOut(xfA, c, cLocal);

    // Find the min separating edge.
    int normalIndex = 0;
    float separation = Float.MIN_VALUE;
    float radius = polygon.m_radius + circle.m_radius;
    int vertexCount = polygon.m_vertexCount;

    Vec2[] vertices = polygon.m_vertices;
    Vec2[] normals = polygon.m_normals;

    for (int i = 0; i < vertexCount; i++) {
      temp.set(cLocal).subLocal(vertices[i]);
      float s = Vec2.dot(normals[i], temp);

      if (s > radius) {
        // early out
        return;
      }

      if (s > separation) {
        separation = s;
        normalIndex = i;
      }
    }

    // Vertices that subtend the incident face.
    int vertIndex1 = normalIndex;
    int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
    Vec2 v1 = vertices[vertIndex1];
    Vec2 v2 = vertices[vertIndex2];

    // If the center is inside the polygon ...
    if (separation < Settings.EPSILON) {
      manifold.pointCount = 1;
      manifold.type = ManifoldType.FACE_A;
      manifold.localNormal.set(normals[normalIndex]);
      manifold.localPoint.set(v1).addLocal(v2).mulLocal(.5f);
      manifold.points[0].localPoint.set(circle.m_p);
      manifold.points[0].id.zero();
      return;
    }

    // Compute barycentric coordinates
    temp.set(cLocal).subLocal(v1);
    temp2.set(v2).subLocal(v1);
    float u1 = Vec2.dot(temp, temp2);
    temp.set(cLocal).subLocal(v2);
    temp2.set(v1).subLocal(v2);
    float u2 = Vec2.dot(temp, temp2);

    if (u1 <= 0f) {
      if (MathUtils.distanceSquared(cLocal, v1) > radius * radius) {
        return;
      }

      manifold.pointCount = 1;
      manifold.type = ManifoldType.FACE_A;
      manifold.localNormal.set(cLocal).subLocal(v1);
      manifold.localNormal.normalize();
      manifold.localPoint.set(v1);
      manifold.points[0].localPoint.set(circle.m_p);
      manifold.points[0].id.zero();
    } else if (u2 <= 0.0f) {
      if (MathUtils.distanceSquared(cLocal, v2) > radius * radius) {
        return;
      }

      manifold.pointCount = 1;
      manifold.type = ManifoldType.FACE_A;
      manifold.localNormal.set(cLocal).subLocal(v2);
      manifold.localNormal.normalize();
      manifold.localPoint.set(v2);
      manifold.points[0].localPoint.set(circle.m_p);
      manifold.points[0].id.zero();
    } else {
      // Vec2 faceCenter = 0.5f * (v1 + v2);
      // (temp is faceCenter)
      temp.set(v1).addLocal(v2).mulLocal(.5f);

      temp2.set(cLocal).subLocal(temp);
      separation = Vec2.dot(temp2, normals[vertIndex1]);
      if (separation > radius) {
        return;
      }

      manifold.pointCount = 1;
      manifold.type = ManifoldType.FACE_A;
      manifold.localNormal.set(normals[vertIndex1]);
      manifold.localPoint.set(temp); // (faceCenter)
      manifold.points[0].localPoint.set(circle.m_p);
      manifold.points[0].id.zero();
    }
  }
Example #2
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;
    }
  }