Example #1
0
  private void computeBounds(int[] lowerValues, int[] upperValues, AABB aabb) {
    if (debugPrint) {
      System.out.println("ComputeBounds()");
    }
    assert (aabb.upperBound.x > aabb.lowerBound.x);
    assert (aabb.upperBound.y > aabb.lowerBound.y);

    Vec2 minVertex =
        MathUtils.clamp(aabb.lowerBound, m_worldAABB.lowerBound, m_worldAABB.upperBound);
    Vec2 maxVertex =
        MathUtils.clamp(aabb.upperBound, m_worldAABB.lowerBound, m_worldAABB.upperBound);

    // System.out.printf("minV = %f %f, maxV = %f %f
    // \n",aabb.minVertex.x,aabb.minVertex.y,aabb.maxVertex.x,aabb.maxVertex.y);

    // Bump lower bounds downs and upper bounds up. This ensures correct
    // sorting of
    // lower/upper bounds that would have equal values.
    // TODO_ERIN implement fast float to int conversion.
    lowerValues[0] =
        (int) (m_quantizationFactor.x * (minVertex.x - m_worldAABB.lowerBound.x))
            & (Integer.MAX_VALUE - 1);
    upperValues[0] = (int) (m_quantizationFactor.x * (maxVertex.x - m_worldAABB.lowerBound.x)) | 1;

    lowerValues[1] =
        (int) (m_quantizationFactor.y * (minVertex.y - m_worldAABB.lowerBound.y))
            & (Integer.MAX_VALUE - 1);
    upperValues[1] = (int) (m_quantizationFactor.y * (maxVertex.y - m_worldAABB.lowerBound.y)) | 1;
  }
Example #2
0
  public void paint(Clock clock) {
    if (!hasLoaded) return;

    // body.setLinearVelocity(new Vec2(30 * MathUtils.cos(GameScreen.angle), 30 *
    // MathUtils.sin(GameScreen.angle)));

    sprite
        .layer()
        .setTranslation(
            (body.getPosition().x / GameScreen.M_PER_PIXEL),
            (body.getPosition().y / GameScreen.M_PER_PIXEL));
    Vec2 delta = new Vec2(80f - body.getPosition().x, 400f - body.getPosition().y);
    float angle = MathUtils.atan2(delta.x, delta.y);
    body.setLinearVelocity(new Vec2(8 * -MathUtils.cos(angle), 8 * MathUtils.sin(angle)));
  }
Example #3
0
  private void validateMetrics(DynamicTreeNode node) {
    if (node == null) {
      return;
    }

    DynamicTreeNode child1 = node.child1;
    DynamicTreeNode child2 = node.child2;

    if (node.child1 == null) {
      assert (child1 == null);
      assert (child2 == null);
      assert (node.height == 0);
      return;
    }

    assert (child1 != null && 0 <= child1.id && child1.id < m_nodeCapacity);
    assert (child2 != null && 0 <= child2.id && child2.id < m_nodeCapacity);

    int height1 = child1.height;
    int height2 = child2.height;
    int height;
    height = 1 + org.jbox2d.common.MathUtils.max(height1, height2);
    assert (node.height == height);

    org.jbox2d.collision.AABB aabb = new org.jbox2d.collision.AABB();
    aabb.combine(child1.aabb, child2.aabb);

    assert (aabb.lowerBound.equals(node.aabb.lowerBound));
    assert (aabb.upperBound.equals(node.aabb.upperBound));

    validateMetrics(child1);
    validateMetrics(child2);
  }
Example #4
0
  @Override
  public int getMaxBalance() {
    int maxBalance = 0;
    for (int i = 0; i < m_nodeCapacity; ++i) {
      final DynamicTreeNode node = m_nodes[i];
      if (node.height <= 1) {
        continue;
      }

      assert (node.child1 == null == false);

      DynamicTreeNode child1 = node.child1;
      DynamicTreeNode child2 = node.child2;
      int balance = org.jbox2d.common.MathUtils.abs(child2.height - child1.height);
      maxBalance = org.jbox2d.common.MathUtils.max(maxBalance, balance);
    }

    return maxBalance;
  }
Example #5
0
  private final int computeHeight(DynamicTreeNode node) {
    assert (0 <= node.id && node.id < m_nodeCapacity);

    if (node.child1 == null) {
      return 0;
    }
    int height1 = computeHeight(node.child1);
    int height2 = computeHeight(node.child2);
    return 1 + org.jbox2d.common.MathUtils.max(height1, height2);
  }
