HashGrids.h 37.8 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
//======================================================================================================================
//
//  This file is part of waLBerla. waLBerla is free software: you can
//  redistribute it and/or modify it under the terms of the GNU General Public
//  License as published by the Free Software Foundation, either version 3 of
//  the License, or (at your option) any later version.
//
//  waLBerla is distributed in the hope that it will be useful, but WITHOUT
//  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
//  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
//  for more details.
//
//  You should have received a copy of the GNU General Public License along
//  with waLBerla (see COPYING.txt). If not, see <http://www.gnu.org/licenses/>.
//
//! \file ICCD.h
//! \author Klaus Iglberger
//! \author Florian Schornbaum
//! \author Sebastian Eibl <sebastian.eibl@fau.de>
//
//======================================================================================================================

#pragma once


//*************************************************************************************************
// Includes
//*************************************************************************************************

#include "ICCD.h"
#include <pe/rigidbody/BodyStorage.h>
#include <pe/Types.h>

#include <core/logging/Logging.h>
#include <core/debug/Debug.h>
#include <core/NonCopyable.h>

#include <cmath>
#include <list>
#include <sstream>
#include <vector>

43
#include <unordered_set>
Lukas Werner's avatar
Lukas Werner committed
44
#include <pe/raytracing/Ray.h>
45
46
#include <pe/utility/BodyCast.h>
#include <pe/raytracing/Intersects.h>
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83

namespace walberla{
namespace pe{

class BodyStorage;

namespace ccd {

//=================================================================================================
//
//  CLASS DEFINITION
//
//=================================================================================================

//*************************************************************************************************
/*!\brief Implementation of the 'Hierarchical Hash Grids' coarse collision detection algorithm.
 *
 * The 'Hierarchical Hash Grids' coarse collision detection algorithm is based on a spatial
 * partitioning scheme that uses a hierarchy of uniform, hash storage based grids. Uniform grids
 * subdivide the simulation space into cubic cells. A hierarchy of spatially superimposed grids
 * provides a set of grids with each grid subdividing the very same space, however possessing
 * different and thus unique-sized cells. Spatial partitioning is achieved by assigning every
 * rigid body to exactly one cell in exactly one grid - that is the grid with the smallest cells
 * that are larger than the rigid body (more precisely cells that are larger than the longest
 * edge of the rigid body's axis-aligned bounding box). As a consequence, the collision detection
 * is reduced to only checking body's that are assigned to spatially directly adjacent cells.
 *
 * Key features of this implementation are not only an <b>average-case computational complexity
 * of order O(N)</b> as well as a space complexity of order O(N), but also a short actual runtime
 * combined with low memory consumption. Moreover, the data structure representing the hierarchy
 * of grids has the ability to, at runtime, automatically and perfectly adapt to the bodies of the
 * underlying simulation. Also, arbitrary bodies can be added and removed in constant time, O(1).
 * Consequently, the coarse collision detection algorithm based on these hierarchical hash grids
 * is <b>especially well-suited for large-scale simulations</b> involving very large numbers of
 * bodies.
 *
 * For further information and a much more detailed explanation of this algorithm see
84
 *     https://www10.cs.fau.de/publications/theses/2009/Schornbaum_SA_2009.pdf
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
 */
class HashGrids : public ICCD
{
public:
   //**Constants **********************************************************************************
   // for documentation see cpp file
   static const size_t xCellCount;
   static const size_t yCellCount;
   static const size_t zCellCount;
   static const size_t cellVectorSize;
   static const size_t occupiedCellsVectorSize;
   static const size_t minimalGridDensity;
   static const size_t gridActivationThreshold;
   static const real_t hierarchyFactor;
   //**********************************************************************************************
100
101
102
   
   static uint64_t intersectionTestCount; // ToDo remove again
   
103
private:
104
105
   //**Type definitions****************************************************************************
   //! Vector for storing (handles to) rigid bodies.
106
   using BodyVector = std::vector<BodyID>;
107
108
109
110
111
112
113
114
115
116
   //**********************************************************************************************

   //**********************************************************************************************
   /*!\brief Implementation of the hash grid data structure.
   */
   class HashGrid
   {
    private:
      //**Type definitions*************************************************************************
      //! The signed integer type that is used for storing offsets to neighboring cells.
117
      using offset_t = long;
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
      //*******************************************************************************************

      //*******************************************************************************************
      /*!\brief Data structure for representing a cell in the hash grid (used by the 'Hierarchical
      //        Hash Grids' coarse collision detection algorithm).
      */
      struct Cell
      {
         BodyVector* bodies_;           /*!< \brief The cell's body container that stores (handles to)
                                             all the bodies that are assigned to this cell. */
                                        /*!< Note that only a pointer to such a body container is
                                             stored: in order to save memory, this container object
                                             is only allocated as long as there are bodies assigned
                                             to this cell. */
         offset_t*   neighborOffset_;   /*!< \brief Pointer to an array that is storing offsets that
                                             can be used to directly access all the neighboring
                                             cells in the hash grid. */
         size_t      occupiedCellsId_;  //!< The cell's index in the \a occupiedCells_ vector.
         int         lastNonFixedBody_; //!< marks the last body in the array which is not fixed
      };
      //*******************************************************************************************

