VecGeom/navigation/HybridNavigator2.h

0001 /*
0002  * HybridNavigator2.h
0003  *
0004  *  Created on: 27.08.2015
0005  *      Author: yang.zhang@cern.ch and sandro.wenzel@cern.ch
0006  */
0007
0008 #ifndef VECGEOM_HYBRIDNAVIGATOR
0009 #define VECGEOM_HYBRIDNAVIGATOR
0010
0011 #include "VecGeom/base/Global.h"
0012
0013 #include "VecGeom/volumes/PlacedVolume.h"
0014 #include "VecGeom/base/SOA3D.h"
0015 #include "VecGeom/base/Vector3D.h"
0016 #include "VecGeom/management/GeoManager.h"
0017 #include "VecGeom/navigation/NavigationState.h"
0018 #include "VecGeom/base/Transformation3D.h"
0019 #include "VecGeom/volumes/kernel/BoxImplementation.h"
0020 #include "VecGeom/management/HybridManager2.h"
0021 #include "VecGeom/navigation/VNavigator.h"
0022 #include "VecGeom/navigation/HybridSafetyEstimator.h"
0023 #include "VecGeom/navigation/SimpleABBoxNavigator.h"
0024
0025 #include <vector>
0026 #include <cmath>
0027
0028 namespace vecgeom {
0029 inline namespace VECGEOM_IMPL_NAMESPACE {
0030
0031 // A navigator using a shallow tree of aligned bounding boxes (hybrid approach) to quickly exclude
0032 // potential hit targets.
0033 // This navigator goes into the direction of "voxel" navigators used in Geant4
0034 // and ROOT. Checking single-rays against a set of aligned bounding boxes can be done
0035 // in a vectorized fashion.
0036 template <bool MotherIsConvex = false>
0037 class HybridNavigator : public VNavigatorHelper<HybridNavigator<MotherIsConvex>, MotherIsConvex> {
0038
0039 private:
0040   HybridManager2 &fAccelerationManager;
0041   HybridNavigator()
0042       : VNavigatorHelper<HybridNavigator<MotherIsConvex>, MotherIsConvex>(SimpleABBoxSafetyEstimator::Instance()),
0043         fAccelerationManager(HybridManager2::Instance())
0044   {
0045   }
0046
0047   static VPlacedVolume const *LookupDaughter(LogicalVolume const *lvol, int const daughterIndex)
0048   {
0049     return lvol->GetDaughters()[daughterIndex];
0050   }
0051
0052   // a simple sort class (based on insertionsort)
0053   template <typename T> //, typename Cmp>
0054   static void insertionsort(T *arr, unsigned int N)
0055   {
0056     for (unsigned short i = 1; i < N; ++i) {
0057       T value    = arr[i];
0058       short hole = i;
0059
0060       for (; hole > 0 && value.second < arr[hole - 1].second; --hole)
0061         arr[hole] = arr[hole - 1];
0062
0063       arr[hole] = value;
0064     }
0065   }
0066
0067   /**
0068    * Returns list of daughter candidates containing the point.
0069    */
0070   size_t GetContainingCandidates_v(HybridManager2::HybridBoxAccelerationStructure const &accstructure,
0071                                    Vector3D<Precision> const &point, size_t *hitlist) const
0072   {
0073     using Float_v      = HybridManager2::Float_v;
0074     using Bool_v       = vecCore::Mask<Float_v>;
0075     constexpr auto kVS = vecCore::VectorSize<Float_v>();
0076     size_t count       = 0;
0077     int numberOfNodes, size;
0078     auto boxes_v                = fAccelerationManager.GetABBoxes_v(accstructure, size, numberOfNodes);
0079     auto const *nodeToDaughters = accstructure.fNodeToDaughters;
0080     for (size_t index = 0, nodeindex = 0; index < size_t(size) * 2; index += 2 * (kVS + 1), nodeindex += kVS) {
0081       Bool_v inside, inside_d;
0082       Vector3D<Float_v> *corners = &boxes_v[index];
0083       ABBoxImplementation::ABBoxContainsKernelGeneric<HybridManager2::Float_v, Precision, Bool_v>(
0084           corners[0], corners[1], point, inside);
0085       if (!vecCore::MaskEmpty(inside)) {
0086         // loop lanes
0087         for (size_t i = 0; i < kVS; ++i) {
0088           if (vecCore::MaskLaneAt(inside, i)) {
0089             corners = &boxes_v[index + 2 * (i + 1)];
0090             ABBoxImplementation::ABBoxContainsKernelGeneric<HybridManager2::Float_v, Precision, Bool_v>(
0091                 corners[0], corners[1], point, inside_d);
0092             if (!vecCore::MaskEmpty(inside_d)) {
0093               // loop lanes at second level
0094               for (size_t j = 0; j < kVS; ++j) {
0095                 if (vecCore::MaskLaneAt(inside_d, j)) {
0096                   VECGEOM_ASSERT(count < VECGEOM_MAXFACETS);
0097                   hitlist[count++] = nodeToDaughters[nodeindex + i][j];
0098                 }
0099               }
0100             }
0101           }
0102         }
0103       }
0104     }
0105     return count;
0106   }
0107
0108   /**
0109    * Returns hitlist of daughter candidates (pairs of [daughter index, step to bounding box]) crossed by ray.
0110    */
0111   size_t GetHitCandidates_v(HybridManager2::HybridBoxAccelerationStructure const &accstructure,
0112                             Vector3D<Precision> const &point, Vector3D<Precision> const &dir, float maxstep,
0113                             HybridManager2::BoxIdDistancePair_t *hitlist) const
0114   {
0115     size_t count = 0;
0116     Vector3D<Precision> invdir(1. / NonZero(dir.x()), 1. / NonZero(dir.y()), 1. / NonZero(dir.z()));
0117     Vector3D<int> sign;
0118     sign[0] = invdir.x() < 0;
0119     sign[1] = invdir.y() < 0;
0120     sign[2] = invdir.z() < 0;
0121     int numberOfNodes, size;
0122     auto boxes_v                = fAccelerationManager.GetABBoxes_v(accstructure, size, numberOfNodes);
0123     constexpr auto kVS          = vecCore::VectorSize<HybridManager2::Float_v>();
0124     auto const *nodeToDaughters = accstructure.fNodeToDaughters;
0125     for (size_t index = 0, nodeindex = 0; index < size_t(size) * 2; index += 2 * (kVS + 1), nodeindex += kVS) {
0126       HybridManager2::Float_v distance = BoxImplementation::IntersectCachedKernel2<HybridManager2::Float_v, float>(
0127           &boxes_v[index], point, invdir, sign.x(), sign.y(), sign.z(), 0, maxstep);
0128       auto hit = distance < maxstep;
0129       if (!vecCore::MaskEmpty(hit)) {
0130         for (size_t i = 0 /*hit.firstOne()*/; i < kVS; ++i) {
0131           if (vecCore::MaskLaneAt(hit, i)) {
0132             distance = BoxImplementation::IntersectCachedKernel2<HybridManager2::Float_v, float>(
0133                 &boxes_v[index + 2 * (i + 1)], point, invdir, sign.x(), sign.y(), sign.z(), 0, maxstep);
0134             auto hit1 = distance < maxstep;
0135             if (!vecCore::MaskEmpty(hit1)) {
0136               for (size_t j = 0 /*hit1.firstOne()*/; j < kVS; ++j) { // leaf node
0137                 if (vecCore::MaskLaneAt(hit1, j)) {
0138                   VECGEOM_ASSERT(count < VECGEOM_MAXFACETS);
0139                   hitlist[count] = HybridManager2::BoxIdDistancePair_t(nodeToDaughters[nodeindex + i][j],
0140                                                                        vecCore::LaneAt(distance, j));
0141                   count++;
0142                 }
0143               }
0144             }
0145           }
0146         }
0147       }
0148     }
0149     return count;
0150   }
0151
0152 public:
0153   // we provide hit detection on the local level and reuse the generic implementations from
0154   // VNavigatorHelper<SimpleABBoxNavigator>
0155
0156   // a generic looper function that
0157   // given an acceleration structure (an aligned bounding box hierarchy),
0158   // a hit-query will be performed, the intersected boxes sorted, looped over
0159   // and a user hook called for processing
0160   // the user hook needs to indicate with a boolean return value whether to continue looping (false)
0161   // or whether we are done (true) and can exit
0162
0163   // FIXME: might be generic enough to work for all possible kinds of BVH structures
0164   // FIXME: offer various sorting directions, etc.
0165   template <typename AccStructure, typename Func>
0166   VECGEOM_FORCE_INLINE void BVHSortedIntersectionsLooper(AccStructure const &accstructure,
0167                                                          Vector3D<Precision> const &localpoint,
0168                                                          Vector3D<Precision> const &localdir, Precision maxstep,
0169                                                          Func &&userhook) const
0170   {
0171     // The following construct reserves stackspace for objects
0172     // of type IdDistPair_t WITHOUT initializing those objects
0173     using IdDistPair_t = HybridManager2::BoxIdDistancePair_t;
0174     char stackspace[VECGEOM_MAXFACETS * sizeof(IdDistPair_t)];
0175     IdDistPair_t *hitlist = reinterpret_cast<IdDistPair_t *>(&stackspace);
0176
0177     // it could be that someone passed InfinityLength<double> to this function
0178     // which we need to reduce to the float version since GetHitCandidates processes floats
0179     const float maxstep_float = std::min((float)maxstep, InfinityLength<float>());
0180     auto ncandidates          = GetHitCandidates_v(accstructure, localpoint, localdir, maxstep_float, hitlist);
0181     // sort candidates according to their bounding volume hit distance
0182     insertionsort(hitlist, ncandidates);
0183
0184     for (size_t index = 0; index < ncandidates; ++index) {
0185       auto hitbox = hitlist[index];
0186       // here we got the hit candidates
0187       // now we execute user specific code to process this "hitbox"
0188       auto done = userhook(hitbox);
0189       if (done) break;
0190     }
0191   }
0192
0193   template <typename AccStructure, typename Func>
0194   VECGEOM_FORCE_INLINE void BVHContainsLooper(AccStructure const &accstructure, Vector3D<Precision> const &localpoint,
0195                                               Func &&userhook) const
0196   {
0197     size_t hitlist[VECGEOM_MAXFACETS];
0198     auto ncandidates = GetContainingCandidates_v(accstructure, localpoint, hitlist);
0199     for (size_t index = 0; index < ncandidates; ++index) {
0200       auto hitbox = hitlist[index];
0201       auto done   = userhook(hitbox);
0202       if (done) break;
0203     }
0204   }
0205
0206   VECGEOM_FORCE_INLINE
0207   virtual bool CheckDaughterIntersections(LogicalVolume const *lvol, Vector3D<Precision> const &localpoint,
0208                                           Vector3D<Precision> const &localdir, NavigationState const *in_state,
0209                                           NavigationState * /*out_state*/, Precision &step,
0210                                           VPlacedVolume const *&hitcandidate) const override
0211   {
0212     if (lvol->GetDaughtersp()->size() == 0) return false;
0213     auto &accstructure = *fAccelerationManager.GetAccStructure(lvol);
0214
0215     float maxstep             = static_cast<float>(step);
0216     auto CheckHitboxCandidate = [&](HybridManager2::BoxIdDistancePair_t hitbox) {
0217       // only consider those hitboxes which are within potential reach of this step
0218       if (!(step < hitbox.second)) {
0219         VPlacedVolume const *candidate = LookupDaughter(lvol, hitbox.first);
0220         if (in_state && in_state->GetLastExited() == candidate) return false;
0221         Precision ddistance = candidate->DistanceToIn(localpoint, localdir, step);
0222         const auto valid    = ddistance < step;
0223         hitcandidate        = valid ? candidate : hitcandidate;
0224         step                = valid ? ddistance : step;
0225         return false; // not yet done; need to continue in looper
0226       }
0227       return true; // mark done in this case
0228     };
0229     BVHSortedIntersectionsLooper(accstructure, localpoint, localdir, maxstep, CheckHitboxCandidate);
0230     return false;
0231   }
0232
0233   VECGEOM_FORCE_INLINE
0234   virtual bool CheckDaughterIntersections(LogicalVolume const *lvol, Vector3D<Precision> const &localpoint,
0235                                           Vector3D<Precision> const &localdir, VPlacedVolume const *blocked,
0236                                           Precision &step, VPlacedVolume const *&hitcandidate) const override
0237   {
0238     if (lvol->GetDaughtersp()->size() == 0) return false;
0239     auto &accstructure = *fAccelerationManager.GetAccStructure(lvol);
0240
0241     const float maxstep = static_cast<float>(step);
0242     BVHSortedIntersectionsLooper(
0243         accstructure, localpoint, localdir, maxstep, [&](HybridManager2::BoxIdDistancePair_t hitbox) {
0244           // only consider those hitboxes which are within potential reach of this step
0245           if (!(step < hitbox.second)) {
0246             // To reuse in printing below - else move it into 'if'
0247             Vector3D<Precision> normal;
0248             VPlacedVolume const *candidate = LookupDaughter(lvol, hitbox.first);
0249             if (candidate == blocked) {
0250               // return false; // return early and go on in the looper
0251               candidate->Normal(localpoint, normal);
0252               if (normal.Dot(localdir) >= 0.0) {
0253                 std::cerr << "HybridNav2> blocked " << candidate << " has normal.dir = " << normal.Dot(localdir)
0254                           << " and distToIn = " << candidate->DistanceToIn(localpoint, localdir, step) << "\n";
0255               }
0256               return false;
0257             }
0258             const Precision ddistance = candidate->DistanceToIn(localpoint, localdir, step);
0259             const auto valid          = !IsInf(ddistance) && ddistance < step && ddistance > -kTolerance;
0260             hitcandidate              = valid ? candidate : hitcandidate;
0261             step                      = valid ? ddistance : step;
0262             return false; // not yet done; need to continue in looper
0263           }
0264           return true; // mark done in this case
0265         });
0266     return false;
0267   }
0268
0269   static VNavigator *Instance()
0270   {
0271     static HybridNavigator instance;
0272     return &instance;
0273   }
0274
0275   static constexpr const char *gClassNameString = "HybridNavigator";
0276   typedef SimpleABBoxSafetyEstimator SafetyEstimator_t;
0277 };
0278 } // namespace VECGEOM_IMPL_NAMESPACE
0279 } // namespace vecgeom
0280
0281 #endif