Example #6
0
  public final void synchronizeTransform() {
    // m_xf.R.set(m_sweep.a);
    //
    // //m_xf.position = m_sweep.c - Mul(m_xf.R, m_sweep.localCenter);
    // Mat22.mulToOut(m_xf.R, m_sweep.localCenter, m_xf.position);
    // m_xf.position.mulLocal(-1).addLocal(m_sweep.c);

    final float c = MathUtils.cos(m_sweep.a), s = MathUtils.sin(m_sweep.a);
    m_xf.R.m11 = c;
    m_xf.R.m21 = -s;
    m_xf.R.m12 = s;
    m_xf.R.m22 = c;
    m_xf.position.x = m_xf.R.m11 * m_sweep.localCenter.x + m_xf.R.m21 * m_sweep.localCenter.y;
    m_xf.position.y = m_xf.R.m12 * m_sweep.localCenter.x + m_xf.R.m22 * m_sweep.localCenter.y;
    m_xf.position.x *= -1f;
    m_xf.position.y *= -1f;
    m_xf.position.x += m_sweep.c.x;
    m_xf.position.y += m_sweep.c.y;
  }
 /** @see Shape#updateSweepRadius(Vec2) */
 @Override
 public void updateSweepRadius(final Vec2 center) {
   // Update the sweep radius (maximum radius) as measured from
   // a local center point.
   final float dx = m_coreV1.x - center.x;
   final float dy = m_coreV1.y - center.y;
   final float d1 = dx * dx + dy * dy;
   final float dx2 = m_coreV2.x - center.x;
   final float dy2 = m_coreV2.y - center.y;
   final float d2 = dx2 * dx2 + dy2 * dy2;
   m_sweepRadius = MathUtils.sqrt(d1 > d2 ? d1 : d2);
 }
Example #8
0
 @Override
 public void drawCircle(Vec2 center, float radius, Vec2 axis, Color3f color) {
   Graphics2D g = getGraphics();
   saveState(g);
   transformGraphics(g, center);
   g.setStroke(stroke);
   Color s = cpool.getColor(color.x, color.y, color.z, 1f);
   g.scale(radius, radius);
   g.setColor(s);
   g.draw(circle);
   if (axis != null) {
     g.rotate(MathUtils.atan2(axis.y, axis.x));
     g.drawLine(0, 0, 1, 0);
   }
   restoreState(g);
 }
Example #9
0
  private final void removeLeaf(DynamicTreeNode leaf) {
    if (leaf == m_root) {
      m_root = null;
      return;
    }

    DynamicTreeNode parent = leaf.parent;
    DynamicTreeNode grandParent = parent.parent;
    DynamicTreeNode sibling;
    if (parent.child1 == leaf) {
      sibling = parent.child2;
    } else {
      sibling = parent.child1;
    }

    if (grandParent != null) {
      // Destroy parent and connect sibling to grandParent.
      if (grandParent.child1 == parent) {
        grandParent.child1 = sibling;
      } else {
        grandParent.child2 = sibling;
      }
      sibling.parent = grandParent;
      freeNode(parent);

      // Adjust ancestor bounds.
      DynamicTreeNode index = grandParent;
      while (index != null) {
        index = balance(index);

        DynamicTreeNode child1 = index.child1;
        DynamicTreeNode child2 = index.child2;

        index.aabb.combine(child1.aabb, child2.aabb);
        index.height = 1 + org.jbox2d.common.MathUtils.max(child1.height, child2.height);

        index = index.parent;
      }
    } else {
      m_root = sibling;
      sibling.parent = null;
      freeNode(parent);
    }

    // validate();
  }
Example #10
0
  public void create(Body body, FixtureDef def) {
    m_userData = def.userData;
    m_friction = def.friction;
    m_restitution = def.restitution;

    m_body = body;
    m_next = null;

    m_filter.set(def.filter);

    m_isSensor = def.isSensor;

    m_shape = def.shape.clone();

    // moj doplneny kod
    m_material = def.material;
    m_polygon = def.polygon;

    // Reserve proxy space
    int childCount = m_shape.getChildCount();
    if (m_proxies == null) {
      m_proxies = new FixtureProxy[childCount];
      for (int i = 0; i < childCount; i++) {
        m_proxies[i] = new FixtureProxy();
        m_proxies[i].fixture = null;
        m_proxies[i].proxyId = BroadPhase.NULL_PROXY;
      }
    }

    if (m_proxies.length < childCount) {
      FixtureProxy[] old = m_proxies;
      int newLen = MathUtils.max(old.length * 2, childCount);
      m_proxies = new FixtureProxy[newLen];
      System.arraycopy(old, 0, m_proxies, 0, old.length);
      for (int i = 0; i < newLen; i++) {
        if (i >= old.length) {
          m_proxies[i] = new FixtureProxy();
        }
        m_proxies[i].fixture = null;
        m_proxies[i].proxyId = BroadPhase.NULL_PROXY;
      }
    }
    m_proxyCount = 0;

    m_density = def.density;
  }