      //**Type definitions*************************************************************************
      //! Vector for storing pointers to (body-occupied) cells.
142
      using CellVector = std::vector<Cell *>;
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
      //*******************************************************************************************

    public:
      //**Constructor******************************************************************************
      /*!\name Constructor */
      //@{
      explicit HashGrid( real_t cellSpan );
      //@}
      //*******************************************************************************************

      //**Destructor*******************************************************************************
      /*!\name Destructor */
      //@{
      ~HashGrid();
      //@}
      //*******************************************************************************************

      //**Getter functions*************************************************************************
      /*!\name Getter functions */
      //@{
      real_t getCellSpan() const { return cellSpan_; }  //!< Getter for \a cellSpan_.
      //@}
      //*******************************************************************************************

      //**Add/remove functions*********************************************************************
      /*!\name Add/remove functions */
      //@{
      inline void add   ( BodyID body );
      inline void remove( BodyID body );
      //@}
      //*******************************************************************************************

      //**Utility functions************************************************************************
      /*!\name Utility functions */
      //@{
      void update( BodyID body );

      template< typename Contacts >
      size_t process      ( BodyID** gridBodies, Contacts& contacts ) const;

      template< typename Contacts >
      void   processBodies( BodyID* bodies, size_t bodyCount, Contacts& contacts ) const;
185
186
      
      template<typename BodyTuple>
187
188
      BodyID getRayIntersectingBody(const raytracing::Ray& ray, const AABB& blockAABB, real_t& t, Vec3& n,
                                    std::function<bool (const BodyID body)> isBodyVisibleFunc) const;
189
      
190
191
      static const int BLOCKCELL_NORMAL_INDETERMINATE = 3;

192
      template<typename BodyTuple>
Lukas Werner's avatar
Lukas Werner committed
193
194
195
      BodyID getBodyIntersectionForBlockCell(const Vector3<int32_t>& blockCell,
                                             const int8_t cellNormalAxis, const int8_t cellNormalDir,
                                             const raytracing::Ray& ray,
196
197
                                             real_t& t_closest, Vec3& n_closest,
                                             std::function<bool (const BodyID body)> isBodyVisibleFunc) const;
198
      
199
200
201
202
      void clear();
      //@}
      //*******************************************************************************************

Lukas Werner's avatar
Lukas Werner committed
203

204
    private:
205
206
207
208
209
210
      //**Utility functions************************************************************************
      /*!\name Utility functions */
      //@{
      void initializeNeighborOffsets();

      size_t hash( BodyID body ) const;
211
      size_t hashPoint(real_t x, real_t y, real_t z) const;
212
213
214
215
216

      void add   ( BodyID body, Cell* cell );
      void remove( BodyID body, Cell* cell );

      void enlarge();
217
      
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
      //@}
      //*******************************************************************************************

      //**Member variables*************************************************************************
      /*!\name Member variables */
      //@{
      Cell* cell_;  //!< Linear array of cells representing the three-dimensional hash grid.

      size_t xCellCount_;  //!< Number of cells allocated in x-axis direction.
      size_t yCellCount_;  //!< Number of cells allocated in y-axis direction.
      size_t zCellCount_;  //!< Number of cells allocated in z-axis direction.

      size_t xHashMask_;  //!< Bit-mask required for the hash calculation in x-axis direction (\a xCellCount_ - 1).
      size_t yHashMask_;  //!< Bit-mask required for the hash calculation in y-axis direction (\a yCellCount_ - 1).
      size_t zHashMask_;  //!< Bit-mask required for the hash calculation in z-axis direction (\a zCellCount_ - 1).

      size_t xyCellCount_;   /*!< \brief Number of cells allocated in x-axis direction multiplied by
                                         the number of cells allocated in y-axis direction. */
236
   public: size_t xyzCellCount_;  //!< Total number of allocated cells.
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264

      size_t enlargementThreshold_;  /*!< \brief The enlargement threshold - the moment the number
                                                 of assigned bodies exceeds this threshold, the
                                                 size of this hash grid is increased. */

      real_t cellSpan_;         //!< The grid's cell size (edge length of the underlying cubic grid cells).
      real_t inverseCellSpan_;  //!< The inverse cell size.
                              /*!< Required for replacing floating point divisions with multiplications
                                   during the hash computation. */

      CellVector occupiedCells_;  //!< A grid-global list that keeps track of all body-occupied cells.
                                  /*!< The list is required in order to avoid iterating through all
                                       grid cells whenever all bodies that are stored in the grid
                                       need to be addressed.  */

