CoCalc -- bvh_collider.cpp

GitHub Repository: godotengine/godot
Path: blob/master/thirdparty/embree/kernels/bvh/bvh_collider.cpp
⁹⁹⁰⁶ views
1
// Copyright 2009-2021 Intel Corporation
2
// SPDX-License-Identifier: Apache-2.0
3

4
#include "bvh_collider.h"
5

6
#include "../geometry/triangle_triangle_intersector.h"
7
#include "../../common/algorithms/parallel_for.h"
8

9
namespace embree
10
{ 
11
  namespace isa
12
  {
13
#define CSTAT(x)
14

15
    size_t parallel_depth_threshold = 3;
16
    CSTAT(std::atomic<size_t> bvh_collide_traversal_steps(0));
17
    CSTAT(std::atomic<size_t> bvh_collide_leaf_pairs(0));
18
    CSTAT(std::atomic<size_t> bvh_collide_leaf_iterations(0));
19
    CSTAT(std::atomic<size_t> bvh_collide_prim_intersections1(0));
20
    CSTAT(std::atomic<size_t> bvh_collide_prim_intersections2(0));
21
    CSTAT(std::atomic<size_t> bvh_collide_prim_intersections3(0));
22
    CSTAT(std::atomic<size_t> bvh_collide_prim_intersections4(0));
23
    CSTAT(std::atomic<size_t> bvh_collide_prim_intersections5(0));
24
    CSTAT(std::atomic<size_t> bvh_collide_prim_intersections(0));
25

26
    struct Collision
27
    {
28
      __forceinline Collision() {}
29

30
      __forceinline Collision (unsigned geomID0, unsigned primID0, unsigned geomID1, unsigned primID1)
31
        : geomID0(geomID0), primID0(primID0), geomID1(geomID1), primID1(primID1) {}
32

33
      unsigned geomID0;
34
      unsigned primID0;
35
      unsigned geomID1;
36
      unsigned primID1;
37
    };
38
    
39
    template<int N>
40
    __forceinline size_t overlap(const BBox3fa& box0, const typename BVHN<N>::AABBNode& node1)
41
    {
42
      const vfloat<N> lower_x = max(vfloat<N>(box0.lower.x),node1.lower_x);
43
      const vfloat<N> lower_y = max(vfloat<N>(box0.lower.y),node1.lower_y);
44
      const vfloat<N> lower_z = max(vfloat<N>(box0.lower.z),node1.lower_z);
45
      const vfloat<N> upper_x = min(vfloat<N>(box0.upper.x),node1.upper_x);
46
      const vfloat<N> upper_y = min(vfloat<N>(box0.upper.y),node1.upper_y);
47
      const vfloat<N> upper_z = min(vfloat<N>(box0.upper.z),node1.upper_z);
48
      return movemask((lower_x <= upper_x) & (lower_y <= upper_y) & (lower_z <= upper_z));
49
    }
50

51
    template<int N>
52
    __forceinline size_t overlap(const BBox3fa& box0, const BBox<Vec3<vfloat<N>>>& box1)
53
    {
54
      const vfloat<N> lower_x = max(vfloat<N>(box0.lower.x),box1.lower.x);
55
      const vfloat<N> lower_y = max(vfloat<N>(box0.lower.y),box1.lower.y);
56
      const vfloat<N> lower_z = max(vfloat<N>(box0.lower.z),box1.lower.z);
57
      const vfloat<N> upper_x = min(vfloat<N>(box0.upper.x),box1.upper.x);
58
      const vfloat<N> upper_y = min(vfloat<N>(box0.upper.y),box1.upper.y);
59
      const vfloat<N> upper_z = min(vfloat<N>(box0.upper.z),box1.upper.z);
60
      return movemask((lower_x <= upper_x) & (lower_y <= upper_y) & (lower_z <= upper_z));
61
    }
62

63
    template<int N>
64
    __forceinline size_t overlap(const BBox<Vec3<vfloat<N>>>& box0, size_t i, const BBox<Vec3<vfloat<N>>>& box1)
65
    {
66
      const vfloat<N> lower_x = max(vfloat<N>(box0.lower.x[i]),box1.lower.x);
67
      const vfloat<N> lower_y = max(vfloat<N>(box0.lower.y[i]),box1.lower.y);
68
      const vfloat<N> lower_z = max(vfloat<N>(box0.lower.z[i]),box1.lower.z);
69
      const vfloat<N> upper_x = min(vfloat<N>(box0.upper.x[i]),box1.upper.x);
70
      const vfloat<N> upper_y = min(vfloat<N>(box0.upper.y[i]),box1.upper.y);
71
      const vfloat<N> upper_z = min(vfloat<N>(box0.upper.z[i]),box1.upper.z);
72
      return movemask((lower_x <= upper_x) & (lower_y <= upper_y) & (lower_z <= upper_z));
73
    }
74

75
    bool intersect_triangle_triangle (Scene* scene0, unsigned geomID0, unsigned primID0, Scene* scene1, unsigned geomID1, unsigned primID1)
76
    {
77
      CSTAT(bvh_collide_prim_intersections1++);
78
      const TriangleMesh* mesh0 = scene0->get<TriangleMesh>(geomID0);
79
      const TriangleMesh* mesh1 = scene1->get<TriangleMesh>(geomID1);
80
      const TriangleMesh::Triangle& tri0 = mesh0->triangle(primID0);
81
      const TriangleMesh::Triangle& tri1 = mesh1->triangle(primID1);
82
      
83
      /* special culling for scene intersection with itself */
84
      if (scene0 == scene1 && geomID0 == geomID1)
85
      {
86
        /* ignore self intersections */
87
        if (primID0 == primID1)
88
          return false;
89
      }
90
      CSTAT(bvh_collide_prim_intersections2++);
91
      
92
      if (scene0 == scene1 && geomID0 == geomID1)
93
      {
94
        /* ignore intersection with topological neighbors */
95
        const vint4 t0(tri0.v[0],tri0.v[1],tri0.v[2],tri0.v[2]);
96
        if (any(vint4(tri1.v[0]) == t0)) return false;
97
        if (any(vint4(tri1.v[1]) == t0)) return false;
98
        if (any(vint4(tri1.v[2]) == t0)) return false;
99
      }
100
      CSTAT(bvh_collide_prim_intersections3++);
101
      
102
      const Vec3fa a0 = mesh0->vertex(tri0.v[0]);
103
      const Vec3fa a1 = mesh0->vertex(tri0.v[1]);
104
      const Vec3fa a2 = mesh0->vertex(tri0.v[2]);
105
      const Vec3fa b0 = mesh1->vertex(tri1.v[0]);
106
      const Vec3fa b1 = mesh1->vertex(tri1.v[1]);
107
      const Vec3fa b2 = mesh1->vertex(tri1.v[2]);
108
      
109
      return TriangleTriangleIntersector::intersect_triangle_triangle(a0,a1,a2,b0,b1,b2);
110
    }
111
    
112
    template<int N>
113
    __forceinline void BVHNColliderUserGeom<N>::processLeaf(NodeRef node0, NodeRef node1)
114
    {
115
      Collision collisions[16];
116
      size_t num_collisions = 0;
117

118
      size_t N0; Object* leaf0 = (Object*) node0.leaf(N0);
119
      size_t N1; Object* leaf1 = (Object*) node1.leaf(N1);
120
      for (size_t i=0; i<N0; i++) {
121
        for (size_t j=0; j<N1; j++) {
122
          const unsigned geomID0 = leaf0[i].geomID();
123
          const unsigned primID0 = leaf0[i].primID();
124
          const unsigned geomID1 = leaf1[j].geomID();
125
          const unsigned primID1 = leaf1[j].primID();
126
          if (this->scene0 == this->scene1 && geomID0 == geomID1 && primID0 == primID1) continue;
127
          collisions[num_collisions++] = Collision(geomID0,primID0,geomID1,primID1);
128
          if (num_collisions == 16) {
129
            this->callback(this->userPtr,(RTCCollision*)&collisions,num_collisions);
130
            num_collisions = 0;
131
          }
132
        }
133
      }
134
      if (num_collisions)
135
        this->callback(this->userPtr,(RTCCollision*)&collisions,num_collisions);
136
    }
137

138
    template<int N>
139
    void BVHNCollider<N>::collide_recurse(NodeRef ref0, const BBox3fa& bounds0, NodeRef ref1, const BBox3fa& bounds1, size_t depth0, size_t depth1)
140
    {
141
      CSTAT(bvh_collide_traversal_steps++);
142
      if (unlikely(ref0.isLeaf())) {
143
        if (unlikely(ref1.isLeaf())) {
144
          CSTAT(bvh_collide_leaf_pairs++);
145
          processLeaf(ref0,ref1);
146
          return;
147
        } else goto recurse_node1;
148
        
149
      } else {
150
        if (unlikely(ref1.isLeaf())) {
151
          goto recurse_node0;
152
        } else {
153
          if (area(bounds0) > area(bounds1)) {
154
            goto recurse_node0;
155
          }
156
          else {
157
            goto recurse_node1;
158
          }
159
        }
160
      }
161

162
      {
163
      recurse_node0:
164
        AABBNode* node0 = ref0.getAABBNode();
165
        size_t mask = overlap<N>(bounds1,*node0);
166
        //for (size_t m=mask, i=bsf(m); m!=0; m=btc(m,i), i=bsf(m)) {
167
        //for (size_t i=0; i<N; i++) {
168
#if 0
169
        if (depth0 < parallel_depth_threshold) 
170
        {
171
          parallel_for(size_t(N), [&] ( size_t i ) {
172
              if (mask & ( 1 << i)) {
173
                BVHN<N>::prefetch(node0->child(i),BVH_FLAG_ALIGNED_NODE);
174
                collide_recurse(node0->child(i),node0->bounds(i),ref1,bounds1,depth0+1,depth1);
175
              }
176
            });
177
        } 
178
        else
179
#endif
180
        {
181
          for (size_t m=mask, i=bsf(m); m!=0; m=btc(m,i), i=bsf(m)) {
182
            BVHN<N>::prefetch(node0->child(i),BVH_FLAG_ALIGNED_NODE);
183
            collide_recurse(node0->child(i),node0->bounds(i),ref1,bounds1,depth0+1,depth1);
184
          }
185
        }
186
        return;
187
      }
188
      
189
      {
190
      recurse_node1:
191
        AABBNode* node1 = ref1.getAABBNode();
192
        size_t mask = overlap<N>(bounds0,*node1);
193
        //for (size_t m=mask, i=bsf(m); m!=0; m=btc(m,i), i=bsf(m)) {
194
        //for (size_t i=0; i<N; i++) {
195
#if 0
196
        if (depth1 < parallel_depth_threshold) 
197
        {
198
          parallel_for(size_t(N), [&] ( size_t i ) {
199
              if (mask & ( 1 << i)) {
200
                BVHN<N>::prefetch(node1->child(i),BVH_FLAG_ALIGNED_NODE);
201
                collide_recurse(ref0,bounds0,node1->child(i),node1->bounds(i),depth0,depth1+1);
202
              }
203
            });
204
        }
205
        else
206
#endif
207
        {
208
          for (size_t m=mask, i=bsf(m); m!=0; m=btc(m,i), i=bsf(m)) {
209
            BVHN<N>::prefetch(node1->child(i),BVH_FLAG_ALIGNED_NODE);
210
            collide_recurse(ref0,bounds0,node1->child(i),node1->bounds(i),depth0,depth1+1);
211
          }
212
        }
213
        return;
214
      }
215
    }
216

217
    template<int N>
218
    void BVHNCollider<N>::split(const CollideJob& job, jobvector& jobs)
219
    {
220
      if (unlikely(job.ref0.isLeaf())) {
221
        if (unlikely(job.ref1.isLeaf())) {
222
          jobs.push_back(job);
223
          return;
224
        } else goto recurse_node1;
225
      } else {
226
        if (unlikely(job.ref1.isLeaf())) {
227
          goto recurse_node0;
228
        } else {
229
          if (area(job.bounds0) > area(job.bounds1)) {
230
            goto recurse_node0;
231
          }
232
          else {
233
            goto recurse_node1;
234
          }
235
        }
236
      }
237
      
238
      {
239
      recurse_node0:
240
        const AABBNode* node0 = job.ref0.getAABBNode();
241
        size_t mask = overlap<N>(job.bounds1,*node0);
242
        for (size_t m=mask, i=bsf(m); m!=0; m=btc(m,i), i=bsf(m)) {
243
          jobs.push_back(CollideJob(node0->child(i),node0->bounds(i),job.depth0+1,job.ref1,job.bounds1,job.depth1));
244
        }
245
        return;
246
      }
247
      
248
      {
249
      recurse_node1:
250
        const AABBNode* node1 = job.ref1.getAABBNode();
251
        size_t mask = overlap<N>(job.bounds0,*node1);
252
        for (size_t m=mask, i=bsf(m); m!=0; m=btc(m,i), i=bsf(m)) {
253
          jobs.push_back(CollideJob(job.ref0,job.bounds0,job.depth0,node1->child(i),node1->bounds(i),job.depth1+1));
254
        }
255
        return;
256
      }
257
    }
258
    
259
    template<int N>
260
    void BVHNCollider<N>::collide_recurse_entry(NodeRef ref0, const BBox3fa& bounds0, NodeRef ref1, const BBox3fa& bounds1)
261
    {
262
      CSTAT(bvh_collide_traversal_steps = 0);
263
      CSTAT(bvh_collide_leaf_pairs = 0);
264
      CSTAT(bvh_collide_leaf_iterations = 0);
265
      CSTAT(bvh_collide_prim_intersections1 = 0);
266
      CSTAT(bvh_collide_prim_intersections2 = 0);
267
      CSTAT(bvh_collide_prim_intersections3 = 0);
268
      CSTAT(bvh_collide_prim_intersections4 = 0);
269
      CSTAT(bvh_collide_prim_intersections5 = 0);
270
      CSTAT(bvh_collide_prim_intersections = 0);
271
#if 0
272
      collide_recurse(ref0,bounds0,ref1,bounds1,0,0);
273
#else
274
      const int M = 2048;
275
      jobvector jobs[2];
276
      jobs[0].reserve(M);
277
      jobs[1].reserve(M);
278
      jobs[0].push_back(CollideJob(ref0,bounds0,0,ref1,bounds1,0));
279
      int source = 0;
280
      int target = 1;
281

282
      /* try to split job until job list is full */
283
      while (jobs[source].size()+8 <= M)
284
      {
285
        for (size_t i=0; i<jobs[source].size(); i++)
286
        {
287
          const CollideJob& job = jobs[source][i];
288
          size_t remaining = jobs[source].size()-i;
289
          if (jobs[target].size()+remaining+8 > M) {
290
            jobs[target].push_back(job);
291
          } else {
292
            split(job,jobs[target]);
293
          }
294
        }
295

296
        /* stop splitting jobs if we reached only leaves and cannot make progress anymore */
297
        if (jobs[target].size() == jobs[source].size())
298
          break;
299

300
        jobs[source].resize(0);
301
        std::swap(source,target);
302
      }
303

304
      /* parallel processing of all jobs */
305
      parallel_for(size_t(jobs[source].size()), [&] ( size_t i ) {
306
          CollideJob& j = jobs[source][i];
307
          collide_recurse(j.ref0,j.bounds0,j.ref1,j.bounds1,j.depth0,j.depth1);
308
        });
309
      
310
      
311
#endif
312
      CSTAT(PRINT(bvh_collide_traversal_steps));
313
      CSTAT(PRINT(bvh_collide_leaf_pairs));
314
      CSTAT(PRINT(bvh_collide_leaf_iterations));
315
      CSTAT(PRINT(bvh_collide_prim_intersections1));
316
      CSTAT(PRINT(bvh_collide_prim_intersections2));
317
      CSTAT(PRINT(bvh_collide_prim_intersections3));
318
      CSTAT(PRINT(bvh_collide_prim_intersections4));
319
      CSTAT(PRINT(bvh_collide_prim_intersections5));
320
      CSTAT(PRINT(bvh_collide_prim_intersections));
321
    }
322
   
323
    template<int N>
324
    void BVHNColliderUserGeom<N>::collide(BVH* __restrict__ bvh0, BVH* __restrict__ bvh1, RTCCollideFunc callback, void* userPtr)
325
    { 
326
      BVHNColliderUserGeom<N>(bvh0->scene,bvh1->scene,callback,userPtr).
327
        collide_recurse_entry(bvh0->root,bvh0->bounds.bounds(),bvh1->root,bvh1->bounds.bounds());
328
    }
329

330
#if defined (EMBREE_LOWEST_ISA)
331
    struct collision_regression_test : public RegressionTest
332
    {
333
      collision_regression_test(const char* name) : RegressionTest(name) {
334
        registerRegressionTest(this);
335
      }
336
    
337
      bool run ()
338
      {
339
        bool passed = true;
340
        passed &= TriangleTriangleIntersector::intersect_triangle_triangle (Vec3fa(-0.008815f, 0.041848f, -2.49875e-06f), Vec3fa(-0.008276f, 0.053318f, -2.49875e-06f), Vec3fa(0.003023f, 0.048969f, -2.49875e-06f),
341
                                                                            Vec3fa(0.00245f, 0.037612f, -2.49875e-06f), Vec3fa(0.01434f, 0.042634f, -2.49875e-06f), Vec3fa(0.013499f, 0.031309f, -2.49875e-06f)) == false;
342
        passed &= TriangleTriangleIntersector::intersect_triangle_triangle (Vec3fa(0,0,0),Vec3fa(1,0,0),Vec3fa(0,1,0), Vec3fa(0,0,0),Vec3fa(1,0,0),Vec3fa(0,1,0)) == true;
343
        passed &= TriangleTriangleIntersector::intersect_triangle_triangle (Vec3fa(0,0,0),Vec3fa(1,0,0),Vec3fa(0,1,0), Vec3fa(0,0,1),Vec3fa(1,0,1),Vec3fa(0,1,1)) == false;
344
        passed &= TriangleTriangleIntersector::intersect_triangle_triangle (Vec3fa(0,0,0),Vec3fa(1,0,0),Vec3fa(0,1,0), Vec3fa(0,0,1),Vec3fa(1,0,0),Vec3fa(0,1,0)) == true;
345
        passed &= TriangleTriangleIntersector::intersect_triangle_triangle (Vec3fa(0,0,0),Vec3fa(1,0,0),Vec3fa(0,1,0), Vec3fa(0,0,0),Vec3fa(1,0,1),Vec3fa(0,1,1)) == true;
346
        passed &= TriangleTriangleIntersector::intersect_triangle_triangle (Vec3fa(0,0,0),Vec3fa(1,0,0),Vec3fa(0,1,0), Vec3fa(0.1f,0.1f,0),Vec3fa(1,0,1),Vec3fa(0,1,1)) == true;
347
        passed &= TriangleTriangleIntersector::intersect_triangle_triangle (Vec3fa(0,0,0),Vec3fa(1,0,0),Vec3fa(0,1,0), Vec3fa(0.1f,0.1f,-0.1f),Vec3fa(1,0,1),Vec3fa(0,1,1)) == true;
348
        passed &= TriangleTriangleIntersector::intersect_triangle_triangle (Vec3fa(0,0,0),Vec3fa(1,0,0),Vec3fa(0,1,0), Vec3fa(0,0,0),Vec3fa(1,0,0),Vec3fa(0,1,0)) == true;
349
        passed &= TriangleTriangleIntersector::intersect_triangle_triangle (Vec3fa(0,0,0),Vec3fa(1,0,0),Vec3fa(0,1,0), Vec3fa(0,0,0),Vec3fa(0.5f,0,0),Vec3fa(0,0.5f,0)) == true;
350
        passed &= TriangleTriangleIntersector::intersect_triangle_triangle (Vec3fa(0,0,0),Vec3fa(1,0,0),Vec3fa(0,1,0), Vec3fa(0.1f,0.1f,0),Vec3fa(0.5f,0,0),Vec3fa(0,0.5f,0)) == true;
351
        passed &= TriangleTriangleIntersector::intersect_triangle_triangle (Vec3fa(0,0,0),Vec3fa(1,0,0),Vec3fa(0,1,0), Vec3fa(0.1f,0.1f,0),Vec3fa(0.5f,0.1f,0),Vec3fa(0.1f,0.5f,0)) == true;
352
        passed &= TriangleTriangleIntersector::intersect_triangle_triangle (Vec3fa(0,0,0),Vec3fa(1,0,0),Vec3fa(0,1,0), Vec3fa(0.1f,-0.1f,0),Vec3fa(0.5f,0.1f,0),Vec3fa(0.1f,0.5f,0)) == true;
353
        passed &= TriangleTriangleIntersector::intersect_triangle_triangle (Vec3fa(0,0,0),Vec3fa(1,0,0),Vec3fa(0,1,0), Vec3fa(-0.1f,0.1f,0),Vec3fa(0.5f,0.1f,0),Vec3fa(0.1f,0.5f,0)) == true;
354
        passed &= TriangleTriangleIntersector::intersect_triangle_triangle (Vec3fa(0,0,0),Vec3fa(1,0,0),Vec3fa(0,1,0), 
355
                                               Vec3fa(-1,1,0) + Vec3fa(0,0,0),Vec3fa(-1,1,0) + Vec3fa(0.1f,0,0),Vec3fa(-1,1,0) + Vec3fa(0,0.1f,0)) == false;
356
        passed &= TriangleTriangleIntersector::intersect_triangle_triangle (Vec3fa(0,0,0),Vec3fa(1,0,0),Vec3fa(0,1,0), 
357
                                               Vec3fa( 2,0.5f,0) + Vec3fa(0,0,0),Vec3fa( 2,0.5f,0) + Vec3fa(0.1f,0,0),Vec3fa( 2,0.5f,0) + Vec3fa(0,0.1f,0)) == false;
358
        passed &= TriangleTriangleIntersector::intersect_triangle_triangle (Vec3fa(0,0,0),Vec3fa(1,0,0),Vec3fa(0,1,0), 
359
                                               Vec3fa(0.5f,-2.0f,0) + Vec3fa(0,0,0),Vec3fa(0.5f,-2.0f,0) + Vec3fa(0.1f,0,0),Vec3fa(0.5f,-2.0f,0) + Vec3fa(0,0.1f,0)) == false;
360
        return passed;
361
      }
362
    };
363

364
    collision_regression_test collision_regression("collision_regression_test");
365
#endif
366

367
    ////////////////////////////////////////////////////////////////////////////////
368
    /// Collider Definitions
369
    ////////////////////////////////////////////////////////////////////////////////
370

371
    DEFINE_COLLIDER(BVH4ColliderUserGeom,BVHNColliderUserGeom<4>);
372

373
#if defined(__AVX__)
374
    DEFINE_COLLIDER(BVH8ColliderUserGeom,BVHNColliderUserGeom<8>);
375
#endif
376
  }
377
}
378

379
Product

Resources

Company