Example #11
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;
    }
  }
  /**
   * Create a convex hull from the given array of points. The count must be in the range [3,
   * Settings.maxPolygonVertices]. This method takes an arraypool for pooling
   *
   * @warning the points may be re-ordered, even if they form a convex polygon
   * @warning collinear points are handled but not removed. Collinear points may lead to poor
   *     stacking behavior.
   */
  public final void set(
      final Vec2[] verts, final int num, final Vec2Array vecPool, final IntArray intPool) {
    assert (3 <= num && num <= Settings.maxPolygonVertices);
    if (num < 3) {
      setAsBox(1.0f, 1.0f);
      return;
    }

    int n = MathUtils.min(num, Settings.maxPolygonVertices);

    // Copy the vertices into a local buffer
    Vec2[] ps = (vecPool != null) ? vecPool.get(n) : new Vec2[n];
    for (int i = 0; i < n; ++i) {
      ps[i] = verts[i];
    }

    // Create the convex hull using the Gift wrapping algorithm
    // http://en.wikipedia.org/wiki/Gift_wrapping_algorithm

    // Find the right most point on the hull
    int i0 = 0;
    float x0 = ps[0].x;
    for (int i = 1; i < num; ++i) {
      float x = ps[i].x;
      if (x > x0 || (x == x0 && ps[i].y < ps[i0].y)) {
        i0 = i;
        x0 = x;
      }
    }

    int[] hull =
        (intPool != null)
            ? intPool.get(Settings.maxPolygonVertices)
            : new int[Settings.maxPolygonVertices];
    int m = 0;
    int ih = i0;

    while (true) {
      hull[m] = ih;

      int ie = 0;
      for (int j = 1; j < n; ++j) {
        if (ie == ih) {
          ie = j;
          continue;
        }

        Vec2 r = pool1.set(ps[ie]).subLocal(ps[hull[m]]);
        Vec2 v = pool2.set(ps[j]).subLocal(ps[hull[m]]);
        float c = Vec2.cross(r, v);
        if (c < 0.0f) {
          ie = j;
        }

        // Collinearity check
        if (c == 0.0f && v.lengthSquared() > r.lengthSquared()) {
          ie = j;
        }
      }

      ++m;
      ih = ie;

      if (ie == i0) {
        break;
      }
    }

    this.m_count = m;

    // Copy vertices.
    for (int i = 0; i < m_count; ++i) {
      if (m_vertices[i] == null) {
        m_vertices[i] = new Vec2();
      }
      m_vertices[i].set(ps[hull[i]]);
    }

    final Vec2 edge = pool1;

    // Compute normals. Ensure the edges have non-zero length.
    for (int i = 0; i < m_count; ++i) {
      final int i1 = i;
      final int i2 = i + 1 < m_count ? i + 1 : 0;
      edge.set(m_vertices[i2]).subLocal(m_vertices[i1]);

      assert (edge.lengthSquared() > Settings.EPSILON * Settings.EPSILON);
      Vec2.crossToOutUnsafe(edge, 1f, m_normals[i]);
      m_normals[i].normalize();
    }

    // Compute the polygon centroid.
    computeCentroidToOut(m_vertices, m_count, m_centroid);
  }
Example #13
0
  @Override
  public void initVelocityConstraints(final org.jbox2d.dynamics.SolverData data) {
    m_indexA = m_bodyA.m_islandIndex;
    m_indexB = m_bodyB.m_islandIndex;
    m_localCenterA.set(m_bodyA.m_sweep.localCenter);
    m_localCenterB.set(m_bodyB.m_sweep.localCenter);
    m_invMassA = m_bodyA.m_invMass;
    m_invMassB = m_bodyB.m_invMass;
    m_invIA = m_bodyA.m_invI;
    m_invIB = m_bodyB.m_invI;

    // Vec2 cA = data.positions[m_indexA].c;
    float aA = data.positions[m_indexA].a;
    Vec2 vA = data.velocities[m_indexA].v;
    float wA = data.velocities[m_indexA].w;

    // Vec2 cB = data.positions[m_indexB].c;
    float aB = data.positions[m_indexB].a;
    Vec2 vB = data.velocities[m_indexB].v;
    float wB = data.velocities[m_indexB].w;
    final Rot qA = pool.popRot();
    final Rot qB = pool.popRot();
    final Vec2 temp = pool.popVec2();

    qA.set(aA);
    qB.set(aB);

    // Compute the effective masses.
    Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), m_rA);
    Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), m_rB);

    // J = [-I -r1_skew I r2_skew]
    // [ 0 -1 0 1]
    // r_skew = [-ry; rx]

    // Matlab
    // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
    // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
    // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]

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

    boolean fixedRotation = (iA + iB == 0.0f);

    m_mass.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;
    m_mass.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;
    m_mass.ez.x = -m_rA.y * iA - m_rB.y * iB;
    m_mass.ex.y = m_mass.ey.x;
    m_mass.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;
    m_mass.ez.y = m_rA.x * iA + m_rB.x * iB;
    m_mass.ex.z = m_mass.ez.x;
    m_mass.ey.z = m_mass.ez.y;
    m_mass.ez.z = iA + iB;

    m_motorMass = iA + iB;
    if (m_motorMass > 0.0f) {
      m_motorMass = 1.0f / m_motorMass;
    }

    if (m_enableMotor == false || fixedRotation) {
      m_motorImpulse = 0.0f;
    }

    if (m_enableLimit && fixedRotation == false) {
      float jointAngle = aB - aA - m_referenceAngle;
      if (MathUtils.abs(m_upperAngle - m_lowerAngle) < 2.0f * Settings.angularSlop) {
        m_limitState = LimitState.EQUAL;
      } else if (jointAngle <= m_lowerAngle) {
        if (m_limitState != LimitState.AT_LOWER) {
          m_impulse.z = 0.0f;
        }
        m_limitState = LimitState.AT_LOWER;
      } else if (jointAngle >= m_upperAngle) {
        if (m_limitState != LimitState.AT_UPPER) {
          m_impulse.z = 0.0f;
        }
        m_limitState = LimitState.AT_UPPER;
      } else {
        m_limitState = LimitState.INACTIVE;
        m_impulse.z = 0.0f;
      }
    } else {
      m_limitState = LimitState.INACTIVE;
    }

    if (data.step.warmStarting) {
      final Vec2 P = pool.popVec2();
      // Scale impulses to support a variable time step.
      m_impulse.x *= data.step.dtRatio;
      m_impulse.y *= data.step.dtRatio;
      m_motorImpulse *= data.step.dtRatio;

      P.x = m_impulse.x;
      P.y = m_impulse.y;

      vA.x -= mA * P.x;
      vA.y -= mA * P.y;
      wA -= iA * (Vec2.cross(m_rA, P) + m_motorImpulse + m_impulse.z);

      vB.x += mB * P.x;
      vB.y += mB * P.y;
      wB += iB * (Vec2.cross(m_rB, P) + m_motorImpulse + m_impulse.z);
      pool.pushVec2(1);
    } else {
      m_impulse.setZero();
      m_motorImpulse = 0.0f;
    }
    // data.velocities[m_indexA].v.set(vA);
    data.velocities[m_indexA].w = wA;
    // data.velocities[m_indexB].v.set(vB);
    data.velocities[m_indexB].w = wB;

    pool.pushVec2(1);
    pool.pushRot(2);
  }
