Beispiel #1
0
  /** Call this to draw shapes and other debug draw data. */
  public void drawDebugData() {
    if (m_debugDraw == null) {
      return;
    }

    int flags = m_debugDraw.getFlags();

    if ((flags & DebugDraw.e_shapeBit) == DebugDraw.e_shapeBit) {
      for (Body b = m_bodyList; b != null; b = b.getNext()) {
        xf.set(b.getTransform());
        for (Fixture f = b.getFixtureList(); f != null; f = f.getNext()) {
          if (b.isActive() == false) {
            color.set(0.5f, 0.5f, 0.3f);
            drawShape(f, xf, color);
          } else if (b.getType() == BodyType.STATIC) {
            color.set(0.5f, 0.9f, 0.3f);
            drawShape(f, xf, color);
          } else if (b.getType() == BodyType.KINEMATIC) {
            color.set(0.5f, 0.5f, 0.9f);
            drawShape(f, xf, color);
          } else if (b.isAwake() == false) {
            color.set(0.5f, 0.5f, 0.5f);
            drawShape(f, xf, color);
          } else {
            color.set(0.9f, 0.7f, 0.7f);
            drawShape(f, xf, color);
          }
        }
      }
    }

    if ((flags & DebugDraw.e_jointBit) == DebugDraw.e_jointBit) {
      for (Joint j = m_jointList; j != null; j = j.getNext()) {
        drawJoint(j);
      }
    }

    if ((flags & DebugDraw.e_pairBit) == DebugDraw.e_pairBit) {
      color.set(0.3f, 0.9f, 0.9f);
      for (Contact c = m_contactManager.m_contactList; c != null; c = c.getNext()) {
        // Fixture fixtureA = c.getFixtureA();
        // Fixture fixtureB = c.getFixtureB();
        //
        // fixtureA.getAABB(childIndex).getCenterToOut(cA);
        // fixtureB.getAABB().getCenterToOut(cB);
        //
        // m_debugDraw.drawSegment(cA, cB, color);
      }
    }

    if ((flags & DebugDraw.e_aabbBit) == DebugDraw.e_aabbBit) {
      color.set(0.9f, 0.3f, 0.9f);

      for (Body b = m_bodyList; b != null; b = b.getNext()) {
        if (b.isActive() == false) {
          continue;
        }

        for (Fixture f = b.getFixtureList(); f != null; f = f.getNext()) {

          for (int i = 0; i < f.m_proxyCount; ++i) {
            FixtureProxy proxy = f.m_proxies[i];
            AABB aabb = m_contactManager.m_broadPhase.getFatAABB(proxy.proxyId);
            Vec2[] vs = avs.get(4);
            vs[0].set(aabb.lowerBound.x, aabb.lowerBound.y);
            vs[1].set(aabb.upperBound.x, aabb.lowerBound.y);
            vs[2].set(aabb.upperBound.x, aabb.upperBound.y);
            vs[3].set(aabb.lowerBound.x, aabb.upperBound.y);

            m_debugDraw.drawPolygon(vs, 4, color);
          }
        }
      }
    }

    if ((flags & DebugDraw.e_centerOfMassBit) == DebugDraw.e_centerOfMassBit) {
      for (Body b = m_bodyList; b != null; b = b.getNext()) {
        xf.set(b.getTransform());
        xf.p.set(b.getWorldCenter());
        m_debugDraw.drawTransform(xf);
      }
    }

    if ((flags & DebugDraw.e_dynamicTreeBit) == DebugDraw.e_dynamicTreeBit) {
      m_contactManager.m_broadPhase.drawTree(m_debugDraw);
    }
  }
