Ray.hxx

Go to the documentation of this file.

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *\
 * Copyright (c) 2025, 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 *
 * e-mail: davide.stocco@unitn.it                             e-mail: 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.");
 
    // Constants for numerical computations
    constexpr static Real EPS{std::numeric_limits<Real>::epsilon()};  
    constexpr static Real INF{std::numeric_limits<Real>::infinity()}; 
    constexpr static Real DUMMY_TOL{EPS*static_cast<Real>(100.0)};    
 
    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 = DUMMY_TOL) 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 = DUMMY_TOL) 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 = DUMMY_TOL) 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(-INF), t_max.setConstant(INF);
      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 = DUMMY_TOL) 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(-INF), t_max.setConstant(INF);
      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 = DUMMY_TOL) 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 = DUMMY_TOL) 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 = DUMMY_TOL) const
    {return std::sqrt(this->squared_distance(p, tol));}

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

    Real squared_interior_distance(Box<Real, N> const & b, Real tol = DUMMY_TOL) 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 = DUMMY_TOL) 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(-INF); t_max.setConstant(INF);
      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 = DUMMY_TOL) const
    {return std::sqrt(this->squared_interior_distance(b, tol));}

    Real interior_distance(Box<Real, N> const & b, Point & p1, Point & p2, Real tol = DUMMY_TOL) 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 = DUMMY_TOL) 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 = DUMMY_TOL)
    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(-INF); t_max.setConstant(INF);
      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 = DUMMY_TOL) const
    {return std::sqrt(this->squared_exterior_distance(b, tol));}

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

    void print(std::ostream & os) const {
      os <<
        "────────────────────────────────────────────────────────────────────────────────\n" <<
        "RAY INFO\n" <<
        "\to = " << m_origin.transpose()    << '\n' <<
        "\td = " << m_direction.transpose() << '\n' <<
        "────────────────────────────────────────────────────────────────────────────────\n";
    }
 
  }; // 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