Example #14
0
  // Perform a left or right rotation if node A is imbalanced.
  // Returns the new root index.
  private DynamicTreeNode balance(DynamicTreeNode iA) {
    assert (iA != null);

    DynamicTreeNode A = iA;
    if (A.child1 == null || A.height < 2) {
      return iA;
    }

    DynamicTreeNode iB = A.child1;
    DynamicTreeNode iC = A.child2;
    assert (0 <= iB.id && iB.id < m_nodeCapacity);
    assert (0 <= iC.id && iC.id < m_nodeCapacity);

    DynamicTreeNode B = iB;
    DynamicTreeNode C = iC;

    int balance = C.height - B.height;

    // Rotate C up
    if (balance > 1) {
      DynamicTreeNode iF = C.child1;
      DynamicTreeNode iG = C.child2;
      DynamicTreeNode F = iF;
      DynamicTreeNode G = iG;
      assert (F != null);
      assert (G != null);
      assert (0 <= iF.id && iF.id < m_nodeCapacity);
      assert (0 <= iG.id && iG.id < m_nodeCapacity);

      // Swap A and C
      C.child1 = iA;
      C.parent = A.parent;
      A.parent = iC;

      // A's old parent should point to C
      if (C.parent != null) {
        if (C.parent.child1 == iA) {
          C.parent.child1 = iC;
        } else {
          assert (C.parent.child2 == iA);
          C.parent.child2 = iC;
        }
      } else {
        m_root = iC;
      }

      // Rotate
      if (F.height > G.height) {
        C.child2 = iF;
        A.child2 = iG;
        G.parent = iA;
        A.aabb.combine(B.aabb, G.aabb);
        C.aabb.combine(A.aabb, F.aabb);

        A.height = 1 + org.jbox2d.common.MathUtils.max(B.height, G.height);
        C.height = 1 + org.jbox2d.common.MathUtils.max(A.height, F.height);
      } else {
        C.child2 = iG;
        A.child2 = iF;
        F.parent = iA;
        A.aabb.combine(B.aabb, F.aabb);
        C.aabb.combine(A.aabb, G.aabb);

        A.height = 1 + org.jbox2d.common.MathUtils.max(B.height, F.height);
        C.height = 1 + org.jbox2d.common.MathUtils.max(A.height, G.height);
      }

      return iC;
    }

    // Rotate B up
    if (balance < -1) {
      DynamicTreeNode iD = B.child1;
      DynamicTreeNode iE = B.child2;
      DynamicTreeNode D = iD;
      DynamicTreeNode E = iE;
      assert (0 <= iD.id && iD.id < m_nodeCapacity);
      assert (0 <= iE.id && iE.id < m_nodeCapacity);

      // Swap A and B
      B.child1 = iA;
      B.parent = A.parent;
      A.parent = iB;

      // A's old parent should point to B
      if (B.parent != null) {
        if (B.parent.child1 == iA) {
          B.parent.child1 = iB;
        } else {
          assert (B.parent.child2 == iA);
          B.parent.child2 = iB;
        }
      } else {
        m_root = iB;
      }

      // Rotate
      if (D.height > E.height) {
        B.child2 = iD;
        A.child1 = iE;
        E.parent = iA;
        A.aabb.combine(C.aabb, E.aabb);
        B.aabb.combine(A.aabb, D.aabb);

        A.height = 1 + org.jbox2d.common.MathUtils.max(C.height, E.height);
        B.height = 1 + org.jbox2d.common.MathUtils.max(A.height, D.height);
      } else {
        B.child2 = iE;
        A.child1 = iD;
        D.parent = iA;
        A.aabb.combine(C.aabb, D.aabb);
        B.aabb.combine(A.aabb, E.aabb);

        A.height = 1 + org.jbox2d.common.MathUtils.max(C.height, D.height);
        B.height = 1 + org.jbox2d.common.MathUtils.max(A.height, E.height);
      }

      return iB;
    }

    return iA;
  }
