content: /*
   * FXGL - JavaFX Game Library. The MIT License (MIT).
   * Copyright (c) AlmasB (almaslvl@gmail.com).
   * See LICENSE for details.
   */
  package com.almasb.fxgl.physics.box2d.collision;
  
  import com.almasb.fxgl.core.math.FXGLMath;
  import com.almasb.fxgl.physics.box2d.collision.Distance.SimplexCache;
  import com.almasb.fxgl.physics.box2d.collision.shapes.Shape;
  import com.almasb.fxgl.physics.box2d.common.JBoxSettings;
  import com.almasb.fxgl.physics.box2d.common.Sweep;
  import com.almasb.fxgl.physics.box2d.common.Transform;
  import com.almasb.fxgl.physics.box2d.pooling.IWorldPool;
  
  /**
   * Class used for computing the time of impact. This class should not be constructed usually, just
   * retrieve from the pool.
   *
   * @author daniel
   */
  public class TimeOfImpact {
      private static final int MAX_ITERATIONS = 1000;
  
      /**
       * Input parameters for TOI
       *
       * @author Daniel Murphy
       */
      public static class TOIInput {
          public final DistanceProxy proxyA = new DistanceProxy();
          public final DistanceProxy proxyB = new DistanceProxy();
          public final Sweep sweepA = new Sweep();
          public final Sweep sweepB = new Sweep();
          /**
           * defines sweep interval [0, tMax]
           */
          public float tMax;
  
          public void setProxyA(Shape shape, int index) {
              proxyA.set(shape, index);
          }
  
          public void setProxyB(Shape shape, int index) {
              proxyB.set(shape, index);
          }
      }
  
      public enum TOIOutputState {
          UNKNOWN, FAILED, OVERLAPPED, TOUCHING, SEPARATED
      }
  
      /**
       * Output parameters for TimeOfImpact
       *
       * @author daniel
       */
      public static class TOIOutput {
          public TOIOutputState state;
          public float t;
      }
  
      // djm pooling
      private final SimplexCache cache = new SimplexCache();
      private final DistanceInput distanceInput = new DistanceInput();
      private final Transform xfA = new Transform();
      private final Transform xfB = new Transform();
      private final DistanceOutput distanceOutput = new DistanceOutput();
      private final SeparationFunction fcn = new SeparationFunction();
      private final int[] indexes = new int[2];
      private final Sweep sweepA = new Sweep();
      private final Sweep sweepB = new Sweep();
  
      private final IWorldPool pool;
  
      public TimeOfImpact(IWorldPool pool) {
          this.pool = pool;
      }
  
      /**
       * Compute the upper bound on time before two shapes penetrate. Time is represented as a fraction
       * between [0,tMax]. This uses a swept separating axis and may miss some intermediate,
       * non-tunneling collision. If you change the time interval, you should call this function again.
       * Note: use Distance to compute the contact point and normal at the time of impact.
       */
      public final void timeOfImpact(TOIOutput output, TOIInput input) {
          // CCD via the local separating axis method. This seeks progression
          // by computing the largest time at which separation is maintained.
  
          output.state = TOIOutputState.UNKNOWN;
          output.t = input.tMax;
  
          final DistanceProxy proxyA = input.proxyA;
          final DistanceProxy proxyB = input.proxyB;
  
          sweepA.set(input.sweepA);
          sweepB.set(input.sweepB);
  
          // Large rotations can make the root finder fail, so we normalize the sweep angles.
          sweepA.normalize();
          sweepB.normalize();
  
          float tMax = input.tMax;
  
          float totalRadius = proxyA.getRadius() + proxyB.getRadius();
          // djm: whats with all these constants?
          float target = Math.max(JBoxSettings.linearSlop, totalRadius - 3.0f * JBoxSettings.linearSlop);
          float tolerance = 0.25f * JBoxSettings.linearSlop;
  
          assert target > tolerance;
  
          float t1 = 0f;
          int iter = 0;
  
          cache.count = 0;
          distanceInput.proxyA = input.proxyA;
          distanceInput.proxyB = input.proxyB;
          distanceInput.useRadii = false;
  
          // The outer loop progressively attempts to compute new separating axes.
          // This loop terminates when an axis is repeated (no progress is made).
          for (; ; ) {
              sweepA.getTransform(xfA, t1);
              sweepB.getTransform(xfB, t1);
  
              // Get the distance between shapes.
              // We can also use the results to get a separating axis
              distanceInput.transformA = xfA;
              distanceInput.transformB = xfB;
              pool.getDistance().distance(distanceOutput, cache, distanceInput);
  
              // If the shapes are overlapped, we give up on continuous collision.
              if (distanceOutput.distance <= 0f) {
                  // Failure!
                  output.state = TOIOutputState.OVERLAPPED;
                  output.t = 0f;
                  break;
              }
  
              if (distanceOutput.distance < target + tolerance) {
                  // Victory!
                  output.state = TOIOutputState.TOUCHING;
                  output.t = t1;
                  break;
              }
  
              // Initialize the separating axis.
              fcn.initialize(cache, proxyA, sweepA, proxyB, sweepB, t1);
  
              // Compute the TOI on the separating axis.
              // We do this by successively resolving the deepest point.
              // This loop is bounded by the number of vertices.
              boolean done = false;
              float t2 = tMax;
              int pushBackIter = 0;
              for (; ; ) {
  
                  // Find the deepest point at t2. Store the witness point indices.
                  float s2 = fcn.findMinSeparation(indexes, t2);
  
                  // Is the final configuration separated?
                  if (s2 > target + tolerance) {
                      // Victory!
                      output.state = TOIOutputState.SEPARATED;
                      output.t = tMax;
                      done = true;
                      break;
                  }
  
                  // Has the separation reached tolerance?
                  if (s2 > target - tolerance) {
                      // Advance the sweeps
                      t1 = t2;
                      break;
                  }
  
                  // Compute the initial separation of the witness points.
                  float s1 = fcn.evaluate(indexes[0], indexes[1], t1);
  
                  // Check for initial overlap.
                  // This might happen if the root finder runs out of iterations.
                  if (s1 < target - tolerance) {
                      output.state = TOIOutputState.FAILED;
                      output.t = t1;
                      done = true;
                      break;
                  }
  
                  // Check for touching
                  if (s1 <= target + tolerance) {
                      // Victory! t1 should hold the TOI (could be 0.0).
                      output.state = TOIOutputState.TOUCHING;
                      output.t = t1;
                      done = true;
                      break;
                  }
  
                  // Compute 1D root of: f(x) - target = 0
                  int rootIterCount = 0;
                  float a1 = t1, a2 = t2;
                  for (; ; ) {
                      // Use a mix of the secant rule and bisection.
                      float t;
                      if ((rootIterCount & 1) == 1) {
                          // Secant rule to improve convergence.
                          t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
                      } else {
                          // Bisection to guarantee progress.
                          t = 0.5f * (a1 + a2);
                      }
  
                      float s = fcn.evaluate(indexes[0], indexes[1], t);
  
                      if (FXGLMath.abs(s - target) < tolerance) {
                          // t2 holds a tentative value for t1
                          t2 = t;
                          break;
                      }
  
                      // Ensure we continue to bracket the root.
                      if (s > target) {
                          a1 = t;
                          s1 = s;
                      } else {
                          a2 = t;
                          s2 = s;
                      }
  
                      ++rootIterCount;
  
                      // djm: whats with this? put in settings?
                      if (rootIterCount == 50) {
                          break;
                      }
                  }
  
                  ++pushBackIter;
  
                  if (pushBackIter == JBoxSettings.maxPolygonVertices) {
                      break;
                  }
              }
  
              ++iter;
  
              if (done) {
                  break;
              }
  
              if (iter == MAX_ITERATIONS) {
                  // Root finder got stuck. Semi-victory.
                  output.state = TOIOutputState.FAILED;
                  output.t = t1;
                  break;
              }
          }
      }
  }