Line data    Source code

       1             : #include "TraverseAABB.h"
       2             : #include "Intersection.h"
       3             : 
       4             : namespace elsa
       5             : {
       6             :     template <int dim>
       7             :     TraverseAABB<dim>::TraverseAABB(const BoundingBox& aabb, const RealRay_t& r,
       8             :                                     IndexArray_t<dim> productOfCoefficientsPerDimension)
       9             :         : _productOfCoefficientsPerDimension{std::move(productOfCoefficientsPerDimension)}
      10       61901 :     {
      11       61901 :         static_assert(dim == 2 || dim == 3);
      12       61901 :         _aabbMin = aabb.min();
      13       61901 :         _aabbMax = aabb.max();
      14             : 
      15             :         // compute the first intersection
      16       61901 :         const RealArray_t<dim> entryPoint = calculateAABBIntersections(r, aabb);
      17       61901 :         if (!isInBoundingBox()) // early abort if necessary
      18        5861 :             return;
      19             : 
      20             :         // constant array containing epsilon
      21       56040 :         const RealArray_t<dim> EPS{
      22       56040 :             RealArray_t<dim>().setConstant(std::numeric_limits<real_t>::epsilon())};
      23             : 
      24             :         // constant vector containing the maximum number
      25       56040 :         const RealArray_t<dim> MAX{
      26       56040 :             RealArray_t<dim>().setConstant(std::numeric_limits<real_t>::max())};
      27             : 
      28             :         // determine whether we go up/down or left/right
      29       56040 :         initStepDirection(r.direction());
      30             : 
      31             :         // select the initial voxel (closest to the entry point)
      32       56040 :         selectClosestVoxel(entryPoint);
      33             : 
      34             :         // initialize the step sizes for the step parameter
      35       56040 :         initDelta(r.direction(), EPS, MAX);
      36             : 
      37             :         // initialize the maximum step parameters
      38       56040 :         initT(r.direction(), EPS, MAX, entryPoint);
      39       56040 :         initCurrentIndex();
      40       56040 :     }
      41             : 
      42             :     template <int dim>
      43             :     void TraverseAABB<dim>::updateTraverse()
      44     2174542 :     {
      45             :         // --> calculate the mask that masks out all but the lowest t values
      46     2174542 :         calcMask();
      47             : 
      48             :         // --> step into the current direction
      49     2174542 :         _currentPos += _mask.select(_stepDirection.template cast<real_t>(), 0);
      50             : 
      51             :         // --> update the T for next iteration
      52     2174542 :         _T += _mask.select(_tDelta, 0);
      53             : 
      54             :         // --> check if we are still in bounding box
      55     2174542 :         _isInAABB = isCurrentPositionInAABB();
      56     2174542 :     }
      57             : 
      58             :     template <int dim>
      59             :     void TraverseAABB<dim>::calcMask()
      60     2073255 :     {
      61     2073255 :         if constexpr (dim == 2) {
      62       63225 :             _mask[0] = (_T[0] <= _T[1]);
      63       63225 :             _mask[1] = (_T[1] <= _T[0]);
      64       63225 :         } else if constexpr (dim == 3) {
      65       63009 :             _mask[0] = ((_T[0] <= _T[1]) && (_T[0] <= _T[2]));
      66       63009 :             _mask[1] = ((_T[1] <= _T[0]) && (_T[1] <= _T[2]));
      67       63009 :             _mask[2] = ((_T[2] <= _T[0]) && (_T[2] <= _T[1]));
      68       63009 :         }
      69     2073255 :     }
      70             : 
      71             :     template <int dim>
      72             :     real_t TraverseAABB<dim>::updateTraverseAndGetDistance()
      73     1709034 :     {
      74             :         // --> compute the distance
      75     1709034 :         real_t tEntry = _tExit;
      76     1709034 :         _tExit = _T.minCoeff();
      77             : 
      78             :         // --> do the update
      79     1709034 :         updateTraverse();
      80     1709034 :         updateCurrentIndex();
      81             : 
      82     1709034 :         return (_tExit - tEntry);
      83     1709034 :     }
      84             : 
      85             :     template <int dim>
      86             :     bool TraverseAABB<dim>::isInBoundingBox() const
      87     2402903 :     {
      88     2402903 :         return _isInAABB;
      89     2402903 :     }
      90             : 
      91             :     template <int dim>
      92             :     IndexArray_t<dim> TraverseAABB<dim>::getCurrentVoxel() const
      93      598505 :     {
      94      598505 :         return _currentPos.template cast<index_t>();
      95      598505 :     }
      96             : 
      97             :     template <int dim>
      98             :     index_t TraverseAABB<dim>::getCurrentIndex() const
      99     1850541 :     {
     100     1850541 :         return _currentIndex;
     101     1850541 :     }
     102             : 
     103             :     template <int dim>
     104             :     RealArray_t<dim> TraverseAABB<dim>::calculateAABBIntersections(const RealRay_t& r,
     105             :                                                                    const BoundingBox& aabb)
     106       61778 :     {
     107       61778 :         RealArray_t<dim> entryPoint;
     108             :         // entry and exit point parameters
     109       61778 :         real_t tmin;
     110             : 
     111             :         // --> calculate intersection parameter and if the volume is hit
     112       61778 :         auto opt = intersectRay(aabb, r);
     113             : 
     114       61778 :         if (opt) { // hit!
     115       56524 :             _isInAABB = true;
     116       56524 :             tmin = opt->_tmin;
     117             : 
     118             :             // --> get points at which they intersect
     119       56524 :             entryPoint = r.pointAt(tmin);
     120             : 
     121             :             // --> because of floating point error it can happen, that values are out of
     122             :             // the bounding box, this can lead to errors
     123       56524 :             entryPoint = (entryPoint < _aabbMin).select(_aabbMin, entryPoint);
     124       56524 :             entryPoint = (entryPoint > _aabbMax).select(_aabbMax, entryPoint);
     125       56524 :         }
     126       61778 :         return entryPoint;
     127       61778 :     }
     128             : 
     129             :     template <int dim>
     130             :     void TraverseAABB<dim>::initStepDirection(const RealArray_t<dim>& rd)
     131       56147 :     {
     132       56147 :         _stepDirection = rd.sign().template cast<index_t>();
     133       56147 :     }
     134             : 
     135             :     template <int dim>
     136             :     void TraverseAABB<dim>::selectClosestVoxel(const RealArray_t<dim>& entryPoint)
     137       56223 :     {
     138       56223 :         RealArray_t<dim> lowerCorner = entryPoint.floor();
     139       56223 :         lowerCorner = ((lowerCorner == entryPoint) && (_stepDirection < 0.0))
     140       56223 :                           .select(lowerCorner - 1, lowerCorner);
     141             : 
     142       56223 :         _currentPos = lowerCorner;
     143             : 
     144             :         // check if we are still inside the aabb
     145       56312 :         if ((_currentPos >= _aabbMax).any() || (_currentPos < _aabbMin).any())
     146          12 :             _isInAABB = false;
     147       56223 :     }
     148             : 
     149             :     template <int dim>
     150             :     void TraverseAABB<dim>::initDelta(const RealArray_t<dim>& rd, const RealArray_t<dim>& EPS,
     151             :                                       const RealArray_t<dim>& MAX)
     152       56178 :     {
     153       56178 :         RealArray_t<dim> tdelta = _stepDirection.template cast<real_t>() / rd;
     154             : 
     155       56178 :         _tDelta = (Eigen::abs(rd) > EPS).select(tdelta, MAX);
     156       56178 :     }
     157             : 
     158             :     template <int dim>
     159             :     void TraverseAABB<dim>::initT(const RealArray_t<dim>& rd, const RealArray_t<dim>& EPS,
     160             :                                   const RealArray_t<dim>& MAX, const RealArray_t<dim>& entryPoint)
     161       56186 :     {
     162       56186 :         RealArray_t<dim> T =
     163       56186 :             (((rd > 0.0f).select(_currentPos + 1., _currentPos)) - entryPoint) / rd;
     164             : 
     165       56186 :         _T = (Eigen::abs(rd) > EPS).select(T, MAX);
     166       56186 :     }
     167             : 
     168             :     template <int dim>
     169             :     bool TraverseAABB<dim>::isCurrentPositionInAABB() const
     170     2086513 :     {
     171     2193193 :         return (_currentPos < _aabbMax).all() && (_currentPos >= _aabbMin).all();
     172     2086513 :     }
     173             : 
     174             :     template <int dim>
     175             :     void TraverseAABB<dim>::updateCurrentIndex()
     176     1693860 :     {
     177     1693860 :         _currentIndex +=
     178     1693860 :             (_stepDirection * _mask.select(_productOfCoefficientsPerDimension, 0)).sum();
     179     1693860 :     }
     180             :     template <int dim>
     181             :     void TraverseAABB<dim>::initCurrentIndex()
     182       56155 :     {
     183       56155 :         _currentIndex = (_productOfCoefficientsPerDimension * getCurrentVoxel()).sum();
     184       56155 :     }
     185             : 
     186             :     template class TraverseAABB<2>;
     187             :     template class TraverseAABB<3>;
     188             : } // namespace elsa