Example #15
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;
  }
Example #16
0
  /** Build an optimal tree. Very expensive. For testing. */
  public void rebuildBottomUp() {
    int[] nodes = new int[m_nodeCount];
    int count = 0;

    // Build array of leaves. Free the rest.
    for (int i = 0; i < m_nodeCapacity; ++i) {
      if (m_nodes[i].height < 0) {
        // free node in pool
        continue;
      }

      DynamicTreeNode node = m_nodes[i];
      if (node.child1 == null) {
        node.parent = null;
        nodes[count] = i;
        ++count;
      } else {
        freeNode(node);
      }
    }

    org.jbox2d.collision.AABB b = new org.jbox2d.collision.AABB();
    while (count > 1) {
      float minCost = Float.MAX_VALUE;
      int iMin = -1, jMin = -1;
      for (int i = 0; i < count; ++i) {
        org.jbox2d.collision.AABB aabbi = m_nodes[nodes[i]].aabb;

        for (int j = i + 1; j < count; ++j) {
          org.jbox2d.collision.AABB aabbj = m_nodes[nodes[j]].aabb;
          b.combine(aabbi, aabbj);
          float cost = b.getPerimeter();
          if (cost < minCost) {
            iMin = i;
            jMin = j;
            minCost = cost;
          }
        }
      }

      int index1 = nodes[iMin];
      int index2 = nodes[jMin];
      DynamicTreeNode child1 = m_nodes[index1];
      DynamicTreeNode child2 = m_nodes[index2];

      DynamicTreeNode parent = allocateNode();
      parent.child1 = child1;
      parent.child2 = child2;
      parent.height = 1 + org.jbox2d.common.MathUtils.max(child1.height, child2.height);
      parent.aabb.combine(child1.aabb, child2.aabb);
      parent.parent = null;

      child1.parent = parent;
      child2.parent = parent;

      nodes[jMin] = nodes[count - 1];
      nodes[iMin] = parent.id;
      --count;
    }

    m_root = m_nodes[nodes[0]];

    validate();
  }
Example #17
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;
      }
    }
  }
