@Override
  public final void computeAABB(final AABB aabb, final Transform xf, int childIndex) {
    final Vec2 v = pool1;
    final Vec2 lower = aabb.lowerBound;
    final Vec2 upper = aabb.upperBound;
    final Vec2 v1 = m_vertices[0];
    lower.x = (xf.q.c * v1.x - xf.q.s * v1.y) + xf.p.x;
    lower.y = (xf.q.s * v1.x + xf.q.c * v1.y) + xf.p.y;
    upper.set(lower);

    for (int i = 1; i < m_count; ++i) {
      Vec2 v2 = m_vertices[i];
      v.x = (xf.q.c * v2.x - xf.q.s * v2.y) + xf.p.x;
      v.y = (xf.q.s * v2.x + xf.q.c * v2.y) + xf.p.y;
      // Vec2 v = Mul(xf, m_vertices[i]);
      Vec2.minToOut(lower, v, lower);
      Vec2.maxToOut(upper, v, upper);
    }

    // Vec2 r(m_radius, m_radius);
    // aabb.lowerBound = lower - r;
    // aabb.upperBound = upper + r;

    aabb.lowerBound.x -= m_radius;
    aabb.lowerBound.y -= m_radius;
    aabb.upperBound.x += m_radius;
    aabb.upperBound.y += m_radius;
  }
示例#2
0
  /**
   * Internal method
   *
   * @param broadPhase
   * @param xf1
   * @param xf2
   */
  protected void synchronize(
      BroadPhase broadPhase, final Transform transform1, final Transform transform2) {
    if (m_proxyCount == 0) {
      return;
    }

    for (int i = 0; i < m_proxyCount; ++i) {
      FixtureProxy proxy = m_proxies[i];

      // Compute an AABB that covers the swept shape (may miss some rotation effect).
      final AABB aabb1 = pool1;
      final AABB aab = pool2;
      m_shape.computeAABB(aabb1, transform1, proxy.childIndex);
      m_shape.computeAABB(aab, transform2, proxy.childIndex);

      proxy.aabb.lowerBound.x =
          aabb1.lowerBound.x < aab.lowerBound.x ? aabb1.lowerBound.x : aab.lowerBound.x;
      proxy.aabb.lowerBound.y =
          aabb1.lowerBound.y < aab.lowerBound.y ? aabb1.lowerBound.y : aab.lowerBound.y;
      proxy.aabb.upperBound.x =
          aabb1.upperBound.x > aab.upperBound.x ? aabb1.upperBound.x : aab.upperBound.x;
      proxy.aabb.upperBound.y =
          aabb1.upperBound.y > aab.upperBound.y ? aabb1.upperBound.y : aab.upperBound.y;
      displacement.x = transform2.p.x - transform1.p.x;
      displacement.y = transform2.p.y - transform1.p.y;

      broadPhase.moveProxy(proxy.proxyId, proxy.aabb, displacement);
    }
  }
示例#3
0
文件: Player.java 项目: kotucz/ld26
  public void jump() {
    Vec2 linearVelocity = getBody().getLinearVelocity();
    linearVelocity.y = JUMP_SPEED;
    getBody().setLinearVelocity(linearVelocity);

    SOUND_JUMP.play();
  }
示例#4
0
文件: Body.java 项目: rdo/speleo
 /**
  * Set the linear velocity of this body
  *
  * @param xVelocity The x component of the velocity
  * @param yVelocity The y component of the velocity
  */
 public void setVelocity(float xVelocity, float yVelocity) {
   checkBody();
   Vec2 vel = jboxBody.getLinearVelocity();
   vel.x = xVelocity;
   vel.y = yVelocity;
   jboxBody.setLinearVelocity(vel);
 }
