vg_bvh.h

   1 #pragma once
   2 #include "vg_mem.h"
   3 #include "vg_m.h"
   4 #include "vg_lines.h"
   5
   6 /*
   7  * Usage:
   8  *
   9  * create a bh_system with functions filled out for expand, centroid, and swap.
  10  * optionally include item_debug and cast_ray functions if needed, otherwise,
  11  *   set them to null
  12  *
  13  * create a bh_tree struct with:
  14  *   user: a pointer back the base of the data you are ordering
  15  *   system: the system we created above which will deal with the data
  16  *
  17  * call bh_create( bh_tree *bh, u32 item_count )
  18  * static int bh_ray( bh_tree *bh, u32 inode, v3f co, v3f dir, ray_hit *hit )
  19  * static int bh_select( bh_tree *bh, boxf box, u32 *buffer, int len )
  20  */
  21
  22 typedef struct bh_node bh_node;
  23 typedef struct bh_tree bh_tree;
  24 typedef struct bh_system bh_system;
  25
  26 typedef struct ray_hit ray_hit;
  27 struct ray_hit{
  28    float dist;
  29    u32 *tri;
  30    v3f pos, normal;
  31 };
  32
  33 struct bh_tree{
  34    u32 node_count;
  35
  36    bh_system *system;
  37    void *user;
  38    u32 max_per_leaf;
  39
  40    struct bh_node{
  41       boxf bbx;
  42
  43       /* if il is 0, this is a leaf */
  44       int il, count;
  45       union{ int ir, start; };
  46    }
  47    nodes[];
  48 };
  49
  50 struct bh_system{
  51    void  (*expand_bound)( void *user, boxf bound, u32 item_index );
  52    float (*item_centroid)( void *user, u32 item_index, int axis );
  53    void  (*item_closest)( void *user, u32 item_index, v3f point, v3f closest );
  54    void  (*item_swap)( void *user, u32 ia, u32 ib );
  55
  56    /*
  57     * Optional:
  58     *   item_debug   - draw this item quickly usually with lines
  59     *   cast_ray     - shoot a ray against the object, if this is not set,
  60     *                  raycasts will simply return the hit on the bvh node
  61     */
  62
  63    void  (*item_debug)( void *user, u32 item_index );
  64    int   (*cast_ray)( void *user, u32 index, v3f co, v3f dir, ray_hit *hit );
  65 };
  66
  67 #define BVH_FIXED_MODE
  68 #ifdef BVH_FIXED_MODE
  69 static f32 shape_bvh_centroid( void *user, u32 item_index, int axis );
  70 static void shape_bvh_swap( void *user, u32 ia, u32 ib );
  71 static void shape_bvh_expand_bound( void *user, boxf bound, u32 item_index );
  72 #endif
  73
  74 static void bh_update_bounds( bh_tree *bh, u32 inode ){
  75    bh_node *node = &bh->nodes[ inode ];
  76
  77    box_init_inf( node->bbx );
  78    for( u32 i=0; i<node->count; i++ ){
  79       u32 idx = node->start+i;
  80 #ifdef BVH_FIXED_MODE
  81       shape_bvh_expand_bound( bh->user, node->bbx, idx );
  82 #else
  83       bh->system->expand_bound( bh->user, node->bbx, idx );
  84 #endif
  85    }
  86 }
  87
  88 static void bh_subdivide( bh_tree *bh, u32 inode ){
  89    bh_node *node = &bh->nodes[ inode ];
  90
  91    if( node->count <= bh->max_per_leaf )
  92       return;
  93
  94    v3f extent;
  95    v3_sub( node->bbx[1], node->bbx[0], extent );
  96
  97    int axis = 0;
  98    if( extent[1] > extent[0] ) axis = 1;
  99    if( extent[2] > extent[axis] ) axis = 2;
 100
 101    float split = node->bbx[0][axis] + extent[axis]*0.5f;
 102    float avg = 0.0;
 103    for( u32 t=0; t<node->count; t++ ){
 104       u32 idx = node->start+t;
 105 #ifdef BVH_FIXED_MODE
 106       avg += shape_bvh_centroid( bh->user, idx, axis );
 107 #else
 108       avg += bh->system->item_centroid( bh->user, idx, axis );
 109 #endif
 110    }
 111    avg /= (float)node->count;
 112    split = avg;
 113
 114
 115    i32 i = node->start,
 116        j = i + node->count-1;
 117
 118    while( i <= j ){
 119 #ifdef BVH_FIXED_MODE
 120       f32 centroid = shape_bvh_centroid( bh->user, i, axis );
 121 #else
 122       f32 centroid = bh->system->item_centroid( bh->user, i, axis );
 123 #endif
 124
 125       if( centroid < split )
 126          i ++;
 127       else{
 128 #ifdef BVH_FIXED_MODE
 129          shape_bvh_swap( bh->user, i, j );
 130 #else
 131          bh->system->item_swap( bh->user, i, j );
 132 #endif
 133          j --;
 134       }
 135    }
 136
 137    u32 left_count = i - node->start;
 138    if( left_count == 0 || left_count == node->count ) return;
 139
 140    u32 il = bh->node_count ++,
 141        ir = bh->node_count ++;
 142
 143    bh_node *lnode = &bh->nodes[il],
 144            *rnode = &bh->nodes[ir];
 145
 146    lnode->start = node->start;
 147    lnode->count = left_count;
 148    rnode->start = i;
 149    rnode->count = node->count - left_count;
 150
 151    node->il = il;
 152    node->ir = ir;
 153    node->count = 0;
 154
 155    bh_update_bounds( bh, il );
 156    bh_update_bounds( bh, ir );
 157    bh_subdivide( bh, il );
 158    bh_subdivide( bh, ir );
 159 }
 160
 161 static void bh_rebuild( bh_tree *bh, u32 item_count ){
 162    bh_node *root = &bh->nodes[0];
 163    bh->node_count = 1;
 164
 165    root->il = 0;
 166    root->ir = 0;
 167    root->count = item_count;
 168    root->start = 0;
 169
 170    bh_update_bounds( bh, 0 );
 171
 172    if( item_count > 2 )
 173       bh_subdivide( bh, 0 );
 174 }
 175
 176 static bh_tree *bh_create( void *lin_alloc, bh_system *system,
 177                               void *user, u32 item_count, u32 max_per_leaf ){
 178    assert( max_per_leaf > 0 );
 179
 180    u32 alloc_count = VG_MAX( 1, item_count );
 181
 182    u32 totsize = sizeof(bh_tree) + sizeof(bh_node)*(alloc_count*2-1);
 183    bh_tree *bh = lin_alloc? vg_linear_alloc( lin_alloc, vg_align8(totsize) ):
 184                             malloc( totsize );
 185    bh->system = system;
 186    bh->user = user;
 187    bh->max_per_leaf = max_per_leaf;
 188    bh_rebuild( bh, item_count );
 189
 190    if( lin_alloc ){
 191       totsize = vg_align8(sizeof(bh_tree) + sizeof(bh_node) * bh->node_count);
 192       bh = vg_linear_resize( lin_alloc, bh, totsize );
 193    }
 194
 195    vg_success( "BVH done, size: %u/%u\n", bh->node_count, (alloc_count*2-1) );
 196    return bh;
 197 }
 198
 199 /*
 200  * Draw items in this leaf node.
 201  * *item_debug() must be set!
 202  */
 203 static void bh_debug_leaf( bh_tree *bh, bh_node *node ){
 204    vg_line_boxf( node->bbx, 0xff00ff00 );
 205
 206    if( bh->system->item_debug ){
 207       for( u32 i=0; i<node->count; i++ ){
 208          u32 idx = node->start+i;
 209          bh->system->item_debug( bh->user, idx );
 210       }
 211    }
 212 }
 213
 214 /*
 215  * Trace the bh tree all the way down to the leaf nodes where pos is inside
 216  */
 217 static void bh_debug_trace( bh_tree *bh, u32 inode, v3f pos, u32 colour ){
 218    bh_node *node = &bh->nodes[ inode ];
 219
 220    if( (pos[0] >= node->bbx[0][0] && pos[0] <= node->bbx[1][0]) &&
 221        (pos[2] >= node->bbx[0][2] && pos[2] <= node->bbx[1][2]) )
 222    {
 223       if( !node->count ){
 224          vg_line_boxf( node->bbx, colour );
 225
 226          bh_debug_trace( bh, node->il, pos, colour );
 227          bh_debug_trace( bh, node->ir, pos, colour );
 228       }
 229       else{
 230          if( bh->system->item_debug )
 231             bh_debug_leaf( bh, node );
 232       }
 233    }
 234 }
 235
 236 typedef struct bh_iter bh_iter;
 237 struct bh_iter{
 238    struct {
 239       i32 id, depth;
 240    }
 241    stack[64];
 242
 243    enum bh_query_type{
 244       k_bh_query_box,
 245       k_bh_query_ray,
 246       k_bh_query_range
 247    }
 248    query;
 249
 250    union{
 251       struct{
 252          boxf box;
 253       }
 254       box;
 255
 256       struct{
 257          v3f co, inv_dir;
 258          f32 max_dist;
 259       }
 260       ray;
 261
 262       struct {
 263          v3f co;
 264          f32 dist_sqr;
 265       }
 266       range;
 267    };
 268
 269    i32 depth, i;
 270 };
 271
 272 static void bh_iter_init_generic( i32 root, bh_iter *it ){
 273    it->stack[0].id = root;
 274    it->stack[0].depth = 0;
 275    it->depth = 0;
 276    it->i = 0;
 277 }
 278
 279 static void bh_iter_init_box( i32 root, bh_iter *it, boxf box ){
 280    bh_iter_init_generic( root, it );
 281    it->query = k_bh_query_box;
 282
 283    box_copy( box, it->box.box );
 284 }
 285
 286 static void bh_iter_init_ray( i32 root, bh_iter *it, v3f co,
 287                                  v3f dir, f32 max_dist ){
 288    bh_iter_init_generic( root, it );
 289    it->query = k_bh_query_ray;
 290
 291    v3_div( (v3f){1.0f,1.0f,1.0f}, dir, it->ray.inv_dir );
 292    v3_copy( co, it->ray.co );
 293    it->ray.max_dist = max_dist;
 294 }
 295
 296 static void bh_iter_init_range( i32 root, bh_iter *it, v3f co, f32 range ){
 297    bh_iter_init_generic( root, it );
 298    it->query = k_bh_query_range;
 299
 300    v3_copy( co, it->range.co );
 301    it->range.dist_sqr = range*range;
 302 }
 303
 304 /* NOTE: does not compute anything beyond the leaf level. element level tests
 305  *       should be implemented by the users code.
 306  *
 307  *       this is like a 'broad phase only' deal.
 308  */
 309 static i32 bh_next( bh_tree *bh, bh_iter *it, i32 *em ){
 310    while( it->depth >= 0 ){
 311       bh_node *inode = &bh->nodes[ it->stack[it->depth].id ];
 312
 313       /* Only process overlapping nodes */
 314       i32 q = 0;
 315
 316       if( it->i ) /* already checked */
 317          q = 1;
 318       else{
 319          if( it->query == k_bh_query_box )
 320             q = box_overlap( inode->bbx, it->box.box );
 321          else if( it->query == k_bh_query_ray )
 322             q = ray_aabb1( inode->bbx, it->ray.co,
 323                            it->ray.inv_dir, it->ray.max_dist );
 324          else {
 325             v3f nearest;
 326             closest_point_aabb( it->range.co, inode->bbx, nearest );
 327
 328             if( v3_dist2( nearest, it->range.co ) <= it->range.dist_sqr )
 329                q = 1;
 330          }
 331       }
 332
 333       if( !q ){
 334          it->depth --;
 335          continue;
 336       }
 337
 338       if( inode->count ){
 339          if( it->i < inode->count ){
 340             *em = inode->start+it->i;
 341             it->i ++;
 342             return 1;
 343          }
 344          else{
 345             it->depth --;
 346             it->i = 0;
 347          }
 348       }
 349       else{
 350          if( it->depth+1 >= vg_list_size(it->stack) ){
 351             vg_error( "Maximum stack reached!\n" );
 352             return 0;
 353          }
 354
 355          it->stack[it->depth  ].id = inode->il;
 356          it->stack[it->depth+1].id = inode->ir;
 357          it->depth ++;
 358          it->i = 0;
 359       }
 360    }
 361
 362    return 0;
 363 }
 364
 365 static int bh_closest_point( bh_tree *bh, v3f pos,
 366                                 v3f closest, float max_dist )
 367 {
 368    if( bh->node_count < 2 )
 369       return -1;
 370
 371    max_dist = max_dist*max_dist;
 372
 373    int queue[ 128 ],
 374        depth = 0,
 375        best_item = -1;
 376
 377    queue[0] = 0;
 378
 379    while( depth >= 0 ){
 380       bh_node *inode = &bh->nodes[ queue[depth] ];
 381
 382       v3f p1;
 383       closest_point_aabb( pos, inode->bbx, p1 );
 384
 385       /* branch into node if its closer than current best */
 386       float node_dist = v3_dist2( pos, p1 );
 387       if( node_dist < max_dist ){
 388          if( inode->count ){
 389             for( int i=0; i<inode->count; i++ ){
 390                v3f p2;
 391                bh->system->item_closest( bh->user, inode->start+i, pos, p2 );
 392
 393                float item_dist = v3_dist2( pos, p2 );
 394                if( item_dist < max_dist ){
 395                   max_dist = item_dist;
 396                   v3_copy( p2, closest );
 397                   best_item = inode->start+i;
 398                }
 399             }
 400
 401             depth --;
 402          }
 403          else{
 404             queue[depth] = inode->il;
 405             queue[depth+1] = inode->ir;
 406
 407             depth ++;
 408          }
 409       }
 410       else
 411          depth --;
 412    }
 413
 414    return best_item;
 415 }