Example #18
0
  private void solveTOI(final TimeStep step) {

    final Island island = toiIsland;
    island.init(
        2 * Settings.maxTOIContacts,
        Settings.maxTOIContacts,
        0,
        m_contactManager.m_contactListener);
    if (m_stepComplete) {
      for (Body b = m_bodyList; b != null; b = b.m_next) {
        b.m_flags &= ~Body.e_islandFlag;
        b.m_sweep.alpha0 = 0.0f;
      }

      for (Contact c = m_contactManager.m_contactList; c != null; c = c.m_next) {
        // Invalidate TOI
        c.m_flags &= ~(Contact.TOI_FLAG | Contact.ISLAND_FLAG);
        c.m_toiCount = 0;
        c.m_toi = 1.0f;
      }
    }

    // Find TOI events and solve them.
    for (; ; ) {
      // Find the first TOI.
      Contact minContact = null;
      float minAlpha = 1.0f;

      for (Contact c = m_contactManager.m_contactList; c != null; c = c.m_next) {
        // Is this contact disabled?
        if (c.isEnabled() == false) {
          continue;
        }

        // Prevent excessive sub-stepping.
        if (c.m_toiCount > Settings.maxSubSteps) {
          continue;
        }

        float alpha = 1.0f;
        if ((c.m_flags & Contact.TOI_FLAG) != 0) {
          // This contact has a valid cached TOI.
          alpha = c.m_toi;
        } else {
          Fixture fA = c.getFixtureA();
          Fixture fB = c.getFixtureB();

          // Is there a sensor?
          if (fA.isSensor() || fB.isSensor()) {
            continue;
          }

          Body bA = fA.getBody();
          Body bB = fB.getBody();

          BodyType typeA = bA.m_type;
          BodyType typeB = bB.m_type;
          assert (typeA == BodyType.DYNAMIC || typeB == BodyType.DYNAMIC);

          boolean activeA = bA.isAwake() && typeA != BodyType.STATIC;
          boolean activeB = bB.isAwake() && typeB != BodyType.STATIC;

          // Is at least one body active (awake and dynamic or kinematic)?
          if (activeA == false && activeB == false) {
            continue;
          }

          boolean collideA = bA.isBullet() || typeA != BodyType.DYNAMIC;
          boolean collideB = bB.isBullet() || typeB != BodyType.DYNAMIC;

          // Are these two non-bullet dynamic bodies?
          if (collideA == false && collideB == false) {
            continue;
          }

          // Compute the TOI for this contact.
          // Put the sweeps onto the same time interval.
          float alpha0 = bA.m_sweep.alpha0;

          if (bA.m_sweep.alpha0 < bB.m_sweep.alpha0) {
            alpha0 = bB.m_sweep.alpha0;
            bA.m_sweep.advance(alpha0);
          } else if (bB.m_sweep.alpha0 < bA.m_sweep.alpha0) {
            alpha0 = bA.m_sweep.alpha0;
            bB.m_sweep.advance(alpha0);
          }

          assert (alpha0 < 1.0f);

          int indexA = c.getChildIndexA();
          int indexB = c.getChildIndexB();

          // Compute the time of impact in interval [0, minTOI]
          final TOIInput input = toiInput;
          input.proxyA.set(fA.getShape(), indexA);
          input.proxyB.set(fB.getShape(), indexB);
          input.sweepA.set(bA.m_sweep);
          input.sweepB.set(bB.m_sweep);
          input.tMax = 1.0f;

          pool.getTimeOfImpact().timeOfImpact(toiOutput, input);

          // Beta is the fraction of the remaining portion of the .
          float beta = toiOutput.t;
          if (toiOutput.state == TOIOutputState.TOUCHING) {
            alpha = MathUtils.min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
          } else {
            alpha = 1.0f;
          }

          c.m_toi = alpha;
          c.m_flags |= Contact.TOI_FLAG;
        }

        if (alpha < minAlpha) {
          // This is the minimum TOI found so far.
          minContact = c;
          minAlpha = alpha;
        }
      }

      if (minContact == null || 1.0f - 10.0f * Settings.EPSILON < minAlpha) {
        // No more TOI events. Done!
        m_stepComplete = true;
        break;
      }

      // Advance the bodies to the TOI.
      Fixture fA = minContact.getFixtureA();
      Fixture fB = minContact.getFixtureB();
      Body bA = fA.getBody();
      Body bB = fB.getBody();

      backup1.set(bA.m_sweep);
      backup2.set(bB.m_sweep);

      bA.advance(minAlpha);
      bB.advance(minAlpha);

      // The TOI contact likely has some new contact points.
      minContact.update(m_contactManager.m_contactListener);
      minContact.m_flags &= ~Contact.TOI_FLAG;
      ++minContact.m_toiCount;

      // Is the contact solid?
      if (minContact.isEnabled() == false || minContact.isTouching() == false) {
        // Restore the sweeps.
        minContact.setEnabled(false);
        bA.m_sweep.set(backup1);
        bB.m_sweep.set(backup2);
        bA.synchronizeTransform();
        bB.synchronizeTransform();
        continue;
      }

      bA.setAwake(true);
      bB.setAwake(true);

      // Build the island
      island.clear();
      island.add(bA);
      island.add(bB);
      island.add(minContact);

      bA.m_flags |= Body.e_islandFlag;
      bB.m_flags |= Body.e_islandFlag;
      minContact.m_flags |= Contact.ISLAND_FLAG;

      // Get contacts on bodyA and bodyB.
      tempBodies[0] = bA;
      tempBodies[1] = bB;
      for (int i = 0; i < 2; ++i) {
        Body body = tempBodies[i];
        if (body.m_type == BodyType.DYNAMIC) {
          for (ContactEdge ce = body.m_contactList; ce != null; ce = ce.next) {
            if (island.m_bodyCount == island.m_bodyCapacity) {
              break;
            }

            if (island.m_contactCount == island.m_contactCapacity) {
              break;
            }

            Contact contact = ce.contact;

            // Has this contact already been added to the island?
            if ((contact.m_flags & Contact.ISLAND_FLAG) != 0) {
              continue;
            }

            // Only add static, kinematic, or bullet bodies.
            Body other = ce.other;
            if (other.m_type == BodyType.DYNAMIC
                && body.isBullet() == false
                && other.isBullet() == false) {
              continue;
            }

            // Skip sensors.
            boolean sensorA = contact.m_fixtureA.m_isSensor;
            boolean sensorB = contact.m_fixtureB.m_isSensor;
            if (sensorA || sensorB) {
              continue;
            }

            // Tentatively advance the body to the TOI.
            backup1.set(other.m_sweep);
            if ((other.m_flags & Body.e_islandFlag) == 0) {
              other.advance(minAlpha);
            }

            // Update the contact points
            contact.update(m_contactManager.m_contactListener);

            // Was the contact disabled by the user?
            if (contact.isEnabled() == false) {
              other.m_sweep.set(backup1);
              other.synchronizeTransform();
              continue;
            }

            // Are there contact points?
            if (contact.isTouching() == false) {
              other.m_sweep.set(backup1);
              other.synchronizeTransform();
              continue;
            }

            // Add the contact to the island
            contact.m_flags |= Contact.ISLAND_FLAG;
            island.add(contact);

            // Has the other body already been added to the island?
            if ((other.m_flags & Body.e_islandFlag) != 0) {
              continue;
            }

            // Add the other body to the island.
            other.m_flags |= Body.e_islandFlag;

            if (other.m_type != BodyType.STATIC) {
              other.setAwake(true);
            }

            island.add(other);
          }
        }
      }

      subStep.dt = (1.0f - minAlpha) * step.dt;
      subStep.inv_dt = 1.0f / subStep.dt;
      subStep.dtRatio = 1.0f;
      subStep.positionIterations = 20;
      subStep.velocityIterations = step.velocityIterations;
      subStep.warmStarting = false;
      island.solveTOI(subStep, bA.m_islandIndex, bB.m_islandIndex);

      // Reset island flags and synchronize broad-phase proxies.
      for (int i = 0; i < island.m_bodyCount; ++i) {
        Body body = island.m_bodies[i];
        body.m_flags &= ~Body.e_islandFlag;

        if (body.m_type != BodyType.DYNAMIC) {
          continue;
        }

        body.synchronizeFixtures();

        // Invalidate all contact TOIs on this displaced body.
        for (ContactEdge ce = body.m_contactList; ce != null; ce = ce.next) {
          ce.contact.m_flags &= ~(Contact.TOI_FLAG | Contact.ISLAND_FLAG);
        }
      }

      // Commit fixture proxy movements to the broad-phase so that new contacts are created.
      // Also, some contacts can be destroyed.
      m_contactManager.findNewContacts();

      if (m_subStepping) {
        m_stepComplete = false;
        break;
      }
    }
  }