示例#5
0
  @Override
  public final boolean moveProxy(
      int proxyId, final org.jbox2d.collision.AABB aabb, Vec2 displacement) {
    assert (aabb.isValid());
    assert (0 <= proxyId && proxyId < m_nodeCapacity);
    final DynamicTreeNode node = m_nodes[proxyId];
    assert (node.child1 == null);

    final org.jbox2d.collision.AABB nodeAABB = node.aabb;
    // if (nodeAABB.contains(aabb)) {
    if (nodeAABB.lowerBound.x <= aabb.lowerBound.x
        && nodeAABB.lowerBound.y <= aabb.lowerBound.y
        && aabb.upperBound.x <= nodeAABB.upperBound.x
        && aabb.upperBound.y <= nodeAABB.upperBound.y) {
      return false;
    }

    removeLeaf(node);

    // Extend AABB
    final Vec2 lowerBound = nodeAABB.lowerBound;
    final Vec2 upperBound = nodeAABB.upperBound;
    lowerBound.x = aabb.lowerBound.x - Settings.aabbExtension;
    lowerBound.y = aabb.lowerBound.y - Settings.aabbExtension;
    upperBound.x = aabb.upperBound.x + Settings.aabbExtension;
    upperBound.y = aabb.upperBound.y + Settings.aabbExtension;

    // Predict AABB displacement.
    final float dx = displacement.x * Settings.aabbMultiplier;
    final float dy = displacement.y * Settings.aabbMultiplier;
    if (dx < 0.0f) {
      lowerBound.x += dx;
    } else {
      upperBound.x += dx;
    }

    if (dy < 0.0f) {
      lowerBound.y += dy;
    } else {
      upperBound.y += dy;
    }

    insertLeaf(proxyId);
    return true;
  }
示例#6
0
  /**
   * Get a direction vector from start point, within screens height and width
   *
   * @return
   */
  public Vec2 getStartDirVec(float screenWidth, float screenHeight) {
    Vec2 start = this.getBody().getWorldCenter().mul(Globals.PHYS_RATIO);
    Vec2 dir = new Vec2();

    // Find where the screen is, so Vec can go through it
    if (start.x <= -screenWidth) {
      dir.x = 1;
    } else if (start.x >= screenWidth) {
      dir.x = -1;
    }

    if (start.y <= -screenHeight) {
      dir.y = 1;
    } else if (start.y >= screenHeight) {
      dir.y = -1;
    }

    return dir;
  }
  @Override
  public void onCreate(Bundle savedInstanceState) {
    super.onCreate(savedInstanceState);
    view = new TestView(this);
    setContentView(view);
    // new World(new Vec2(0.0f, -10.0f), true);
    gravity = getPreferences(Context.MODE_PRIVATE).getInt(PREF_GRAVITY, GRAVITY_ACCELEROMETER);
    w = (PhysicsWorld) getLastNonConfigurationInstance();
    if (w == null) {
      Vec2 grav = new Vec2(0.0f, 0.0f);
      if (gravity == GRAVITY_NORMAL) {
        grav.y = -10.0f;
      } else if (gravity == GRAVITY_MOON) {
        grav.y = -1.67f;
      }
      w = new PhysicsWorld();
      w.create(grav);
    }
    view.setModel(w);
    if (savedInstanceState != null) {
      defaultLandscape = savedInstanceState.getBoolean(KEY_DEFAULT_LANDSCAPE);

    } else {
      defaultLandscape =
          getResources().getConfiguration().orientation == Configuration.ORIENTATION_LANDSCAPE;

      rotation = getPreferences(Context.MODE_PRIVATE).getInt(PREF_ROTATION, Surface.ROTATION_0);
      int orientation;
      if (defaultLandscape && rotation == Surface.ROTATION_90
          || !defaultLandscape && rotation == Surface.ROTATION_180) {
        orientation = 9;
      } else if (defaultLandscape && rotation == Surface.ROTATION_180
          || !defaultLandscape && rotation == Surface.ROTATION_270) {
        orientation = 8;
      } else if (defaultLandscape && rotation == Surface.ROTATION_270
          || !defaultLandscape && rotation == Surface.ROTATION_0) {
        orientation = ActivityInfo.SCREEN_ORIENTATION_PORTRAIT;
      } else {
        orientation = ActivityInfo.SCREEN_ORIENTATION_LANDSCAPE;
      }
      setRequestedOrientation(orientation);
    }
  }
