raycastWithPool method

bool raycastWithPool(
  1. RayCastOutput output,
  2. RayCastInput input,
  3. IWorldPool argPool
)

From Real-time Collision Detection, p179.

Implementation

bool raycastWithPool(final RayCastOutput output, final RayCastInput input,
    IWorldPool argPool) {
  double tmin = -double.maxFinite;
  double tmax = double.maxFinite;

  final Vector2 p = argPool.popVec2();
  final Vector2 d = argPool.popVec2();
  final Vector2 absD = argPool.popVec2();
  final Vector2 normal = argPool.popVec2();

  p.setFrom(input.p1);
  d
    ..setFrom(input.p2)
    ..sub(input.p1);
  absD
    ..setFrom(d)
    ..absolute();

  // x then y
  if (absD.x < Settings.EPSILON) {
    // Parallel.
    if (p.x < lowerBound.x || upperBound.x < p.x) {
      argPool.pushVec2(4);
      return false;
    }
  } else {
    final double inv_d = 1.0 / d.x;
    double t1 = (lowerBound.x - p.x) * inv_d;
    double t2 = (upperBound.x - p.x) * inv_d;

    // Sign of the normal vector.
    double s = -1.0;

    if (t1 > t2) {
      final double temp = t1;
      t1 = t2;
      t2 = temp;
      s = 1.0;
    }

    // Push the min up
    if (t1 > tmin) {
      normal.setZero();
      normal.x = s;
      tmin = t1;
    }

    // Pull the max down
    tmax = Math.min(tmax, t2);

    if (tmin > tmax) {
      argPool.pushVec2(4);
      return false;
    }
  }

  if (absD.y < Settings.EPSILON) {
    // Parallel.
    if (p.y < lowerBound.y || upperBound.y < p.y) {
      argPool.pushVec2(4);
      return false;
    }
  } else {
    double inv_d = 1.0 / d.y;
    double t1 = (lowerBound.y - p.y) * inv_d;
    double t2 = (upperBound.y - p.y) * inv_d;

    // Sign of the normal vector.
    double s = -1.0;

    if (t1 > t2) {
      final double temp = t1;
      t1 = t2;
      t2 = temp;
      s = 1.0;
    }

    // Push the min up
    if (t1 > tmin) {
      normal.setZero();
      normal.y = s;
      tmin = t1;
    }

    // Pull the max down
    tmax = Math.min(tmax, t2);

    if (tmin > tmax) {
      argPool.pushVec2(4);
      return false;
    }
  }

  // Does the ray start inside the box?
  // Does the ray intersect beyond the max fraction?
  if (tmin < 0.0 || input.maxFraction < tmin) {
    argPool.pushVec2(4);
    return false;
  }

  // Intersection.
  output.fraction = tmin;
  output.normal.x = normal.x;
  output.normal.y = normal.y;
  argPool.pushVec2(4);
  return true;
}