      size_t bodyCount_;  //!< Number of bodies assigned to this hash grid.

      offset_t stdNeighborOffset_[27];  /*!< \brief Array of offsets to the neighboring cells (valid
                                                    for all the inner cells of the hash grid). */
      //@}
      //*******************************************************************************************
   };
   //**********************************************************************************************

   //**Type definitions****************************************************************************
   //! List for storing all the hash grids that are in use.
   /*! This data structure is used to represent the grid hierarchy. All hash grids are stored in
       ascending order by the size of their cells. */
265
   using GridList = std::list<HashGrid *>;
266
267
268
269
270
271
272
273
274
275
276
277
278
279
   //**********************************************************************************************

public:
   //**Constructor*********************************************************************************
   /*!\name Constructor */
   //@{
//   HashGrids( BodyStorage& bodystorage );
   HashGrids( BodyStorage& globalStorage, BodyStorage& bodystorage, BodyStorage& bodystorageShadowCopies );
   //@}
   //**********************************************************************************************

   //**Destructor**********************************************************************************
   /*!\name Destructor */
   //@{
Sebastian Eibl's avatar
Sebastian Eibl committed
280
   ~HashGrids() override;
281
282
283
284
285
286
287
288
   //@}
   //**********************************************************************************************

   //**Add/remove functions************************************************************************
   /*!\name Add/remove functions */
   //@{
   inline void add   ( BodyID body );
          void remove( BodyID body );
Sebastian Eibl's avatar
Sebastian Eibl committed
289
   inline int getObservedBodyCount() const override { return observedBodyCount_ + static_cast<int> (globalStorage_.size()); }
290
291
292
293
294
295
296
   //@}
   //**********************************************************************************************

   //**Utility functions***************************************************************************
   /*!\name Utility functions */
   //@{
          void clear       ();
Sebastian Eibl's avatar
Sebastian Eibl committed
297
          void reloadBodies() override;
298
299
300
301
   //@}
   //**********************************************************************************************

   //**Implementation of ICCD interface ********************************************************
Sebastian Eibl's avatar
Sebastian Eibl committed
302
303
   PossibleContacts& generatePossibleContacts( WcTimingTree* tt = nullptr ) override;
   void update(WcTimingTree* tt = nullptr);
304
   
305
   bool active() const { return gridActive_; }
306
307
   
   template<typename BodyTuple>
308
   BodyID getClosestBodyIntersectingWithRay(const raytracing::Ray& ray, const AABB& blockAABB,
309
                                            real_t& t, Vec3& n,
310
                                            std::function<bool (const BodyID body)> isBodyVisibleFunc) const;
311
   
312
protected:
313
314
315
316
317
318
319
320
   //**Utility functions***************************************************************************
   /*!\name Utility functions */
   //@{
   template< typename Contacts >
   static inline void collide( BodyID a, BodyID b, Contacts& contacts );
   //@}
   //**********************************************************************************************

321
private:
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
   //**Add functions*******************************************************************************
   /*!\name Add functions */
   //@{
   void addGrid( BodyID body );
   void addList( BodyID body );
   //@}
   //**********************************************************************************************

   //**Utility functions***************************************************************************
   /*!\name Utility functions */
   //@{
   static inline bool powerOfTwo( size_t number );
   //@}
   //**********************************************************************************************