示例#8
0
  @Override
  public void drawTransform(Transform xf) {
    Graphics2D g = getGraphics();
    getWorldToScreenToOut(xf.p, temp);
    temp2.setZero();
    float k_axisScale = 0.4f;

    Color c = cpool.getColor(1, 0, 0);
    g.setColor(c);

    temp2.x = xf.p.x + k_axisScale * xf.q.c;
    temp2.y = xf.p.y + k_axisScale * xf.q.s;
    getWorldToScreenToOut(temp2, temp2);
    g.drawLine((int) temp.x, (int) temp.y, (int) temp2.x, (int) temp2.y);

    c = cpool.getColor(0, 1, 0);
    g.setColor(c);
    temp2.x = xf.p.x + -k_axisScale * xf.q.s;
    temp2.y = xf.p.y + k_axisScale * xf.q.c;
    getWorldToScreenToOut(temp2, temp2);
    g.drawLine((int) temp.x, (int) temp.y, (int) temp2.x, (int) temp2.y);
  }
  public float computeSubmergedArea(final Vec2 normal, float offset, XForm xf, Vec2 c) {
    final Vec2 v0 = tlV0.get();
    final Vec2 v1 = tlV1.get();
    final Vec2 v2 = tlV2.get();
    final Vec2 temp = tlTemp.get();

    // Note that v0 is independent of any details of the specific edge
    // We are relying on v0 being consistent between multiple edges of the same body
    v0.set(normal).mul(offset);
    // b2Vec2 v0 = xf.position + (offset - b2Dot(normal, xf.position)) * normal;

    XForm.mulToOut(xf, m_v1, v1);
    XForm.mulToOut(xf, m_v2, v2);

    float d1 = Vec2.dot(normal, v1) - offset;
    float d2 = Vec2.dot(normal, v2) - offset;

    if (d1 > 0.0f) {
      if (d2 > 0.0f) {
        return 0.0f;
      } else {
        temp.set(v2).mulLocal(d1 / (d1 - d2));
        v1.mulLocal(-d2 / (d1 - d2)).addLocal(temp);
      }
    } else {
      if (d2 > 0.0f) {
        temp.set(v1).mulLocal(-d2 / (d1 - d2));
        v2.mulLocal(d1 / (d1 - d2)).addLocal(temp);
      } else {
        // Nothing
      }
    }

    final Vec2 e1 = tlE1.get();
    final Vec2 e2 = tlE2.get();

    // v0,v1,v2 represents a fully submerged triangle
    float k_inv3 = 1.0f / 3.0f;

    // Area weighted centroid
    c.x = k_inv3 * (v0.x + v1.x + v2.x);
    c.y = k_inv3 * (v0.y + v1.y + v2.y);

    e1.set(v1).subLocal(v0);
    e2.set(v2).subLocal(v0);

    return 0.5f * Vec2.cross(e1, e2);
  }
示例#10
0
  /**
   * Apply a force at a world point. If the force is not applied at the center of mass, it will
   * generate a torque and affect the angular velocity. This wakes up the body.
   *
   * @param force the world force vector, usually in Newtons (N).
   * @param point the world position of the point of application.
   */
  public final void applyForce(Vec2 force, Vec2 point) {
    if (m_type != BodyType.DYNAMIC) {
      return;
    }

    if (isAwake() == false) {
      setAwake(true);
    }

    // m_force.addLocal(force);
    // Vec2 temp = tltemp.get();
    // temp.set(point).subLocal(m_sweep.c);
    // m_torque += Vec2.cross(temp, force);

    m_force.x += force.x;
    m_force.y += force.y;

    m_torque += (point.x - m_sweep.c.x) * force.y - (point.y - m_sweep.c.y) * force.x;
  }