Example #19
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 #20
0
  private final void insertLeaf(int leaf_index) {
    DynamicTreeNode leaf = m_nodes[leaf_index];
    if (m_root == null) {
      m_root = leaf;
      m_root.parent = null;
      return;
    }

    // find the best sibling
    org.jbox2d.collision.AABB leafAABB = leaf.aabb;
    DynamicTreeNode index = m_root;
    while (index.child1 != null) {
      final DynamicTreeNode node = index;
      DynamicTreeNode child1 = node.child1;
      DynamicTreeNode child2 = node.child2;

      float area = node.aabb.getPerimeter();

      combinedAABB.combine(node.aabb, leafAABB);
      float combinedArea = combinedAABB.getPerimeter();

      // Cost of creating a new parent for this node and the new leaf
      float cost = 2.0f * combinedArea;

      // Minimum cost of pushing the leaf further down the tree
      float inheritanceCost = 2.0f * (combinedArea - area);

      // Cost of descending into child1
      float cost1;
      if (child1.child1 == null) {
        combinedAABB.combine(leafAABB, child1.aabb);
        cost1 = combinedAABB.getPerimeter() + inheritanceCost;
      } else {
        combinedAABB.combine(leafAABB, child1.aabb);
        float oldArea = child1.aabb.getPerimeter();
        float newArea = combinedAABB.getPerimeter();
        cost1 = (newArea - oldArea) + inheritanceCost;
      }

      // Cost of descending into child2
      float cost2;
      if (child2.child1 == null) {
        combinedAABB.combine(leafAABB, child2.aabb);
        cost2 = combinedAABB.getPerimeter() + inheritanceCost;
      } else {
        combinedAABB.combine(leafAABB, child2.aabb);
        float oldArea = child2.aabb.getPerimeter();
        float newArea = combinedAABB.getPerimeter();
        cost2 = newArea - oldArea + inheritanceCost;
      }

      // Descend according to the minimum cost.
      if (cost < cost1 && cost < cost2) {
        break;
      }

      // Descend
      if (cost1 < cost2) {
        index = child1;
      } else {
        index = child2;
      }
    }

    DynamicTreeNode sibling = index;
    DynamicTreeNode oldParent = m_nodes[sibling.id].parent;
    final DynamicTreeNode newParent = allocateNode();
    newParent.parent = oldParent;
    newParent.userData = null;
    newParent.aabb.combine(leafAABB, sibling.aabb);
    newParent.height = sibling.height + 1;

    if (oldParent != null) {
      // The sibling was not the root.
      if (oldParent.child1 == sibling) {
        oldParent.child1 = newParent;
      } else {
        oldParent.child2 = newParent;
      }

      newParent.child1 = sibling;
      newParent.child2 = leaf;
      sibling.parent = newParent;
      leaf.parent = newParent;
    } else {
      // The sibling was the root.
      newParent.child1 = sibling;
      newParent.child2 = leaf;
      sibling.parent = newParent;
      leaf.parent = newParent;
      m_root = newParent;
    }

    // Walk back up the tree fixing heights and AABBs
    index = leaf.parent;
    while (index != null) {
      index = balance(index);

      DynamicTreeNode child1 = index.child1;
      DynamicTreeNode child2 = index.child2;

      assert (child1 != null);
      assert (child2 != null);

      index.height = 1 + org.jbox2d.common.MathUtils.max(child1.height, child2.height);
      index.aabb.combine(child1.aabb, child2.aabb);

      index = index.parent;
    }
    // validate();
  }
