示例#1
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;
  }
  /** @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;
  }
示例#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;
  }
示例#4
0
文件: World.java 项目: robinp/libgdx
  private void drawShape(Fixture fixture, Transform xf, Color3f color) {
    switch (fixture.getType()) {
      case CIRCLE:
        {
          CircleShape circle = (CircleShape) fixture.getShape();

          // Vec2 center = Mul(xf, circle.m_p);
          Transform.mulToOutUnsafe(xf, circle.m_p, center);
          float radius = circle.m_radius;
          xf.q.getXAxis(axis);

          if (fixture.getUserData() != null && fixture.getUserData().equals(LIQUID_INT)) {
            Body b = fixture.getBody();
            liquidOffset.set(b.m_linearVelocity);
            float linVelLength = b.m_linearVelocity.length();
            if (averageLinearVel == -1) {
              averageLinearVel = linVelLength;
            } else {
              averageLinearVel = .98f * averageLinearVel + .02f * linVelLength;
            }
            liquidOffset.mulLocal(liquidLength / averageLinearVel / 2);
            circCenterMoved.set(center).addLocal(liquidOffset);
            center.subLocal(liquidOffset);
            m_debugDraw.drawSegment(center, circCenterMoved, liquidColor);
            return;
          }

          m_debugDraw.drawSolidCircle(center, radius, axis, color);
        }
        break;

      case POLYGON:
        {
          PolygonShape poly = (PolygonShape) fixture.getShape();
          int vertexCount = poly.m_count;
          assert (vertexCount <= Settings.maxPolygonVertices);
          Vec2[] vertices = tlvertices.get(Settings.maxPolygonVertices);

          for (int i = 0; i < vertexCount; ++i) {
            // vertices[i] = Mul(xf, poly.m_vertices[i]);
            Transform.mulToOutUnsafe(xf, poly.m_vertices[i], vertices[i]);
          }

          m_debugDraw.drawSolidPolygon(vertices, vertexCount, color);
        }
        break;
      case EDGE:
        {
          EdgeShape edge = (EdgeShape) fixture.getShape();
          Transform.mulToOutUnsafe(xf, edge.m_vertex1, v1);
          Transform.mulToOutUnsafe(xf, edge.m_vertex2, v2);
          m_debugDraw.drawSegment(v1, v2, color);
        }
        break;

      case CHAIN:
        {
          ChainShape chain = (ChainShape) fixture.getShape();
          int count = chain.m_count;
          Vec2[] vertices = chain.m_vertices;

          Transform.mulToOutUnsafe(xf, vertices[0], v1);
          for (int i = 1; i < count; ++i) {
            Transform.mulToOutUnsafe(xf, vertices[i], v2);
            m_debugDraw.drawSegment(v1, v2, color);
            m_debugDraw.drawCircle(v1, 0.05f, color);
            v1.set(v2);
          }
        }
        break;
      default:
        break;
    }
  }
  /**
   * Rozbije objekt. Upravi objekt world tak, ze vymaze triesteny objekt a nahradi ho fragmentami na
   * zaklade nastaveneho materialu a clenskych premennych.
   *
   * @param dt casova dlzka framu
   */
  public void smash(float dt) {
    if (contact == null) { // riesi sa staticky prvok, ktory ma priliz maly obsah
      b1.setType(BodyType.DYNAMIC);
      return;
    }

    World w = b1.m_world;
    Shape s = f1.m_shape;
    Polygon p = f1.m_polygon;

    if (p == null) {
      switch (s.m_type) {
        case POLYGON:
          PolygonShape ps = (PolygonShape) s;
          Vec2[] vertices = ps.m_vertices;
          p = new Polygon();
          for (int i = 0; i < ps.m_count; ++i) {
            p.add(vertices[ps.m_count - i - 1]);
          }
          break;
        case CIRCLE:
          CircleShape cs = (CircleShape) s;
          p = new Polygon();
          float radius = cs.m_radius;

          double u = Math.PI * 2 / CIRCLEVERTICES;
          radius =
              (float) Math.sqrt(u / Math.sin(u))
                  * radius; // upravim radius tak, aby bola zachovana velkost obsahu

          Vec2 center = cs.m_p;
          for (int i = 0; i < CIRCLEVERTICES; ++i) {
            double j = u * i; // uhol
            float sin = (float) Math.sin(j);
            float cos = (float) Math.cos(j);
            Vec2 v = new Vec2(sin, cos).mulLocal(radius).addLocal(center);
            p.add(v);
          }
          break;
        default:
          throw new RuntimeException("Dany typ tvaru nepodporuje stiepenie");
      }
    }

    float mConst = f1.m_material.m_rigidity / normalImpulse; // sila v zavislosti na pevnosti telesa

    boolean fixA = f1 == contact.m_fixtureA; // true, ak f2 je v objekte contact ako m_fixtureA
    float oldAngularVelocity =
        fixA ? contact.m_angularVelocity_bodyA : contact.m_angularVelocity_bodyB;
    Vec2 oldLinearVelocity = fixA ? contact.m_linearVelocity_bodyA : contact.m_linearVelocity_bodyB;
    b1.setAngularVelocity(
        (b1.m_angularVelocity - oldAngularVelocity) * mConst + oldAngularVelocity);
    b1.setLinearVelocity(
        b1.m_linearVelocity.sub(oldLinearVelocity).mulLocal(mConst).addLocal(oldLinearVelocity));
    if (!w.isFractured(f2)
        && b2.m_type == BodyType.DYNAMIC
        && !b2.m_fractureTransformUpdate) { // ak sa druhy objekt nerozbija, tak sa jej nahodia
      // povodne hodnoty (TREBA MODIFIKOVAT POHYB OBJEKTU,
      // KTORY SPOSOBUJE ROZPAD)
      oldAngularVelocity =
          !fixA ? contact.m_angularVelocity_bodyA : contact.m_angularVelocity_bodyB;
      oldLinearVelocity = !fixA ? contact.m_linearVelocity_bodyA : contact.m_linearVelocity_bodyB;
      b2.setAngularVelocity(
          (b2.m_angularVelocity - oldAngularVelocity) * mConst + oldAngularVelocity);
      b2.setLinearVelocity(
          b2.m_linearVelocity.sub(oldLinearVelocity).mulLocal(mConst).addLocal(oldLinearVelocity));
      b2.setTransform(
          b2.m_xf0.p.add(b2.m_linearVelocity.mul(dt)),
          b2.m_xf0.q.getAngle()); // osetruje jbox2d od posuvania telesa pri rieseni kolizie
      b2.m_fractureTransformUpdate = true;
    }

    Vec2 localPoint = Transform.mulTrans(b1.m_xf, point);
    Vec2 b1Vec = b1.getLinearVelocityFromWorldPoint(point);
    Vec2 b2Vec = b2.getLinearVelocityFromWorldPoint(point);
    Vec2 localVector = b2Vec.subLocal(b1Vec);

    localVector.mulLocal(dt);
    Polygon[] fragment;
    try {
      fragment = m.split(p, localPoint, localVector, normalImpulse); // rodeli to
    } catch (RuntimeException ex) {
      return;
    }

    if (fragment.length == 1) { // nerozbilo to na ziadne fragmenty
      return;
    }

    // definuje tela fragmentov - tie maju vsetky rovnaku definiciu (preberaju parametre z povodneho
    // objektu)
    BodyDef bodyDef = new BodyDef();
    bodyDef.position.set(b1.m_xf.p); // pozicia
    bodyDef.angle = b1.m_xf.q.getAngle(); // otocenie
    bodyDef.fixedRotation = b1.isFixedRotation();
    bodyDef.angularDamping = b1.m_angularDamping;
    bodyDef.allowSleep = b1.isSleepingAllowed();

    FixtureDef fd = new FixtureDef();
    fd.friction = f1.m_friction; // trenie
    fd.restitution = f1.m_restitution; // odrazivost
    fd.isSensor = f1.m_isSensor;
    fd.density = f1.m_density;

    // odstrani fragmentacne predmety/cele teleso
    ArrayList<Fixture> fixtures = new ArrayList<>();
    if (f1.m_polygon != null) {
      for (Fixture f = b1.m_fixtureList; f != null; f = f.m_next) {
        if (f.m_polygon == f1.m_polygon) {
          fixtures.add(f);
        }
      }
    } else {
      fixtures.add(f1);
    }

    for (Fixture f : fixtures) {
      b1.destroyFixture(f);
    }

    if (b1.m_fixtureCount == 0) {
      w.destroyBody(b1);
    }

    // prida fragmenty do simulacie
    MyList<Body> newbodies = new MyList<>();
    for (Polygon pg : fragment) { // vytvori tela, prida fixtury, poriesi konvexnu dekompoziciu
      if (pg.isCorrect()) {
        if (pg instanceof Fragment) {
          Polygon[] convex = pg.convexDecomposition();
          bodyDef.type = BodyType.DYNAMIC;
          for (Polygon pgx : convex) {
            Body f_body = w.createBody(bodyDef);
            pgx.flip();
            PolygonShape ps = new PolygonShape();
            ps.set(pgx.getArray(), pgx.size());
            fd.shape = ps;
            fd.polygon = null;
            fd.material = f1.m_material.m_fragments; // rekurzivne stiepenie

            f_body.createFixture(fd);
            f_body.setAngularVelocity(b1.m_angularVelocity);
            f_body.setLinearVelocity(b1.getLinearVelocityFromLocalPoint(f_body.getLocalCenter()));
            newbodies.add(f_body);
          }

        } else {
          fd.material = f1.m_material.m_fragments; // rekurzivne stiepenie
          bodyDef.type = b1.getType();
          Body f_body = w.createBody(bodyDef);
          PolygonFixture pf = new PolygonFixture(pg);

          f_body.createFixture(pf, fd);
          f_body.setLinearVelocity(b1.getLinearVelocityFromLocalPoint(f_body.getLocalCenter()));
          f_body.setAngularVelocity(b1.m_angularVelocity);
          newbodies.add(f_body);
        }
      }
    }

    // zavola sa funkcia z fraction listeneru (pokial je nadefinovany)
    FractureListener fl = w.getContactManager().m_fractureListener;
    if (fl != null) {
      fl.action(m, normalImpulse, newbodies);
    }
  }