Ray.hxx

Go to the documentation of this file.

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *\
 * Copyright (c) 2026, Davide Stocco and Enrico Bertolazzi.                  *
 *                                                                           *
 * The AABBtree project is distributed under the BSD 2-Clause License.       *
 *                                                                           *
 * Davide Stocco Enrico Bertolazzi                                           *
 * University of Trento University of Trento                                 *
 * davide.stocco@unitn.it                         enrico.bertolazzi@unitn.it *
\* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
 
// DISCLAIMER: The code in this file is a modified version of the Eigen library.
 
#pragma once
 
#ifndef AABBTREE_RAY_HXX
#define AABBTREE_RAY_HXX
 
#include "AABBtree/Box.hxx"
 
namespace AABBtree {

template <typename Real, Integer N> class Ray {
  static_assert(std::is_floating_point<Real>::value,
                "Ray Real type must be a floating-point type.");
  static_assert(N > 0, "Ray dimension must be positive.");
 
  using Point = AABBtree::Point<Real, N>;
  using Vector = AABBtree::Vector<Real, N>;
 
  Point m_origin;     
  Vector m_direction; 
 
public:
  ~Ray() = default;

  Ray() = default;

  Ray(Ray const &r) : m_origin(r.m_origin), m_direction(r.m_direction) {}

  Ray(Point const &o, Vector const &d) : m_origin(o), m_direction(d) {}

  template <typename = std::enable_if<N == 1>>
  Ray(Real const o, Real const d) : m_origin(o), m_direction(d) {}

  template <typename = std::enable_if<N == 2>>
  Ray(Real const o_x, Real const o_y, Real const d_x, Real const d_y)
      : m_origin(o_x, o_y), m_direction(d_x, d_y) {}

  template <typename = std::enable_if<N == 3>>
  Ray(Real const o_x, Real const o_y, Real const o_z, Real const d_x,
      Real const d_y, Real const d_z)
      : m_origin(o_x, o_y, o_z), m_direction(d_x, d_y, d_z) {}

  template <typename OtherReal>
  explicit Ray(Ray<OtherReal, N> const &r)
      : m_origin(r.m_origin.template cast<Real>()),
        m_direction(r.m_direction.template cast<Real>()) {}

  template <typename NewReal> Ray<NewReal, N> cast() const {
    if constexpr (std::is_same<Real, NewReal>::value) {
      return *this;
    }
    return Ray<NewReal, N>(m_origin.template cast<NewReal>(),
                           m_direction.template cast<NewReal>());
  }

  Point &origin() { return m_origin; }

  Point const &origin() const { return m_origin; }

  Vector &direction() { return m_direction; }

  Vector const &direction() const { return m_direction; }

  Ray &normalize() {
    m_direction.normalize();
    return *this;
  }

  Ray normalized() const { return Ray(m_origin, m_direction.normalized()); }

  bool
  is_approx(Ray const &r,
            Real const tol = Eigen::NumTraits<Real>::dummy_precision()) const {
    return m_origin.isApprox(r.m_origin, tol) &&
           m_direction.isApprox(r.m_direction, tol);
  }

  Ray &translate(Vector const &t) {
    m_origin += t;
    return *this;
  }

  Ray translated(Vector const &t) const {
    return Ray(m_origin + t, m_direction);
  }

  template <typename Transform> Ray transformed(Transform const &t) const {
    return Ray(m_origin.transform(t), m_direction.rotate(t));
  }

  template <typename Transform> Ray &transform(Transform const &t) {
    m_origin.transform(t);
    m_direction.rotate(t);
    return *this;
  }

  bool contains(Point const &p,
                Real tol = Eigen::NumTraits<Real>::dummy_precision()) const {
    Vector v((p - m_origin).normalized());
    return std::abs(v.cross(m_direction).norm()) < tol &&
           v.dot(m_direction) >= -tol;
  }

  bool intersects(Box<Real, N> const &b,
                  Real tol = Eigen::NumTraits<Real>::dummy_precision()) const {
    if (b.contains(m_origin)) {
      return true;
    }
    Point const &b_min{b.min()};
    Point const &b_max{b.max()};
    Vector t_min, t_max;
    t_min.setConstant(-std::numeric_limits<Real>::infinity());
    t_max.setConstant(std::numeric_limits<Real>::infinity());
    for (Integer i{0}; i < N; ++i) {
      if (std::abs(m_direction[i]) > tol) {
        t_min[i] = (b_min[i] - m_origin[i]) / m_direction[i];
        t_max[i] = (b_max[i] - m_origin[i]) / m_direction[i];
        if (t_min[i] > t_max[i]) {
          std::swap(t_min[i], t_max[i]);
        }
      } else if (m_origin[i] < b_min[i] || m_origin[i] > b_max[i]) {
        return false;
      }
    }
    Real t_entry{t_min.maxCoeff()};
    Real t_exit{t_max.minCoeff()};
    return t_entry <= t_exit && t_exit >= -tol;
  }

  bool intersect(Box<Real, N> const &b, Point &c, Point &f,
                 Real tol = Eigen::NumTraits<Real>::dummy_precision()) const {
    if (b.contains(m_origin)) {
      return true;
    }
    Point const &b_min{b.min()};
    Point const &b_max{b.max()};
    Vector t_min, t_max;
    t_min.setConstant(-std::numeric_limits<Real>::infinity());
    t_max.setConstant(std::numeric_limits<Real>::infinity());
    for (Integer i{0}; i < N; ++i) {
      if (std::abs(m_direction[i]) > tol) {
        t_min[i] = (b_min[i] - m_origin[i]) / m_direction[i];
        t_max[i] = (b_max[i] - m_origin[i]) / m_direction[i];
        if (t_min[i] > t_max[i]) {
          std::swap(t_min[i], t_max[i]);
        }
      } else if (m_origin[i] < b_min[i] || m_origin[i] > b_max[i]) {
        return false;
      }
    }
    Real t_entry{t_min.maxCoeff()};
    Real t_exit{t_max.minCoeff()};
    if (t_entry > t_exit && t_exit < -tol) {
      return false;
    }
    c = m_origin + t_entry * m_direction;
    f = m_origin + t_exit * m_direction;
    return true;
  }

  Real
  squared_distance(Point const &p,
                   Real tol = Eigen::NumTraits<Real>::dummy_precision()) const {
    Real t{(p - m_origin).dot(m_direction) / m_direction.squaredNorm()};
    return (m_origin + std::max(static_cast<Real>(-tol), t) * m_direction - p)
        .squaredNorm();
  }

  Real
  squared_distance(Point const &p, Point &c,
                   Real tol = Eigen::NumTraits<Real>::dummy_precision()) const {
    Real t{(p - m_origin).transpose().dot(m_direction) /
           m_direction.squaredNorm()};
    c = m_origin + std::max(static_cast<Real>(-tol), t) * m_direction;
    return (c - p).squaredNorm();
  }

  Real distance(Point const &p,
                Real tol = Eigen::NumTraits<Real>::dummy_precision()) const {
    return std::sqrt(this->squared_distance(p, tol));
  }

  Real distance(Point const &p, Point &c,
                Real tol = Eigen::NumTraits<Real>::dummy_precision()) const {
    return std::sqrt(this->squared_distance(p, c, tol));
  }

  Real squared_interior_distance(
      Box<Real, N> const &b,
      Real tol = Eigen::NumTraits<Real>::dummy_precision()) const {
    Point p1, p2;
    return this->squared_interior_distance(b, p1, p2, tol);
  }

  Real squared_interior_distance(
      Box<Real, N> const &b, Point &p1, Point &p2,
      Real tol = Eigen::NumTraits<Real>::dummy_precision()) const {
    if (b.contains(m_origin)) {
      return 0.0;
    }
    Point const &b_min{b.min()};
    Point const &b_max{b.max()};
 
    // Compute intersection parameters
    Vector t_min, t_max;
    t_min.setConstant(-std::numeric_limits<Real>::infinity());
    t_max.setConstant(std::numeric_limits<Real>::infinity());
    for (Integer i{0}; i < N; ++i) {
      if (std::abs(m_direction[i]) > tol) {
        t_min[i] = (b_min[i] - m_origin[i]) / m_direction[i];
        t_max[i] = (b_max[i] - m_origin[i]) / m_direction[i];
        if (t_min[i] > t_max[i]) {
          std::swap(t_min[i], t_max[i]);
        }
      } else if (m_origin[i] < b_min[i] || m_origin[i] > b_max[i]) {
        // Ray is parallel and outside the box and non-intersecting
        b.interior_distance(m_origin, p2);
        Vector sides{b_max - b_min};
        this->distance(p2, p1);
        for (Integer j{0}; j < N; ++j) {
          if (j == i) {
            continue;
          }
          if (m_direction[j] > 0.0) {
            p1[j] += 0.5 * sides[j];
            p2[j] += 0.5 * sides[j];
          } else {
            p1[j] -= 0.5 * sides[j];
            p2[j] -= 0.5 * sides[j];
          }
        }
        return (p2 - m_origin).norm();
      }
    }
 
    // Compute global intersection range
    Real t_entry{t_min.maxCoeff()}, t_exit{t_max.minCoeff()};
    if (t_entry <= t_exit && t_exit >= 0.0) {
      return 0.0;
    }
 
    // Compute closest point if no intersection
    for (Integer i{0}; i < N; ++i) {
      if (m_origin[i] < b_min[i]) {
        p2[i] = b_min[i];
      } else if (m_origin[i] > b_max[i]) {
        p2[i] = b_max[i];
      } else {
        p2[i] = m_origin[i];
      }
    }
 
    // Project closest point onto the ray
    Vector v(p2 - m_origin);
    Real t_proj{v.dot(m_direction) / m_direction.squaredNorm()};
    if (t_proj < 0.0) {
      p1 = m_origin;
      return v.norm();
    }
 
    p1 = m_origin + t_proj * m_direction;
    ;
    return (p2 - p1).squaredNorm();
  }

  Real interior_distance(
      Box<Real, N> const &b,
      Real tol = Eigen::NumTraits<Real>::dummy_precision()) const {
    return std::sqrt(this->squared_interior_distance(b, tol));
  }

  Real interior_distance(
      Box<Real, N> const &b, Point &p1, Point &p2,
      Real tol = Eigen::NumTraits<Real>::dummy_precision()) const {
    return std::sqrt(
        static_cast<Real>(this->squared_interior_distance(b, p1, p2, tol)));
  }

  Real squared_exterior_distance(
      Box<Real, N> const &b,
      Real tol = Eigen::NumTraits<Real>::dummy_precision()) const {
    Point p1, p2;
    return this->squared_exterior_distance(b, p1, p2, tol);
  }

  Real squared_exterior_distance(
      Box<Real, N> const &b, Point &p1, Point &p2,
      Real tol = Eigen::NumTraits<Real>::dummy_precision()) const {
    if (b.contains(m_origin)) {
      return 0.0;
    }
    Point const &b_min{b.min()};
    Point const &b_max{b.max()};
 
    // Compute intersection parameters
    Vector t_min, t_max;
    t_min.setConstant(-std::numeric_limits<Real>::infinity());
    t_max.setConstant(std::numeric_limits<Real>::infinity());
    for (Integer i{0}; i < N; ++i) {
      if (std::abs(m_direction[i]) > tol) {
        t_min[i] = (b_min[i] - m_origin[i]) / m_direction[i];
        t_max[i] = (b_max[i] - m_origin[i]) / m_direction[i];
        if (t_min[i] > t_max[i]) {
          std::swap(t_min[i], t_max[i]);
        }
      } else if (m_origin[i] < b_min[i] || m_origin[i] > b_max[i]) {
        // Ray is parallel and outside the box and non-intersecting
        b.exterior_distance(m_origin, p2);
        Vector sides{b_max - b_min};
        this->distance(p2, p1);
        for (Integer j{0}; j < N; ++j) {
          if (j == i) {
            continue;
          }
          if (m_direction[j] > 0.0) {
            p1[j] -= 0.5 * sides[j];
            p2[j] -= 0.5 * sides[j];
          } else {
            p1[j] += 0.5 * sides[j];
            p2[j] += 0.5 * sides[j];
          }
        }
        return (p2 - m_origin).norm();
      }
    }
 
    // Compute global intersection range
    Real t_entry{t_min.maxCoeff()}, t_exit{t_max.minCoeff()};
    if (t_entry <= t_exit && t_exit >= 0) {
      return 0.0;
    }
 
    // Compute closest point if no intersection
    for (Integer i{0}; i < N; ++i) {
      if (m_origin[i] < b_min[i]) {
        p2[i] = b_max[i];
      } else if (m_origin[i] > b_max[i]) {
        p2[i] = b_min[i];
      } else {
        p2[i] = m_origin[i];
      }
    }
 
    // Project closest point onto the ray
    Vector v(p2 - m_origin);
    Real t_proj{v.dot(m_direction) / m_direction.squaredNorm()};
    if (t_proj < 0.0) {
      p1 = m_origin;
      return v.norm();
    }
 
    p1 = m_origin + t_proj * m_direction;
    ;
    return (p2 - p1).squaredNorm();
  }

  Real exterior_distance(
      Box<Real, N> const &b,
      Real tol = Eigen::NumTraits<Real>::dummy_precision()) const {
    return std::sqrt(this->squared_exterior_distance(b, tol));
  }

  Real exterior_distance(
      Box<Real, N> const &b, Point &p1, Point &p2,
      Real tol = Eigen::NumTraits<Real>::dummy_precision()) const {
    return std::sqrt(this->squared_exterior_distance(b, p1, p2, tol));
  }

  void print(std::ostream &os) const {
    os << "RAY INFO" << std::endl
       << "\to = " << m_origin.transpose() << std::endl
       << "\td = " << m_direction.transpose() << std::endl;
  }
 
}; // class Ray

template <typename Real, Integer N>
std::ostream &operator<<(std::ostream &os, Ray<Real, N> const &r) {
  r.print(os);
  return os;
}
 
} // namespace AABBtree
 
#endif // AABBTREE_Ray_HXX