Example #21
0
  @Override
  public void solveVelocityConstraints(final org.jbox2d.dynamics.SolverData data) {
    Vec2 vA = data.velocities[m_indexA].v;
    float wA = data.velocities[m_indexA].w;
    Vec2 vB = data.velocities[m_indexB].v;
    float wB = data.velocities[m_indexB].w;

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

    boolean fixedRotation = (iA + iB == 0.0f);

    // Solve motor constraint.
    if (m_enableMotor && m_limitState != LimitState.EQUAL && fixedRotation == false) {
      float Cdot = wB - wA - m_motorSpeed;
      float impulse = -m_motorMass * Cdot;
      float oldImpulse = m_motorImpulse;
      float maxImpulse = data.step.dt * m_maxMotorTorque;
      m_motorImpulse = MathUtils.clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
      impulse = m_motorImpulse - oldImpulse;

      wA -= iA * impulse;
      wB += iB * impulse;
    }
    final Vec2 temp = pool.popVec2();

    // Solve limit constraint.
    if (m_enableLimit && m_limitState != LimitState.INACTIVE && fixedRotation == false) {

      final Vec2 Cdot1 = pool.popVec2();
      final Vec3 Cdot = pool.popVec3();

      // Solve point-to-point constraint
      Vec2.crossToOutUnsafe(wA, m_rA, temp);
      Vec2.crossToOutUnsafe(wB, m_rB, Cdot1);
      Cdot1.addLocal(vB).subLocal(vA).subLocal(temp);
      float Cdot2 = wB - wA;
      Cdot.set(Cdot1.x, Cdot1.y, Cdot2);

      Vec3 impulse = pool.popVec3();
      m_mass.solve33ToOut(Cdot, impulse);
      impulse.negateLocal();

      if (m_limitState == LimitState.EQUAL) {
        m_impulse.addLocal(impulse);
      } else if (m_limitState == LimitState.AT_LOWER) {
        float newImpulse = m_impulse.z + impulse.z;
        if (newImpulse < 0.0f) {
          final Vec2 rhs = pool.popVec2();
          rhs.set(m_mass.ez.x, m_mass.ez.y).mulLocal(m_impulse.z).subLocal(Cdot1);
          m_mass.solve22ToOut(rhs, temp);
          impulse.x = temp.x;
          impulse.y = temp.y;
          impulse.z = -m_impulse.z;
          m_impulse.x += temp.x;
          m_impulse.y += temp.y;
          m_impulse.z = 0.0f;
          pool.pushVec2(1);
        } else {
          m_impulse.addLocal(impulse);
        }
      } else if (m_limitState == LimitState.AT_UPPER) {
        float newImpulse = m_impulse.z + impulse.z;
        if (newImpulse > 0.0f) {
          final Vec2 rhs = pool.popVec2();
          rhs.set(m_mass.ez.x, m_mass.ez.y).mulLocal(m_impulse.z).subLocal(Cdot1);
          m_mass.solve22ToOut(rhs, temp);
          impulse.x = temp.x;
          impulse.y = temp.y;
          impulse.z = -m_impulse.z;
          m_impulse.x += temp.x;
          m_impulse.y += temp.y;
          m_impulse.z = 0.0f;
          pool.pushVec2(1);
        } else {
          m_impulse.addLocal(impulse);
        }
      }
      final Vec2 P = pool.popVec2();

      P.set(impulse.x, impulse.y);

      vA.x -= mA * P.x;
      vA.y -= mA * P.y;
      wA -= iA * (Vec2.cross(m_rA, P) + impulse.z);

      vB.x += mB * P.x;
      vB.y += mB * P.y;
      wB += iB * (Vec2.cross(m_rB, P) + impulse.z);

      pool.pushVec2(2);
      pool.pushVec3(2);
    } else {

      // Solve point-to-point constraint
      Vec2 Cdot = pool.popVec2();
      Vec2 impulse = pool.popVec2();

      Vec2.crossToOutUnsafe(wA, m_rA, temp);
      Vec2.crossToOutUnsafe(wB, m_rB, Cdot);
      Cdot.addLocal(vB).subLocal(vA).subLocal(temp);
      m_mass.solve22ToOut(Cdot.negateLocal(), impulse); // just leave negated

      m_impulse.x += impulse.x;
      m_impulse.y += impulse.y;

      vA.x -= mA * impulse.x;
      vA.y -= mA * impulse.y;
      wA -= iA * Vec2.cross(m_rA, impulse);

      vB.x += mB * impulse.x;
      vB.y += mB * impulse.y;
      wB += iB * Vec2.cross(m_rB, impulse);

      pool.pushVec2(2);
    }

    // data.velocities[m_indexA].v.set(vA);
    data.velocities[m_indexA].w = wA;
    // data.velocities[m_indexB].v.set(vB);
    data.velocities[m_indexB].w = wB;

    pool.pushVec2(1);
  }