示例#11
0
  /**
   * Apply an impulse at a point. This immediately modifies the velocity. It also modifies the
   * angular velocity if the point of application is not at the center of mass. This wakes up the
   * body.
   *
   * @param impulse the world impulse vector, usually in N-seconds or kg-m/s.
   * @param point the world position of the point of application.
   */
  public final void applyLinearImpulse(Vec2 impulse, Vec2 point) {
    if (m_type != BodyType.DYNAMIC) {
      return;
    }

    if (isAwake() == false) {
      setAwake(true);
    }

    // Vec2 temp = tltemp.get();
    // temp.set(impulse).mulLocal(m_invMass);
    // m_linearVelocity.addLocal(temp);
    //
    // temp.set(point).subLocal(m_sweep.c);
    // m_angularVelocity += m_invI * Vec2.cross(temp, impulse);

    m_linearVelocity.x += impulse.x * m_invMass;
    m_linearVelocity.y += impulse.y * m_invMass;

    m_angularVelocity +=
        m_invI * ((point.x - m_sweep.c.x) * impulse.y - (point.y - m_sweep.c.y) * impulse.x);
  }
  public PhysicsObject(float x, float y, int w, int h, BodyType t) {
    pos.x = x;
    pos.y = y;
    height = h;
    width = w;

    shape = new PolygonShape();
    shape.setAsBox(
        (width / 2) / PhysicsInvaders.PTM_RATIO, (height / 2) / PhysicsInvaders.PTM_RATIO);

    FixtureDef fd = new FixtureDef();
    fd.shape = shape;
    fd.density = 100;
    fd.friction = 0.9f;
    fd.restitution = 0.1f;

    BodyDef bodyDef = new BodyDef();
    bodyDef.position.set(pos.x / PhysicsInvaders.PTM_RATIO, pos.y / PhysicsInvaders.PTM_RATIO);
    bodyDef.type = t;

    body = PhysicsInvaders.world.createBody(bodyDef);
    body.createFixture(fd);
  }
示例#13
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;
    }
  }
  public void computeMass(final MassData massData, float density) {
    // Polygon mass, centroid, and inertia.
    // Let rho be the polygon density in mass per unit area.
    // Then:
    // mass = rho * int(dA)
    // centroid.x = (1/mass) * rho * int(x * dA)
    // centroid.y = (1/mass) * rho * int(y * dA)
    // I = rho * int((x*x + y*y) * dA)
    //
    // We can compute these integrals by summing all the integrals
    // for each triangle of the polygon. To evaluate the integral
    // for a single triangle, we make a change of variables to
    // the (u,v) coordinates of the triangle:
    // x = x0 + e1x * u + e2x * v
    // y = y0 + e1y * u + e2y * v
    // where 0 <= u && 0 <= v && u + v <= 1.
    //
    // We integrate u from [0,1-v] and then v from [0,1].
    // We also need to use the Jacobian of the transformation:
    // D = cross(e1, e2)
    //
    // Simplification: triangle centroid = (1/3) * (p1 + p2 + p3)
    //
    // The rest of the derivation is handled by computer algebra.

    assert (m_count >= 3);

    final Vec2 center = pool1;
    center.setZero();
    float area = 0.0f;
    float I = 0.0f;

    // pRef is the reference point for forming triangles.
    // It's location doesn't change the result (except for rounding error).
    final Vec2 s = pool2;
    s.setZero();
    // This code would put the reference point inside the polygon.
    for (int i = 0; i < m_count; ++i) {
      s.addLocal(m_vertices[i]);
    }
    s.mulLocal(1.0f / m_count);

    final float k_inv3 = 1.0f / 3.0f;

    final Vec2 e1 = pool3;
    final Vec2 e2 = pool4;

    for (int i = 0; i < m_count; ++i) {
      // Triangle vertices.
      e1.set(m_vertices[i]).subLocal(s);
      e2.set(s).negateLocal().addLocal(i + 1 < m_count ? m_vertices[i + 1] : m_vertices[0]);

      final float D = Vec2.cross(e1, e2);

      final float triangleArea = 0.5f * D;
      area += triangleArea;

      // Area weighted centroid
      center.x += triangleArea * k_inv3 * (e1.x + e2.x);
      center.y += triangleArea * k_inv3 * (e1.y + e2.y);

      final float ex1 = e1.x, ey1 = e1.y;
      final float ex2 = e2.x, ey2 = e2.y;

      float intx2 = ex1 * ex1 + ex2 * ex1 + ex2 * ex2;
      float inty2 = ey1 * ey1 + ey2 * ey1 + ey2 * ey2;

      I += (0.25f * k_inv3 * D) * (intx2 + inty2);
    }

    // Total mass
    massData.mass = density * area;

    // Center of mass
    assert (area > Settings.EPSILON);
    center.mulLocal(1.0f / area);
    massData.center.set(center).addLocal(s);

    // Inertia tensor relative to the local origin (point s)
    massData.I = I * density;

    // Shift to center of mass then to original body origin.
    massData.I += massData.mass * (Vec2.dot(massData.center, massData.center));
  }
示例#15
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;
      }
    }
  }
示例#16
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;
  }
示例#17
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);
  }
示例#18
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);
  }