Line data    Source code

       1             : #include "TraverseAABBJosephsMethod.h"
       2             : 
       3             : namespace elsa
       4             : {
       5             : 
       6             :     TraverseAABBJosephsMethod::TraverseAABBJosephsMethod(const BoundingBox& aabb,
       7             :                                                          const RealRay_t& r)
       8             :         : _aabb{aabb}
       9       16135 :     {
      10       16135 :         initStepDirection(r.direction());
      11             : 
      12       16135 :         RealRay_t rt(r.origin(), r.direction());
      13             : 
      14             :         // determinge length of entire intersection with AABB
      15       16135 :         real_t intersectionLength = calculateAABBIntersections(rt);
      16             : 
      17       16135 :         if (!isInBoundingBox())
      18        1258 :             return;
      19             : 
      20       14877 :         selectClosestVoxel(_entryPoint);
      21             : 
      22             :         // exit direction stored in _exitDirection
      23       14877 :         (_exitPoint - (_exitPoint.array().floor() + static_cast<real_t>(0.5)).matrix())
      24       14877 :             .cwiseAbs()
      25       14877 :             .maxCoeff(&_exitDirection);
      26             : 
      27       14877 :         moveToFirstSamplingPoint(r.direction(), intersectionLength);
      28             : 
      29             :         // last pixel/voxel handled separately, reduce box dimensionality for easy detection
      30       14877 :         int mainDir;
      31       14877 :         rt.direction().cwiseAbs().maxCoeff(&mainDir);
      32       14877 :         real_t prevBorder = std::floor(_exitPoint(mainDir));
      33       14877 :         if (prevBorder == _exitPoint(mainDir) && rt.direction()[mainDir] > 0) {
      34        6904 :             prevBorder--;
      35        6904 :         }
      36       14877 :         if (rt.direction()[mainDir] < 0) {
      37        4096 :             _aabb._min[mainDir] = prevBorder + 1;
      38       10781 :         } else {
      39       10781 :             _aabb._max[mainDir] = prevBorder;
      40       10781 :         }
      41       14877 :     }
      42             : 
      43             :     void TraverseAABBJosephsMethod::updateTraverse()
      44      172052 :     {
      45      172052 :         _fractionals += _nextStep;
      46      483484 :         for (int i = 0; i < _aabb._dim; i++) {
      47             :             //*0.5 because a change of 1 spacing corresponds to a change of
      48             :             // fractionals from -0.5 to 0.5  0.5 or -0.5 = voxel border is still
      49             :             // ok
      50      311432 :             if (std::abs(_fractionals(i)) > 0.5) {
      51      221796 :                 _fractionals(i) -= _stepDirection(i);
      52      221796 :                 _currentPos(i) += _stepDirection(i);
      53             :                 // --> is the traverse algorithm still in the bounding box?
      54      221796 :                 _isInAABB = _isInAABB && isCurrentPositionInAABB(i);
      55      221796 :             }
      56      311432 :         }
      57      172052 :         switch (_stage) {
      58       14562 :             case FIRST:
      59       14562 :                 _nextStep.cwiseAbs().maxCoeff(&_ignoreDirection);
      60       14562 :                 if (_isInAABB) {
      61             :                     // now ignore main direction
      62       14038 :                     _nextStep /= std::abs(_nextStep(_ignoreDirection));
      63       14038 :                     _fractionals(_ignoreDirection) = 0;
      64       14038 :                     _intersectionLength = _nextStep.norm();
      65       14038 :                     _stage = INTERIOR;
      66       14038 :                 }
      67       14562 :                 [[fallthrough]];
      68      153248 :             case INTERIOR:
      69      153248 :                 if (!_isInAABB) {
      70             :                     // revert to exit position and adjust values
      71       14636 :                     real_t prevBorder = _nextStep(_ignoreDirection) < 0
      72       14636 :                                             ? _aabb._min[_ignoreDirection]
      73       14636 :                                             : _aabb._max[_ignoreDirection];
      74       14636 :                     _nextStep.normalize();
      75       14636 :                     _intersectionLength =
      76       14636 :                         (_exitPoint[_ignoreDirection] - prevBorder) / (_nextStep[_ignoreDirection]);
      77             :                     // determine exit direction (the exit coordinate furthest from the center of the
      78             :                     // volume)
      79       14636 :                     _ignoreDirection = _exitDirection;
      80             :                     // move to last sampling point
      81       14636 :                     RealVector_t currentPosition = _exitPoint - _intersectionLength * _nextStep / 2;
      82       14636 :                     selectClosestVoxel(currentPosition);
      83       14636 :                     initFractionals(currentPosition);
      84       14636 :                     _isInAABB = true;
      85       14636 :                     _stage = LAST;
      86       14636 :                 }
      87      153248 :                 break;
      88       15044 :             case LAST:
      89       15044 :                 _isInAABB = false;
      90       15044 :                 break;
      91       14562 :             default:
      92           0 :                 break;
      93      172052 :         }
      94      172052 :     }
      95             : 
      96      171888 :     const RealVector_t& TraverseAABBJosephsMethod::getFractionals() const { return _fractionals; }
      97      173302 :     int TraverseAABBJosephsMethod::getIgnoreDirection() const { return _ignoreDirection; }
      98             : 
      99             :     void TraverseAABBJosephsMethod::initFractionals(const RealVector_t& currentPosition)
     100       29675 :     {
     101       88389 :         for (int i = 0; i < _aabb._dim; i++)
     102       58714 :             _fractionals(i) =
     103       58714 :                 static_cast<real_t>(std::abs(currentPosition(i)) - _currentPos(i) - 0.5);
     104       29675 :     }
     105             : 
     106             :     void TraverseAABBJosephsMethod::moveToFirstSamplingPoint(const RealVector_t& rd,
     107             :                                                              real_t intersectionLength)
     108       14876 :     {
     109             :         // determine main direction
     110       14876 :         rd.cwiseAbs().maxCoeff(&_ignoreDirection);
     111             : 
     112             :         // initialize _nextStep as the step for interior pixels
     113       14876 :         _nextStep(_ignoreDirection) = _stepDirection(_ignoreDirection);
     114       44455 :         for (int i = 0; i < _aabb._dim; ++i) {
     115       29579 :             if (i != _ignoreDirection) {
     116             :                 // tDelta(i) is given in relation to tDelta(_ignoreDirection)
     117       14883 :                 _nextStep(i) = _nextStep(_ignoreDirection) * rd(i) / rd(_ignoreDirection);
     118       14883 :             }
     119       29579 :         }
     120             : 
     121       14876 :         RealVector_t currentPosition = _entryPoint;
     122             :         // move to first sampling point
     123       14876 :         if (std::abs(_entryPoint[_ignoreDirection] - _exitPoint[_ignoreDirection])
     124       14876 :             <= static_cast<real_t>(1.)) {
     125         397 :             _stage = LAST;
     126             :             // use midpoint of intersection as the interpolation value
     127         397 :             currentPosition += rd * intersectionLength / 2;
     128         397 :             _intersectionLength = intersectionLength;
     129       14479 :         } else {
     130             :             // find distance to next plane orthogonal to main direction
     131       14479 :             real_t nextBoundary = std::trunc(_currentPos(_ignoreDirection));
     132       14479 :             if (_stepDirection(_ignoreDirection) > 0)
     133       10566 :                 nextBoundary += static_cast<real_t>(1.);
     134             : 
     135       14479 :             real_t distToBoundary =
     136       14479 :                 (nextBoundary - _entryPoint[_ignoreDirection]) / rd[_ignoreDirection];
     137             : 
     138       14479 :             currentPosition += rd * distToBoundary / 2;
     139       14479 :             _nextStep = _nextStep / 2 + _nextStep * (distToBoundary / (2 * _nextStep.norm()));
     140       14479 :             _intersectionLength = distToBoundary;
     141             : 
     142             :             // determine entry direction (the entry coordinate furthest from the center of the
     143             :             // volume)
     144       14479 :             (_entryPoint - (_entryPoint.array().floor() + static_cast<real_t>(0.5)).matrix())
     145       14479 :                 .cwiseAbs()
     146       14479 :                 .maxCoeff(&_ignoreDirection);
     147       14479 :         }
     148       14876 :         selectClosestVoxel(currentPosition);
     149             : 
     150             :         // init fractionals
     151       14876 :         initFractionals(currentPosition);
     152       14876 :     }
     153             : 
     154             :     inline bool TraverseAABBJosephsMethod::isCurrentPositionInAABB(index_t index) const
     155      216058 :     {
     156      216058 :         return _currentPos(index) < _aabb._max(index) && _currentPos(index) >= _aabb._min(index);
     157      216058 :     }
     158             : 
     159             :     void TraverseAABBJosephsMethod::selectClosestVoxel(const RealVector_t& currentPosition)
     160       44220 :     {
     161       44220 :         RealVector_t lowerCorner = currentPosition.array().floor();
     162       44220 :         lowerCorner =
     163       44220 :             (((lowerCorner.array() == currentPosition.array()) && (_stepDirection.array() < 0.0))
     164       44220 :              || currentPosition.array() == _aabb._max.array())
     165       44220 :                 .select(lowerCorner.array() - 1, lowerCorner);
     166             : 
     167             :         // --> If ray is parallel and we are close, choose the next previous/next voxel
     168       44220 :         _currentPos = lowerCorner;
     169             : 
     170             :         // check if we are still inside the aabb
     171       44220 :         if ((_currentPos.array() >= _aabb._max.array()).any()
     172       44220 :             || (_currentPos.array() < _aabb._min.array()).any())
     173       14672 :             _isInAABB = false;
     174       44220 :     }
     175             : 
     176             :     RealVector_t floorFallback(const RealVector_t& v)
     177           0 :     {
     178           0 :         RealVector_t ret(v.rows());
     179           0 :         for (int i = 0; i < v.rows(); i++)
     180           0 :             ret(i) = std::floor(v(i));
     181           0 :         return ret;
     182           0 :     }
     183             : 
     184             :     void TraverseAABBJosephsMethod::initStepDirection(const RealVector_t& rd)
     185       16129 :     {
     186       16129 :         _stepDirection = rd.array().sign();
     187       16129 :     }
     188             : 
     189             :     real_t TraverseAABBJosephsMethod::calculateAABBIntersections(const RealRay_t& ray)
     190       16138 :     {
     191       16138 :         real_t tmin;
     192             : 
     193             :         // --> calculate intersection parameter and if the volume is hit
     194       16138 :         auto opt = Intersection::withRay(_aabb, ray);
     195             : 
     196       16138 :         if (opt) {
     197       14971 :             _isInAABB = true;
     198       14971 :             tmin = opt->_tmin;
     199             : 
     200             :             // --> get points at which they intersect
     201       14971 :             _entryPoint = ray.pointAt(tmin);
     202             : 
     203             :             // --> because of floating point error it can happen, that values are out of
     204             :             // the bounding box, this can lead to errors
     205       14971 :             _entryPoint =
     206       14971 :                 (_entryPoint.array() < _aabb._min.array()).select(_aabb._min, _entryPoint);
     207       14971 :             _entryPoint =
     208       14971 :                 (_entryPoint.array() > _aabb._max.array()).select(_aabb._max, _entryPoint);
     209             : 
     210             :             // exit point stored in _exitPoint
     211       14971 :             _exitPoint = ray.pointAt(opt->_tmax);
     212       14971 :             _exitPoint = (_exitPoint.array() < _aabb._min.array()).select(_aabb._min, _exitPoint);
     213       14971 :             _exitPoint = (_exitPoint.array() > _aabb._max.array()).select(_aabb._max, _exitPoint);
     214             : 
     215       14971 :             return opt->_tmax - opt->_tmin;
     216       14971 :         } else {
     217        1167 :             return 0;
     218        1167 :         }
     219       16138 :     }
     220             : 
     221      205078 :     bool TraverseAABBJosephsMethod::isInBoundingBox() const { return _isInAABB; }
     222             : 
     223      169679 :     real_t TraverseAABBJosephsMethod::getIntersectionLength() const { return _intersectionLength; }
     224             : 
     225             :     IndexVector_t TraverseAABBJosephsMethod::getCurrentVoxel() const
     226      173340 :     {
     227      173340 :         return _currentPos.template cast<index_t>();
     228      173340 :     }
     229             : 
     230             : } // namespace elsa