   //**Member variables****************************************************************************
   /*!\name Member variables */
   //@{
   BodyStorage& globalStorage_;    //!< Reference to the global body storage.
   BodyStorage& bodystorage_;    //!< Reference to the central body storage.
   BodyStorage& bodystorageShadowCopies_;    //!< Reference to the body storage containing body shadow copies.
   BodyVector   bodiesToAdd_;    //!< Vector of all bodies to be added to the HHG data structure.
                                 /*!< This vector contains all bodies that were registered with
                                      the coarse collision detector and have not yet been added
                                      to the hierarchical hash grids data structure. */
   BodyVector   nonGridBodies_;  //!< Vector of all unassigned rigid bodies.
                                 /*!< This vector contains all bodies that are not assigned to any
                                      grid in the hierarchy. A body is not assigned to any grid if
                                      the body is infinite in size or if the detection algorithm
                                      based on hierarchical hash grids has not yet been activated
                                      (see \a gridActive_). */
   GridList     gridList_;       //!< List of all grids that form the hierarchy.
                                 /*!< The grids in this list are sorted in ascending order by the
                                      size of their cells. */
   bool         gridActive_;     //!< Grid activation flag.
                                 /*!< This flag indicates whether the detection algorithm based on
                                      hierarchical hash grids is activated. If the simulation only
                                      consists of a very small number of bodies, each body is simply
                                      checked against every other body. This strategy proves to be
                                      faster than involving the far more complex mechanisms of the
                                      hierarchical hash grids. */
   int          observedBodyCount_;  /// number of bodies currently tracked by this hashgrid
   //@}
   //**********************************************************************************************
};
//*************************************************************************************************




//=================================================================================================
//
//  UTILITY FUNCTIONS
//
//=================================================================================================


//*************************************************************************************************
/*!\brief Processes this hash grid for colliding bodies.
 *
 * \param gridBodies Linear array of (handles to) all the bodies stored in this hash grid.
 * \param contacts Contact container for the generated contacts.
 * \return The number of bodies stored in this grid - which is the number of bodies stored in \a gridBodies.
 *
 * This function generates all contacts between all rigid bodies that are assigned to this grid. The
 * contacts are added to the contact container \a contacts. Moreover, a linear array that contains
 * (handles to) all bodies that are stored in this grid is returned in order to being able to check
 * these bodies against other bodies that are stored in grids with larger sized cells.
 */
template< typename Contacts >  // Contact container type
size_t HashGrids::HashGrid::process( BodyID** gridBodies, Contacts& contacts ) const
{
   BodyID* bodies = new BodyID[ bodyCount_ ];
   *gridBodies    = bodies;

   // Iterate through all cells that are occupied by bodies (=> 'occupiedCells_') and ...
   for( typename CellVector::const_iterator cell = occupiedCells_.begin(); cell < occupiedCells_.end(); ++cell )
   {
      BodyVector* cellBodies = (*cell)->bodies_;

      // ... perform pairwise collision checks within each of these cells.
      for( auto aIt = cellBodies->begin(); aIt < cellBodies->end(); ++aIt ) {
         auto end = cellBodies->begin();
         if ((*aIt)->isFixed())
         {
            end = cellBodies->begin() + ((*cell)->lastNonFixedBody_ + 1);
         } else
         {
            end = cellBodies->end();
         }
         for( auto bIt = aIt + 1; bIt < end; ++bIt ) {
            WALBERLA_ASSERT( !((*aIt)->isFixed() && (*bIt)->isFixed()), "collision between two fixed bodies" );
            HashGrids::collide( *aIt, *bIt, contacts );
         }
         *(bodies++) = *aIt;
      }

      // Moreover, check all the bodies that are stored in the currently processed cell against all
      // bodies that are stored in the first half of all directly adjacent cells.
      for( unsigned int i = 0; i < 13; ++i )
      {
         Cell*       nbCell   = (*cell) + (*cell)->neighborOffset_[i];
         BodyVector* nbBodies = nbCell->bodies_;

Sebastian Eibl's avatar
Sebastian Eibl committed
426
         if( nbBodies != nullptr )
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
         {
            for( auto aIt = cellBodies->begin(); aIt < cellBodies->end(); ++aIt ) {
               auto endNeighbour = nbBodies->begin();
               if ((*aIt)->isFixed())
               {
                  endNeighbour = nbBodies->begin() + (nbCell->lastNonFixedBody_ + 1);
               } else
               {
                  endNeighbour = nbBodies->end();
               }
               for( auto bIt = nbBodies->begin(); bIt < endNeighbour; ++bIt ) {
                  WALBERLA_ASSERT( !((*aIt)->isFixed() && (*bIt)->isFixed()), "collision between two fixed bodies" );
                  HashGrids::collide( *aIt, *bIt, contacts );
               }
            }
         }
      }
   }

   return bodyCount_;
}
//*************************************************************************************************


//*************************************************************************************************
/*!\brief Checks all the bodies that are stored in \a bodies for collisions with the bodies that are
 *        stored in this grid.
 *
 * \param bodies Linear array of (handles to) all the bodies that are about to be checked for
 *        collisions with the bodies that are stored in this grid.
 * \param bodyCount The number of bodies that are stored in \a bodies.
 * \param contacts Contact container for the generated contacts.
 * \return void
 *
 * This function generates all contacts between the rigid bodies that are stored in \a bodies and
 * all the rigid bodies that are assigned to this grid. The contacts are added to the contact
 * container \a contacts.
 */
template< typename Contacts >  // Contact container type
void HashGrids::HashGrid::processBodies( BodyID* bodies, size_t bodyCount, Contacts& contacts ) const
{
   // For each body 'a' that is stored in 'bodies' ...
   for( BodyID* aIt = bodies; aIt < bodies + bodyCount; ++aIt )
   {
      // ... calculate the body's cell association (=> "hash()") within this hash grid and ...
      Cell* cell = cell_ + hash( *aIt );

      // ... check 'a' against every body that is stored in this or in any of the directly adjacent
      // cells. Note: one entry in the offset array of a cell is always referring back to the cell
      // itself. As a consequence, a specific cell X and all of its neighbors can be addressed by
      // simply iterating through all entries of X's offset array!
      for( unsigned int i = 0; i < 27; ++i )
      {
         Cell*       nbCell   = cell + cell->neighborOffset_[i];
         BodyVector* nbBodies = nbCell->bodies_;

Sebastian Eibl's avatar
Sebastian Eibl committed
483
         if( nbBodies != nullptr ) {
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
            auto endNeighbour = nbBodies->begin();
            if ((*aIt)->isFixed())
            {
               endNeighbour = nbBodies->begin() + (nbCell->lastNonFixedBody_ + 1);
            } else
            {
               endNeighbour = nbBodies->end();
            }
            for( auto bIt = nbBodies->begin(); bIt != endNeighbour; ++bIt ) {
               WALBERLA_ASSERT( !((*aIt)->isFixed() && (*bIt)->isFixed()), "collision between two fixed bodies" );
               HashGrids::collide( *aIt, *bIt, contacts );
            }
         }
      }
   }
}
//*************************************************************************************************

502
503
/*!\brief Computes closest ray-body intersection of cell with center at point x,y,z and neighboring ones.
 *
504
 * \param blockCell index of cell within block grid.
505
506
 * \param blockAABB AABB of the block this grid corresponds to.
 * \param ray Ray being casted trough grid.
507
508
509
510
 * \param t_closest Distance of closest object from ray origin. Will be updated if closer body found.
 * \param n_closest Normal of intersection point.
 *
 * \return BodyID of intersected body, NULL if no intersection found.
511
512
513
514
515
516
 *
 * Inserts bodies of specified cell into bodiesContainer and additionally considers bodies in neighboring cells
 * in all negative coordinate direction to take bodies in consideration, which protrude from their
 * cell into the intersected cell (and thus, possibly intersect with the ray as well).
 */
template<typename BodyTuple>
Lukas Werner's avatar
Lukas Werner committed
517
518
519
BodyID HashGrids::HashGrid::getBodyIntersectionForBlockCell(const Vector3<int32_t>& blockCell,
                                                            const int8_t cellNormalAxis, const int8_t cellNormalDir,
                                                            const raytracing::Ray& ray,
520
521
                                                            real_t& t_closest, Vec3& n_closest,
                                                            std::function<bool (const BodyID body)> isBodyVisibleFunc) const {
522
523
   real_t t_local;
   Vec3 n_local;
Sebastian Eibl's avatar
Sebastian Eibl committed
524
   BodyID body = nullptr;
525
526
   
   raytracing::IntersectsFunctor intersectsFunc(ray, t_local, n_local);
527
528
529
530
531
   
   // calculate center coordinates of the cell in the block
   real_t x = (real_c(blockCell[0]) + real_t(0.5)) * cellSpan_;
   real_t y = (real_c(blockCell[1]) + real_t(0.5)) * cellSpan_;
   real_t z = (real_c(blockCell[2]) + real_t(0.5)) * cellSpan_;
532
   
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
   // hash of cell in the hashgrid
   size_t cellHash = hashPoint(x, y, z);
   
   const Cell& centerCell = cell_[cellHash];
   
   std::vector<offset_t> relevantNeighborIndices;
   
   if (cellNormalAxis == 0) {
      if (cellNormalDir == -1) {
         relevantNeighborIndices = {2,5,8, 11,14,17, 20,23,26};
      } else {
         relevantNeighborIndices = {0,3,6, 9,12,15, 18,21,24};
      }
   } else if (cellNormalAxis == 1) {
      if (cellNormalDir == -1) {
         relevantNeighborIndices = {6,7,8, 15,16,17, 24,25,26};
      } else {
         relevantNeighborIndices = {0,1,2, 9,10,11, 18,19,20};
      }
   } else if (cellNormalAxis == 2) {
      if (cellNormalDir == -1) {
         relevantNeighborIndices = {18,19,20,21,22,23,24,25,26};
      } else {
         relevantNeighborIndices = {0,1,2,3,4,5,6,7,8};
      }
   } else {
      // cellNormalAxis == BLOCKCELL_NORMAL_INDETERMINATE
      relevantNeighborIndices = {
         0, 1, 2, 3, 4, 5, 6, 7, 8,
         9, 10, 11, 12, 13, 14, 15, 16, 17,
         18, 19, 20, 21, 22, 23, 24, 25, 26
      };
   }
   
567
568
569
570
571
572
573
574
#ifdef HASHGRIDS_RAYTRACING_CHECK_ALL_NEIGHBORS
   relevantNeighborIndices = {
      0, 1, 2, 3, 4, 5, 6, 7, 8,
      9, 10, 11, 12, 13, 14, 15, 16, 17,
      18, 19, 20, 21, 22, 23, 24, 25, 26
   };
#endif
   
Sebastian Eibl's avatar
Sebastian Eibl committed
575
   for (uint_t i = 0; i < relevantNeighborIndices.size(); ++i) {
576
577
      const offset_t neighborIndex = relevantNeighborIndices[i];
      const Cell* nbCell = &centerCell + centerCell.neighborOffset_[neighborIndex];
578
579
      const BodyVector* nbBodies = nbCell->bodies_;
      
Sebastian Eibl's avatar
Sebastian Eibl committed
580
      if (nbBodies != nullptr) {
581
         for (const BodyID& cellBody: *nbBodies) {
Lukas Werner's avatar
Lukas Werner committed
582
583
584
            if (cellBody->isRemote()) {
               continue;
            }
585
586
587
588
            if (!isBodyVisibleFunc(cellBody)) {
               continue;
            }
               
589
            HashGrids::intersectionTestCount++;
590
591
592
            bool intersects = SingleCast<BodyTuple, raytracing::IntersectsFunctor, bool>::execute(cellBody, intersectsFunc);
            if (intersects && t_local < t_closest) {
               body = cellBody;
593
594
               t_closest = t_local;
               n_closest = n_local;
595
            }
596
597
598
         }
      }
   }
599
      
600
   return body;
601
}
602

603
/*!\brief Calculates ray-cell intersections and determines the closest body to the ray origin.
604
605
606
607
 *
 * \param ray Ray getting shot through this grid.
 * \param blockAABB AABB of the block this grid corresponds to.
 * \param t Reference for the distance.
608
 * \param n Reference for the intersection normal.
609
610
611
612
613
614
615
 * \return BodyID of closest body, NULL if none found.
 *
 * This function calculates ray-cell intersections and the closest body in those cells. Also, neighboring
 * cells in all negative coordinate directions are regarded to take bodies in consideration, which
 * protrude from their cell into the intersected cell (and thus, possibly intersect with the ray as well).
 */
template<typename BodyTuple>
Lukas Werner's avatar
Lukas Werner committed
616
BodyID HashGrids::HashGrid::getRayIntersectingBody(const raytracing::Ray& ray, const AABB& blockAABB,
617
618
                                                   real_t& t_closest, Vec3& n_closest,
                                                   std::function<bool (const BodyID body)> isBodyVisibleFunc) const {
Lukas Werner's avatar
Lukas Werner committed
619
   const real_t realMax = std::numeric_limits<real_t>::max();
Lukas Werner's avatar
Lukas Werner committed
620
   
Sebastian Eibl's avatar
Sebastian Eibl committed
621
622
   BodyID body_local = nullptr;
   BodyID body_closest = nullptr;
623
   
624
625
626
627
628
629
   int32_t blockXCellCountMin = int32_c(blockAABB.xMin() * inverseCellSpan_) - 1;
   int32_t blockXCellCountMax = int32_c(std::ceil(blockAABB.xMax() * inverseCellSpan_)) + 1;
   int32_t blockYCellCountMin = int32_c(blockAABB.yMin() * inverseCellSpan_) - 1;
   int32_t blockYCellCountMax = int32_c(std::ceil(blockAABB.yMax() * inverseCellSpan_)) + 1;
   int32_t blockZCellCountMin = int32_c(blockAABB.zMin() * inverseCellSpan_) - 1;
   int32_t blockZCellCountMax = int32_c(std::ceil(blockAABB.zMax() * inverseCellSpan_)) + 1;
630
   
631
   Vec3 firstPoint;
Lukas Werner's avatar
Lukas Werner committed
632
   Vec3 firstPointCenteredInCell;
633
   real_t tRayOriginToGrid = 0;
634
635
   if (blockAABB.contains(ray.getOrigin(), cellSpan_)) {
      firstPoint = ray.getOrigin();
Lukas Werner's avatar
Lukas Werner committed
636
      firstPointCenteredInCell = firstPoint;
637
638
   } else {
      real_t t_start;
639
      Vec3 firstPointNormal;
640
      if (intersects(blockAABB, ray, t_start, cellSpan_, &firstPointNormal)) {
641
         firstPoint = ray.getOrigin() + ray.getDirection()*t_start;
Lukas Werner's avatar
Lukas Werner committed
642
         firstPointCenteredInCell = firstPoint - firstPointNormal * (cellSpan_/real_t(2));
643
         tRayOriginToGrid = (ray.getOrigin() - firstPoint).length();
644
      } else {
Sebastian Eibl's avatar
Sebastian Eibl committed
645
         return nullptr;
646
647
648
      }
   }
   
Lukas Werner's avatar
Lukas Werner committed
649
650
651
   Vector3<int32_t> firstCell(int32_c(std::floor(firstPointCenteredInCell[0]*inverseCellSpan_)),
                              int32_c(std::floor(firstPointCenteredInCell[1]*inverseCellSpan_)),
                              int32_c(std::floor(firstPointCenteredInCell[2]*inverseCellSpan_)));
652
653
654
655
656
   
   const int8_t stepX = ray.xDir() >= 0 ? 1 : -1;
   const int8_t stepY = ray.yDir() >= 0 ? 1 : -1;
   const int8_t stepZ = ray.zDir() >= 0 ? 1 : -1;
   
657
658
659
   Vec3 nearPoint((stepX >= 0) ? real_c(firstCell[0]+1)*cellSpan_-firstPoint[0] : firstPoint[0]-real_c(firstCell[0])*cellSpan_,
                  (stepY >= 0) ? real_c(firstCell[1]+1)*cellSpan_-firstPoint[1] : firstPoint[1]-real_c(firstCell[1])*cellSpan_,
                  (stepZ >= 0) ? real_c(firstCell[2]+1)*cellSpan_-firstPoint[2] : firstPoint[2]-real_c(firstCell[2])*cellSpan_);
660
   
661
   // tMax: distance along the ray to the next cell change in the axis direction
Lukas Werner's avatar
Lukas Werner committed
662
663
664
   real_t tMaxX = (!realIsEqual(ray.xDir(), 0)) ? std::abs(nearPoint[0]*ray.xInvDir()) : realMax;
   real_t tMaxY = (!realIsEqual(ray.yDir(), 0)) ? std::abs(nearPoint[1]*ray.yInvDir()) : realMax;
   real_t tMaxZ = (!realIsEqual(ray.zDir(), 0)) ? std::abs(nearPoint[2]*ray.zInvDir()) : realMax;
665
   
666
   // tDelta: how far along the ray must be moved to encounter a new cell in the specified axis direction
Lukas Werner's avatar
Lukas Werner committed
667
668
669
   real_t tDeltaX = (!realIsEqual(ray.xDir(), 0)) ? std::abs(cellSpan_*ray.xInvDir()) : realMax;
   real_t tDeltaY = (!realIsEqual(ray.yDir(), 0)) ? std::abs(cellSpan_*ray.yInvDir()) : realMax;
   real_t tDeltaZ = (!realIsEqual(ray.zDir(), 0)) ? std::abs(cellSpan_*ray.zInvDir()) : realMax;
670
671
672
673
674
675
676
677
678
   
   Vector3<int32_t> currentCell = firstCell;
   
   // First cell needs extra treatment, as it might lay out of the blocks upper bounds
   // due to the nature of how it is calculated: If the first point lies on a upper border
   // it maps to the cell "above" the grid.
   if (currentCell[0] < blockXCellCountMax &&
       currentCell[1] < blockYCellCountMax &&
       currentCell[2] < blockZCellCountMax) {
679
      body_local = getBodyIntersectionForBlockCell<BodyTuple>(currentCell, BLOCKCELL_NORMAL_INDETERMINATE, 0,
680
681
                                                              ray, t_closest, n_closest,
                                                              isBodyVisibleFunc);
Sebastian Eibl's avatar
Sebastian Eibl committed
682
      if (body_local != nullptr) {
683
         body_closest = body_local;
684
685
686
      }
   }
   
687
688
689
   int8_t blockCellNormalAxis;
   int8_t blockCellNormalDir;
   
690
691
692
   while (true) {
      if (tMaxX < tMaxY) {
         if (tMaxX < tMaxZ) {
693
694
695
696
697
#if !defined(HASHGRIDS_DISABLE_EARLY_CUTOFF)
            if (tRayOriginToGrid+tMaxX-tDeltaX > t_closest) {
               break;
            }
#endif
698
699
            tMaxX += tDeltaX;
            currentCell[0] += stepX;
700
            blockCellNormalAxis = 0;
Lukas Werner's avatar
Lukas Werner committed
701
            blockCellNormalDir = int8_c(-stepX);
702
            if (currentCell[0] >= blockXCellCountMax || currentCell[0] < blockXCellCountMin) {
703
704
705
               break;
            }
         } else {
706
707
708
709
710
#if !defined(HASHGRIDS_DISABLE_EARLY_CUTOFF)
            if (tRayOriginToGrid+tMaxZ-tDeltaZ > t_closest) {
               break;
            }
#endif
711
712
            tMaxZ += tDeltaZ;
            currentCell[2] += stepZ;
713
            blockCellNormalAxis = 2;
Lukas Werner's avatar
Lukas Werner committed
714
            blockCellNormalDir = int8_c(-stepZ);
715
            if (currentCell[2] >= blockZCellCountMax || currentCell[2] < blockZCellCountMin) {
716
717
718
719
720
               break;
            }
         }
      } else {
         if (tMaxY < tMaxZ) {
721
722
723
724
725
#if !defined(HASHGRIDS_DISABLE_EARLY_CUTOFF)
            if (tRayOriginToGrid+tMaxY-tDeltaY > t_closest) {
               break;
            }
#endif
726
727
            tMaxY += tDeltaY;
            currentCell[1] += stepY;
728
            blockCellNormalAxis = 1;
Lukas Werner's avatar
Lukas Werner committed
729
            blockCellNormalDir = int8_c(-stepY);
730
            if (currentCell[1] >= blockYCellCountMax || currentCell[1] < blockYCellCountMin) {
731
732
733
               break;
            }
         } else {
734
735
736
737
738
#if !defined(HASHGRIDS_DISABLE_EARLY_CUTOFF)
            if (tRayOriginToGrid+tMaxZ-tDeltaZ > t_closest) {
               break;
            }
#endif
739
740
            tMaxZ += tDeltaZ;
            currentCell[2] += stepZ;
741
            blockCellNormalAxis = 2;
Lukas Werner's avatar
Lukas Werner committed
742
            blockCellNormalDir = int8_c(-stepZ);
743
            if (currentCell[2] >= blockZCellCountMax || currentCell[2] < blockZCellCountMin) {
744
745
746
               break;
            }
         }
747
      }
748
      
749
      body_local = getBodyIntersectionForBlockCell<BodyTuple>(currentCell, blockCellNormalAxis, blockCellNormalDir,
750
751
                                                              ray, t_closest, n_closest,
                                                              isBodyVisibleFunc);
Sebastian Eibl's avatar
Sebastian Eibl committed
752
      if (body_local != nullptr) {
753
         body_closest = body_local;
754
755
756
757
758
759
760
761
      }
   }
   
   return body_closest;
}

/*!\brief Determines the closest intersecting body in the underlying hash grids.
 *
762
763
 * \param ray Ray getting shot through those grids.
 * \param blockAABB AABB of the block the grids correspond to.
764
 * \param t Reference for the distance.
765
 * \param n Reference for the intersection normal.
766
767
768
 * \return BodyID of closest body, NULL if none found.
 */
template<typename BodyTuple>
Lukas Werner's avatar
Lukas Werner committed
769
BodyID HashGrids::getClosestBodyIntersectingWithRay(const raytracing::Ray& ray, const AABB& blockAABB,
770
                                                    real_t& t, Vec3& n,
771
                                                    std::function<bool (const BodyID body)> isBodyVisibleFunc) const {
Lukas Werner's avatar
Lukas Werner committed
772
   const real_t realMax = std::numeric_limits<real_t>::max();
773

Sebastian Eibl's avatar
Sebastian Eibl committed
774
   BodyID body_closest = nullptr;
Lukas Werner's avatar
Lukas Werner committed
775
   real_t t_closest = realMax;
776
777
   Vec3 n_closest;
   
778
   BodyID body_local;
Lukas Werner's avatar
Lukas Werner committed
779
   real_t t_local = realMax;
780
781
782
783
784
   Vec3 n_local;
   
   raytracing::IntersectsFunctor intersectsFunc(ray, t_local, n_local);
   for(auto body: nonGridBodies_) {
      bool intersects = SingleCast<BodyTuple, raytracing::IntersectsFunctor, bool>::execute(body, intersectsFunc);
785
      if (intersects && t_local < t_closest) {
786
787
788
789
790
791
         body_closest = body;
         t_closest = t_local;
         n_closest = n_local;
      }
   }
   
Lukas Werner's avatar
Lukas Werner committed
792
   for(auto grid: gridList_) {
793
      body_local = grid->getRayIntersectingBody<BodyTuple>(ray, blockAABB, t_closest, n_closest, isBodyVisibleFunc);
Sebastian Eibl's avatar
Sebastian Eibl committed
794
      if (body_local != nullptr){
795
796
         body_closest = body_local;
      }
Lukas Werner's avatar
Lukas Werner committed
797
   }
798
   
799
   t = t_closest;
800
801
   n = n_closest;

802
803
   return body_closest;
}
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823


//=================================================================================================
//
//  UTILITY FUNCTIONS
//
//=================================================================================================

//*************************************************************************************************
/*!\brief Checks whether two bodies are colliding or not, and if so generates the related points
 *        of contact.
 *
 * \param a The first body.
 * \param b The second body.
 * \param contacts Contact container for the generated contacts.
 * \return void
 */
template< typename Contacts >  // Contact container type
void HashGrids::collide( BodyID a, BodyID b, Contacts& contacts )
{
824
825
826
827
   //make sure to always check in the correct order (a<b)
   if (a->getSystemID() > b->getSystemID())
      std::swap(a, b);

828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
   if( ( !a->hasInfiniteMass() || !b->hasInfiniteMass() ) &&        // Ignoring contacts between two fixed bodies
       ( a->getAABB().intersects( b->getAABB() ) ) )  // Testing for overlapping bounding boxes
   {
      //FineDetector::collide( a, b, contacts );
      contacts.push_back( std::make_pair(a, b) );
   }
}
//*************************************************************************************************


}  // namespace ccd

}  // namespace pe

}  // namespace walberla