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                   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                   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
0167   void BVHSortedIntersectionsLooper(AccStructure const &accstructure, Vector3D<Precision> const &localpoint,
0168                                     Vector3D<Precision> const &localdir, Precision maxstep, Func &&userhook) const
0169   {
0170     // The following construct reserves stackspace for objects
0171     // of type IdDistPair_t WITHOUT initializing those objects
0172     using IdDistPair_t = HybridManager2::BoxIdDistancePair_t;
0173     char stackspace[VECGEOM_MAXFACETS * sizeof(IdDistPair_t)];
0174     IdDistPair_t *hitlist = reinterpret_cast<IdDistPair_t *>(&stackspace);
0175
0176     // it could be that someone passed InfinityLength<double> to this function
0177     // which we need to reduce to the float version since GetHitCandidates processes floats
0178     const float maxstep_float = std::min((float)maxstep, InfinityLength<float>());
0179     auto ncandidates          = GetHitCandidates_v(accstructure, localpoint, localdir, maxstep_float, hitlist);
0180     // sort candidates according to their bounding volume hit distance
0181     insertionsort(hitlist, ncandidates);
0182
0183     for (size_t index = 0; index < ncandidates; ++index) {
0184       auto hitbox = hitlist[index];
0185       // here we got the hit candidates
0186       // now we execute user specific code to process this "hitbox"
0187       auto done = userhook(hitbox);
0188       if (done) break;
0189     }
0190   }
0191
0192   template <typename AccStructure, typename Func>
0193   VECGEOM_FORCE_INLINE
0194   void BVHContainsLooper(AccStructure const &accstructure, Vector3D<Precision> const &localpoint, Func &&userhook) const
0195   {
0196     size_t hitlist[VECGEOM_MAXFACETS];
0197     auto ncandidates = GetContainingCandidates_v(accstructure, localpoint, hitlist);
0198     for (size_t index = 0; index < ncandidates; ++index) {
0199       auto hitbox = hitlist[index];
0200       auto done   = userhook(hitbox);
0201       if (done) break;
0202     }
0203   }
0204
0205   VECGEOM_FORCE_INLINE
0206   virtual bool CheckDaughterIntersections(LogicalVolume const *lvol, Vector3D<Precision> const &localpoint,
0207                                           Vector3D<Precision> const &localdir, NavigationState const *in_state,
0208                                           NavigationState * /*out_state*/, Precision &step,
0209                                           VPlacedVolume const *&hitcandidate) const override
0210   {
0211     if (lvol->GetDaughtersp()->size() == 0) return false;
0212     auto &accstructure = *fAccelerationManager.GetAccStructure(lvol);
0213
0214     float maxstep = step;
0215     BVHSortedIntersectionsLooper(
0216         accstructure, localpoint, localdir, maxstep, [&](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             Precision ddistance            = candidate->DistanceToIn(localpoint, localdir, step);
0221             const auto valid               = !IsInf(ddistance) && ddistance < step &&
0222                                !((ddistance <= 0.) && in_state && in_state->GetLastExited() == candidate);
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     return false;
0230   }
0231
0232   VECGEOM_FORCE_INLINE
0233   virtual bool CheckDaughterIntersections(LogicalVolume const *lvol, Vector3D<Precision> const &localpoint,
0234                                           Vector3D<Precision> const &localdir, VPlacedVolume const *blocked,
0235                                           Precision &step, VPlacedVolume const *&hitcandidate) const override
0236   {
0237     if (lvol->GetDaughtersp()->size() == 0) return false;
0238     auto &accstructure = *fAccelerationManager.GetAccStructure(lvol);
0239
0240     const float maxstep = step;
0241     BVHSortedIntersectionsLooper(accstructure, localpoint, localdir, maxstep,
0242                                  [&](HybridManager2::BoxIdDistancePair_t hitbox) {
0243                                    // only consider those hitboxes which are within potential reach of this step
0244                                    if (!(step < hitbox.second)) {
0245                                      // To reuse in printing below - else move it into 'if'
0246                                      Vector3D<Precision> normal;
0247                                      VPlacedVolume const *candidate = LookupDaughter(lvol, hitbox.first);
0248                                      if (candidate == blocked) {
0249                                        // return false; // return early and go on in the looper
0250                                        candidate->Normal(localpoint, normal);
0251                                        if (normal.Dot(localdir) >= 0.0) {
0252                                          std::cerr << "HybridNav2> blocked " << candidate
0253                                                    << " has normal.dir = " << normal.Dot(localdir) << " and distToIn = "
0254                                                    << candidate->DistanceToIn(localpoint, localdir, step) << "\n";
0255                                        }
0256                                      }
0257                                      const Precision ddistance = candidate->DistanceToIn(localpoint, localdir, step);
0258                                      const auto valid          = !IsInf(ddistance) && ddistance < step &&
0259                                                         !((ddistance <= 0.) &&
0260                                                           blocked == candidate); // && normal.Dot(localdir) > 0.0);
0261                                      hitcandidate              = valid ? candidate : hitcandidate;
0262                                      step                      = valid ? ddistance : step;
0263 #if 0 // enable for debugging
0264         if ( ddistance<=0 ) {
0265            std::cerr << "HybridNav2> negative distance found for " << candidate->GetName() << "\n";
0266            auto inside = candidate->Inside(localpoint);
0267            static std::string InsideCode[4] = { "N/A", "Inside", "Surface", "Outside" } ;
0268            std::cerr << InsideCode[inside];
0269            const auto transf = candidate->GetTransformation();
0270            const auto unpl = candidate->GetUnplacedVolume();
0271            Vector3D<Precision> normalDg;
0272            const auto testdaughterlocal = transf->Transform(localpoint);
0273            auto valid = unpl->Normal(testdaughterlocal, normalDg);
0274
0275            const auto directiondaughterlocal = transf->TransformDirection(localdir);
0276            const auto dot = normalDg.Dot(directiondaughterlocal);
0277            std::cerr << " normal.dir = " << dot;
0278            if (dot >= 0) {
0279              std::cerr << " exiting " << valid << "\n";
0280            }
0281            else {
0282              std::cerr << " entering " << valid << "\n";
0283            }
0284         }
0285 #endif
0286                                      return false; // not yet done; need to continue in looper
0287                                    }
0288                                    return true; // mark done in this case
0289                                  });
0290     return false;
0291   }
0292
0293   static VNavigator *Instance()
0294   {
0295     static HybridNavigator instance;
0296     return &instance;
0297   }
0298
0299   static constexpr const char *gClassNameString = "HybridNavigator";
0300   typedef SimpleABBoxSafetyEstimator SafetyEstimator_t;
0301 };
0302 } // namespace VECGEOM_IMPL_NAMESPACE
0303 } // namespace vecgeom
0304
0305 #endif