/*    */ public class CompoundCollisionAlgorithmExt$SwappedCreateFunc
    extends CollisionAlgorithmCreateFunc
/*    */ {
  /* 58 */ private final ObjectPool pool = ObjectPool.get(CompoundCollisionAlgorithmExt.class);
  /*    */
  /*    */ public CollisionAlgorithm createCollisionAlgorithm(
      CollisionAlgorithmConstructionInfo paramCollisionAlgorithmConstructionInfo,
      CollisionObject paramCollisionObject1,
      CollisionObject paramCollisionObject2)
        /*    */ {
    /*    */ CompoundCollisionAlgorithmExt localCompoundCollisionAlgorithmExt;
    /* 67 */ CompoundCollisionAlgorithmExt.access$002(
        localCompoundCollisionAlgorithmExt = new CompoundCollisionAlgorithmExt(),
        /* 67 */ paramCollisionObject2);
    /* 68 */ CompoundCollisionAlgorithmExt.access$102(
        localCompoundCollisionAlgorithmExt, paramCollisionObject1);
    /*    */
    /* 70 */ localCompoundCollisionAlgorithmExt.init(paramCollisionAlgorithmConstructionInfo);
    /* 71 */ localCompoundCollisionAlgorithmExt.swapped = true;
    /* 72 */ return localCompoundCollisionAlgorithmExt;
    /*    */ }
  /*    */
  /*    */ public void releaseCollisionAlgorithm(CollisionAlgorithm paramCollisionAlgorithm)
        /*    */ {
    /* 79 */ CompoundCollisionAlgorithmExt.access$200(
            paramCollisionAlgorithm = (CompoundCollisionAlgorithmExt) paramCollisionAlgorithm)
        .pool
        /* 79 */ .release(paramCollisionAlgorithm);
    /*    */ }
  /*    */ }
예제 #2
0
  public boolean calcTimeOfImpact(
      Transform fromA, Transform toA, Transform fromB, Transform toB, CastResult result) {
    simplexSolver.reset();

    // compute linear velocity for this interval, to interpolate
    // assume no rotation/angular velocity, assert here?
    Vector3d linVelA = new Vector3d();
    Vector3d linVelB = new Vector3d();

    linVelA.sub(toA.origin, fromA.origin);
    linVelB.sub(toB.origin, fromB.origin);

    double radius = 0.001f;
    double lambda = 0f;
    Vector3d v = new Vector3d();
    v.set(1f, 0f, 0f);

    int maxIter = MAX_ITERATIONS;

    Vector3d n = new Vector3d();
    n.set(0f, 0f, 0f);
    boolean hasResult = false;
    Vector3d c = new Vector3d();
    Vector3d r = new Vector3d();
    r.sub(linVelA, linVelB);

    double lastLambda = lambda;
    // btScalar epsilon = btScalar(0.001);

    int numIter = 0;
    // first solution, using GJK

    Transform identityTrans = new Transform();
    identityTrans.setIdentity();

    // result.drawCoordSystem(sphereTr);

    PointCollector pointCollector = new PointCollector();

    gjk.init(convexA, convexB, simplexSolver, null); // penetrationDepthSolver);	
    ClosestPointInput input = pointInputsPool.get();
    input.init();
    try {
      // we don't use margins during CCD
      //	gjk.setIgnoreMargin(true);

      input.transformA.set(fromA);
      input.transformB.set(fromB);
      gjk.getClosestPoints(input, pointCollector, null);

      hasResult = pointCollector.hasResult;
      c.set(pointCollector.pointInWorld);

      if (hasResult) {
        double dist;
        dist = pointCollector.distance;
        n.set(pointCollector.normalOnBInWorld);

        // not close enough
        while (dist > radius) {
          numIter++;
          if (numIter > maxIter) {
            return false; // todo: report a failure
          }
          double dLambda = 0f;

          double projectedLinearVelocity = r.dot(n);

          dLambda = dist / (projectedLinearVelocity);

          lambda = lambda - dLambda;

          if (lambda > 1f) {
            return false;
          }
          if (lambda < 0f) {
            return false; // todo: next check with relative epsilon
          }

          if (lambda <= lastLambda) {
            return false;
            // n.setValue(0,0,0);
            // break;
          }
          lastLambda = lambda;

          // interpolate to next lambda
          result.debugDraw(lambda);
          VectorUtil.setInterpolate3(input.transformA.origin, fromA.origin, toA.origin, lambda);
          VectorUtil.setInterpolate3(input.transformB.origin, fromB.origin, toB.origin, lambda);

          gjk.getClosestPoints(input, pointCollector, null);
          if (pointCollector.hasResult) {
            if (pointCollector.distance < 0f) {
              result.fraction = lastLambda;
              n.set(pointCollector.normalOnBInWorld);
              result.normal.set(n);
              result.hitPoint.set(pointCollector.pointInWorld);
              return true;
            }
            c.set(pointCollector.pointInWorld);
            n.set(pointCollector.normalOnBInWorld);
            dist = pointCollector.distance;
          } else {
            // ??
            return false;
          }
        }

        // is n normalized?
        // don't report time of impact for motion away from the contact normal (or causes minor
        // penetration)
        if (n.dot(r) >= -result.allowedPenetration) {
          return false;
        }
        result.fraction = lambda;
        result.normal.set(n);
        result.hitPoint.set(c);
        return true;
      }

      return false;
    } finally {
      pointInputsPool.release(input);
    }
  }