bvh.h

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