コード例 #1
0
ファイル: Island.java プロジェクト: tomerd/gwt-box2d
  public void solveTOI(TimeStep subStep) {
    ContactSolver contactSolver = new ContactSolver(subStep, m_contacts, m_contactCount);

    // No warm starting needed for TOI events.

    // Solve velocity constraints.
    for (int i = 0; i < subStep.maxIterations; ++i) {
      contactSolver.solveVelocityConstraints();
    }

    // Don't store the TOI contact forces for warm starting
    // because they can be quite large.

    // Integrate positions.
    for (int i = 0; i < m_bodyCount; ++i) {
      Body b = m_bodies[i];

      if (b.isStatic()) continue;
      // System.out.println("(Island::SolveTOI 1) :"+b.m_sweep);
      // Store positions for continuous collision.
      b.m_sweep.c0.set(b.m_sweep.c);
      b.m_sweep.a0 = b.m_sweep.a;

      // Integrate
      b.m_sweep.c.x += subStep.dt * b.m_linearVelocity.x;
      b.m_sweep.c.y += subStep.dt * b.m_linearVelocity.y;
      b.m_sweep.a += subStep.dt * b.m_angularVelocity;

      // System.out.println("(Island::SolveTOI 2) :"+b.m_sweep);
      // Compute new transform
      b.synchronizeTransform();

      //	System.out.println("(Island::SolveTOI 3) :"+b.m_sweep);
      // Note: shapes are synchronized later.
    }

    // Solve position constraints.
    final float k_toiBaumgarte = 0.75f;
    for (int i = 0; i < subStep.maxIterations; ++i) {
      boolean contactsOkay = contactSolver.solvePositionConstraints(k_toiBaumgarte);
      if (contactsOkay) {
        break;
      }
    }

    report(contactSolver.m_constraints);
  }
コード例 #2
0
ファイル: Island.java プロジェクト: tomerd/gwt-box2d
  public void solve(TimeStep step, Vec2 gravity, boolean correctPositions, boolean allowSleep) {
    // Integrate velocities and apply damping.
    for (int i = 0; i < m_bodyCount; ++i) {
      Body b = m_bodies[i];

      if (b.isStatic()) continue;

      // Integrate velocities.
      b.m_linearVelocity.x += step.dt * (gravity.x + b.m_invMass * b.m_force.x);
      b.m_linearVelocity.y += step.dt * (gravity.y + b.m_invMass * b.m_force.y);
      b.m_angularVelocity += step.dt * b.m_invI * b.m_torque;

      // Reset forces.
      b.m_force.set(0.0f, 0.0f);
      b.m_torque = 0.0f;

      // Apply damping.
      // ODE: dv/dt + c * v = 0
      // Solution: v(t) = v0 * exp(-c * t)
      // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v *
      // exp(-c * dt)
      // v2 = exp(-c * dt) * v1
      // Taylor expansion:
      // v2 = (1.0f - c * dt) * v1
      b.m_linearVelocity.mulLocal(MathUtils.clamp(1.0f - step.dt * b.m_linearDamping, 0.0f, 1.0f));
      b.m_angularVelocity *= MathUtils.clamp(1.0f - step.dt * b.m_angularDamping, 0.0f, 1.0f);

      // Check for large velocities.
      if (Vec2.dot(b.m_linearVelocity, b.m_linearVelocity) > Settings.maxLinearVelocitySquared) {
        b.m_linearVelocity.normalize();
        b.m_linearVelocity.mulLocal(Settings.maxLinearVelocity);
      }

      if (b.m_angularVelocity * b.m_angularVelocity > Settings.maxAngularVelocitySquared) {
        if (b.m_angularVelocity < 0.0f) {
          b.m_angularVelocity = -Settings.maxAngularVelocity;
        } else {
          b.m_angularVelocity = Settings.maxAngularVelocity;
        }
      }
    }

    ContactSolver contactSolver = new ContactSolver(step, m_contacts, m_contactCount);

    // Initialize velocity constraints.
    contactSolver.initVelocityConstraints(step);

    for (int i = 0; i < m_jointCount; ++i) {
      m_joints[i].initVelocityConstraints(step);
    }

    // Solve velocity constraints.
    for (int i = 0; i < step.maxIterations; ++i) {
      contactSolver.solveVelocityConstraints();

      for (int j = 0; j < m_jointCount; ++j) {
        m_joints[j].solveVelocityConstraints(step);
      }
    }

    // Post-solve (store impulses for warm starting).
    contactSolver.finalizeVelocityConstraints();

    // Integrate positions.
    for (int i = 0; i < m_bodyCount; ++i) {
      Body b = m_bodies[i];

      if (b.isStatic()) continue;

      // Store positions for continuous collision.
      b.m_sweep.c0.set(b.m_sweep.c);
      b.m_sweep.a0 = b.m_sweep.a;

      // Integrate
      b.m_sweep.c.x += step.dt * b.m_linearVelocity.x;
      b.m_sweep.c.y += step.dt * b.m_linearVelocity.y;
      b.m_sweep.a += step.dt * b.m_angularVelocity;

      // Compute new transform
      b.synchronizeTransform();

      // Note: shapes are synchronized later.
    }

    if (correctPositions) {
      // Initialize position constraints.
      // Contacts don't need initialization.
      for (int i = 0; i < m_jointCount; ++i) {
        m_joints[i].initPositionConstraints();
      }

      // Iterate over constraints.
      for (m_positionIterationCount = 0;
          m_positionIterationCount < step.maxIterations;
          ++m_positionIterationCount) {
        boolean contactsOkay = contactSolver.solvePositionConstraints(Settings.contactBaumgarte);

        boolean jointsOkay = true;
        for (int i = 0; i < m_jointCount; ++i) {
          boolean jointOkay = m_joints[i].solvePositionConstraints();
          jointsOkay = jointsOkay && jointOkay;
        }

        if (contactsOkay && jointsOkay) {
          break;
        }
      }
    }

    report(contactSolver.m_constraints);

    if (allowSleep) {
      float minSleepTime = Float.MAX_VALUE;

      final float linTolSqr = Settings.linearSleepTolerance * Settings.linearSleepTolerance;
      final float angTolSqr = Settings.angularSleepTolerance * Settings.angularSleepTolerance;

      for (int i = 0; i < m_bodyCount; ++i) {
        Body b = m_bodies[i];
        if (b.m_invMass == 0.0f) {
          continue;
        }

        if ((b.m_flags & Body.e_allowSleepFlag) == 0) {
          b.m_sleepTime = 0.0f;
          minSleepTime = 0.0f;
        }

        if ((b.m_flags & Body.e_allowSleepFlag) == 0
            || b.m_angularVelocity * b.m_angularVelocity > angTolSqr
            || Vec2.dot(b.m_linearVelocity, b.m_linearVelocity) > linTolSqr) {
          b.m_sleepTime = 0.0f;
          minSleepTime = 0.0f;
        } else {
          b.m_sleepTime += step.dt;
          minSleepTime = Math.min(minSleepTime, b.m_sleepTime);
        }
      }

      if (minSleepTime >= Settings.timeToSleep) {
        for (int i = 0; i < m_bodyCount; ++i) {
          Body b = m_bodies[i];
          b.m_flags |= Body.e_sleepFlag;
          b.m_linearVelocity = new Vec2(0.0f, 0.0f);
          b.m_angularVelocity = 0.0f;
        }
      }
    }
  }