Beispiel #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;
  }
Beispiel #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;
  }
Beispiel #3
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;
  }
Beispiel #4
0
 public final void getLocalVectorToOut(Vec2 worldVector, Vec2 out) {
   Mat22.mulTransToOut(m_xf.R, worldVector, out);
 }