Beispiel #2
0
  private void solve(TimeStep step) {
    m_profile.solveInit = 0;
    m_profile.solveVelocity = 0;
    m_profile.solvePosition = 0;

    // Size the island for the worst case.
    island.init(
        m_bodyCount,
        m_contactManager.m_contactCount,
        m_jointCount,
        m_contactManager.m_contactListener);

    // Clear all the island flags.
    for (Body b = m_bodyList; b != null; b = b.m_next) {
      b.m_flags &= ~Body.e_islandFlag;
    }
    for (Contact c = m_contactManager.m_contactList; c != null; c = c.m_next) {
      c.m_flags &= ~Contact.ISLAND_FLAG;
    }
    for (Joint j = m_jointList; j != null; j = j.m_next) {
      j.m_islandFlag = false;
    }

    // Build and simulate all awake islands.
    int stackSize = m_bodyCount;
    if (stack.length < stackSize) {
      stack = new Body[stackSize];
    }
    for (Body seed = m_bodyList; seed != null; seed = seed.m_next) {
      if ((seed.m_flags & Body.e_islandFlag) == Body.e_islandFlag) {
        continue;
      }

      if (seed.isAwake() == false || seed.isActive() == false) {
        continue;
      }

      // The seed can be dynamic or kinematic.
      if (seed.getType() == BodyType.STATIC) {
        continue;
      }

      // Reset island and stack.
      island.clear();
      int stackCount = 0;
      stack[stackCount++] = seed;
      seed.m_flags |= Body.e_islandFlag;

      // Perform a depth first search (DFS) on the constraint graph.
      while (stackCount > 0) {
        // Grab the next body off the stack and add it to the island.
        Body b = stack[--stackCount];
        assert (b.isActive() == true);
        island.add(b);

        // Make sure the body is awake.
        b.setAwake(true);

        // To keep islands as small as possible, we don't
        // propagate islands across static bodies.
        if (b.getType() == BodyType.STATIC) {
          continue;
        }

        // Search all contacts connected to this body.
        for (ContactEdge ce = b.m_contactList; ce != null; ce = ce.next) {
          Contact contact = ce.contact;

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

          // Is this contact solid and touching?
          if (contact.isEnabled() == false || contact.isTouching() == false) {
            continue;
          }

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

          island.add(contact);
          contact.m_flags |= Contact.ISLAND_FLAG;

          Body other = ce.other;

          // Was the other body already added to this island?
          if ((other.m_flags & Body.e_islandFlag) == Body.e_islandFlag) {
            continue;
          }

          assert (stackCount < stackSize);
          stack[stackCount++] = other;
          other.m_flags |= Body.e_islandFlag;
        }

        // Search all joints connect to this body.
        for (JointEdge je = b.m_jointList; je != null; je = je.next) {
          if (je.joint.m_islandFlag == true) {
            continue;
          }

          Body other = je.other;

          // Don't simulate joints connected to inactive bodies.
          if (other.isActive() == false) {
            continue;
          }

          island.add(je.joint);
          je.joint.m_islandFlag = true;

          if ((other.m_flags & Body.e_islandFlag) == Body.e_islandFlag) {
            continue;
          }

          assert (stackCount < stackSize);
          stack[stackCount++] = other;
          other.m_flags |= Body.e_islandFlag;
        }
      }
      island.solve(islandProfile, step, m_gravity, m_allowSleep);
      m_profile.solveInit += islandProfile.solveInit;
      m_profile.solveVelocity += islandProfile.solveVelocity;
      m_profile.solvePosition += islandProfile.solvePosition;

      // Post solve cleanup.
      for (int i = 0; i < island.m_bodyCount; ++i) {
        // Allow static bodies to participate in other islands.
        Body b = island.m_bodies[i];
        if (b.getType() == BodyType.STATIC) {
          b.m_flags &= ~Body.e_islandFlag;
        }
      }
    }

    broadphaseTimer.reset();
    // Synchronize fixtures, check for out of range bodies.
    for (Body b = m_bodyList; b != null; b = b.getNext()) {
      // If a body was not in an island then it did not move.
      if ((b.m_flags & Body.e_islandFlag) == 0) {
        continue;
      }

      if (b.getType() == BodyType.STATIC) {
        continue;
      }

      // Update fixtures (for broad-phase).
      b.synchronizeFixtures();
    }

    // Look for new contacts.
    m_contactManager.findNewContacts();
    m_profile.broadphase = broadphaseTimer.getMilliseconds();
  }
Beispiel #3
0
 /**
  * Call this after you are done with time steps to clear the forces. You normally call this after
  * each call to Step, unless you are performing sub-steps. By default, forces will be
  * automatically cleared, so you don't need to call this function.
  *
  * @see setAutoClearForces
  */
 public void clearForces() {
   for (Body body = m_bodyList; body != null; body = body.getNext()) {
     body.m_force.setZero();
     body.m_torque = 0.0f;
   }
 }