// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2016 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "ray_box_intersect.h"
#include <array>
#include <igl/matlab_format.h>
#include <igl/increment_ulp.h>

template <
  typename Derivedsource,
  typename Deriveddir,
  typename Scalar>
IGL_INLINE bool igl::ray_box_intersect(
  const Eigen::MatrixBase<Derivedsource> & origin,
  const Eigen::MatrixBase<Deriveddir> & inv_dir,
  const Eigen::MatrixBase<Deriveddir> & inv_dir_pad,
  const Eigen::AlignedBox<Scalar,3> & box,
  const Scalar & t0,
  const Scalar & t1,
  Scalar & tmin,
  Scalar & tmax)
{
  using namespace Eigen;
  typedef Matrix<Scalar,1,3>  RowVector3S;
  const std::array<bool, 3> sign = { inv_dir(0)<0, inv_dir(1)<0, inv_dir(2)<0};
  // http://people.csail.mit.edu/amy/papers/box-jgt.pdf
  // "An Efficient and Robust Ray–Box Intersection Algorithm"
  // corrected in "Robust BVH Ray Traversal" by Thiago Ize, section 3:
  Scalar tymin, tymax, tzmin, tzmax;
  std::array<RowVector3S, 2> bounds = {box.min().array(),box.max().array()};
  tmin = ( bounds[sign[0]](0)   - origin(0)) * inv_dir(0);
  tmax = ( bounds[1-sign[0]](0) - origin(0)) * inv_dir_pad(0);
  tymin = (bounds[sign[1]](1)   - origin(1)) * inv_dir(1);
  tymax = (bounds[1-sign[1]](1) - origin(1)) * inv_dir_pad(1);
  // NaN-safe min and max
  const auto berger_perrin_min = [&](
      const Scalar a,
      const Scalar b) -> Scalar
  {
    return (a < b) ? a : b;
  };
  const auto berger_perrin_max = [&](
      const Scalar a,
      const Scalar b) -> Scalar
  {
    return (a > b) ? a : b;
  };
  if ( (tmin > tymax) || (tymin > tmax) )
  {
    return false;
  }
  tmin = berger_perrin_max(tmin, tymin);
  tmax = berger_perrin_min(tmax, tymax);
  tzmin = (bounds[sign[2]](2) - origin(2))   * inv_dir(2);
  tzmax = (bounds[1-sign[2]](2) - origin(2)) * inv_dir_pad(2);
  if ((tmin > tzmax) || (tzmin > tmax))
  {
    return false;
  }
  tmin = berger_perrin_max(tmin, tzmin);
  tmax = berger_perrin_min(tmax, tzmax);
  return ((tmin < t1) && (tmax > t0));
}

template <
  typename Derivedsource,
  typename Deriveddir,
  typename Scalar>
IGL_INLINE bool igl::ray_box_intersect(
  const Eigen::MatrixBase<Derivedsource> & origin,
  const Eigen::MatrixBase<Deriveddir> & dir,
  const Eigen::AlignedBox<Scalar,3> & box,
  const Scalar & t0,
  const Scalar & t1,
  Scalar & tmin,
  Scalar & tmax)
{
    // precompute the inv_dir
    Eigen::Matrix<Scalar, 1, 3> inv_dir = dir.cwiseInverse();
    // see "Robust BVH Ray Traversal" by Thiago Ize, section 3:
    // for why we need this
    Eigen::Matrix<Scalar, 1, 3> inv_dir_pad = inv_dir;
    igl::increment_ulp(inv_dir_pad, 2);
    return igl::ray_box_intersect(origin, inv_dir, inv_dir_pad, box, t0, t1, tmin, tmax);
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
// generated by autoexplicit.sh
template bool igl::ray_box_intersect<Eigen::Matrix<float, 1, 3, 1, 1, 3>, Eigen::Matrix<float, 1, 3, 1, 1, 3>, float>(Eigen::MatrixBase<Eigen::Matrix<float, 1, 3, 1, 1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<float, 1, 3, 1, 1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<float, 1, 3, 1, 1, 3> > const&, Eigen::AlignedBox<float, 3> const&, float const&, float const&, float&, float&);
template bool igl::ray_box_intersect<Eigen::Matrix<double, 1, 3, 1, 1, 3>, Eigen::Matrix<double, 1, 3, 1, 1, 3>, double>(Eigen::MatrixBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> > const&, Eigen::AlignedBox<double, 3> const&, double const&, double const&, double&, double&);
template bool igl::ray_box_intersect<Eigen::Matrix<float, 3, 1, 0, 3, 1>, Eigen::Matrix<float, 3, 1, 0, 3, 1>, float>(Eigen::MatrixBase<Eigen::Matrix<float, 3, 1, 0, 3, 1>> const&, Eigen::MatrixBase<Eigen::Matrix<float, 3, 1, 0, 3, 1>> const&, Eigen::AlignedBox<float, 3> const&, float const&, float const&, float&, float&);
template bool igl::ray_box_intersect<Eigen::Matrix<float, 1, 3, 1, 1, 3>, Eigen::Matrix<float, 1, 3, 1, 1, 3>, float>(Eigen::MatrixBase<Eigen::Matrix<float, 1, 3, 1, 1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<float, 1, 3, 1, 1, 3> > const&, Eigen::AlignedBox<float, 3> const&, float const&, float const&, float&, float&);
template bool igl::ray_box_intersect<Eigen::Matrix<double, 1, 3, 1, 1, 3>, Eigen::Matrix<double, 1, 3, 1, 1, 3>, double>(Eigen::MatrixBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> > const&, Eigen::AlignedBox<double, 3> const&, double const&, double const&, double&, double&);
#endif