vg_bvh.c

   1 #include "vg_bvh.h"
   2 #include "vg_mem.h"
   3 #include "vg_m.h"
   4 #include "vg_lines.h"
   5 #include "vg_log.h"
   6
   7 void bh_update_bounds( bh_tree *bh, u32 inode ){
   8    bh_node *node = &bh->nodes[ inode ];
   9
  10    box_init_inf( node->bbx );
  11    for( u32 i=0; i<node->count; i++ ){
  12       u32 idx = node->start+i;
  13       bh->system->expand_bound( bh->user, node->bbx, idx );
  14    }
  15 }
  16
  17 void bh_subdivide( bh_tree *bh, u32 inode ){
  18    bh_node *node = &bh->nodes[ inode ];
  19
  20    if( node->count <= bh->max_per_leaf )
  21       return;
  22
  23    v3f extent;
  24    v3_sub( node->bbx[1], node->bbx[0], extent );
  25
  26    int axis = 0;
  27    if( extent[1] > extent[0] ) axis = 1;
  28    if( extent[2] > extent[axis] ) axis = 2;
  29
  30    float split = node->bbx[0][axis] + extent[axis]*0.5f;
  31    float avg = 0.0;
  32    for( u32 t=0; t<node->count; t++ ){
  33       u32 idx = node->start+t;
  34       avg += bh->system->item_centroid( bh->user, idx, axis );
  35    }
  36    avg /= (float)node->count;
  37    split = avg;
  38
  39
  40    i32 i = node->start,
  41        j = i + node->count-1;
  42
  43    while( i <= j ){
  44       f32 centroid = bh->system->item_centroid( bh->user, i, axis );
  45
  46       if( centroid < split )
  47          i ++;
  48       else{
  49          bh->system->item_swap( bh->user, i, j );
  50          j --;
  51       }
  52    }
  53
  54    u32 left_count = i - node->start;
  55    if( left_count == 0 || left_count == node->count ) return;
  56
  57    u32 il = bh->node_count ++,
  58        ir = bh->node_count ++;
  59
  60    bh_node *lnode = &bh->nodes[il],
  61            *rnode = &bh->nodes[ir];
  62
  63    lnode->start = node->start;
  64    lnode->count = left_count;
  65    rnode->start = i;
  66    rnode->count = node->count - left_count;
  67
  68    node->il = il;
  69    node->ir = ir;
  70    node->count = 0;
  71
  72    bh_update_bounds( bh, il );
  73    bh_update_bounds( bh, ir );
  74    bh_subdivide( bh, il );
  75    bh_subdivide( bh, ir );
  76 }
  77
  78 void bh_rebuild( bh_tree *bh, u32 item_count )
  79 {
  80    bh_node *root = &bh->nodes[0];
  81    bh->node_count = 1;
  82
  83    root->il = 0;
  84    root->ir = 0;
  85    root->count = item_count;
  86    root->start = 0;
  87
  88    bh_update_bounds( bh, 0 );
  89
  90    if( item_count > 2 )
  91       bh_subdivide( bh, 0 );
  92 }
  93
  94 bh_tree *bh_create( void *lin_alloc, bh_system *system,
  95                     void *user, u32 item_count, u32 max_per_leaf )
  96 {
  97    u32 alloc_count = VG_MAX( 1, item_count );
  98
  99    u32 totsize = sizeof(bh_tree) + sizeof(bh_node)*(alloc_count*2-1);
 100    bh_tree *bh = lin_alloc? vg_linear_alloc( lin_alloc, vg_align8(totsize) ):
 101                             malloc( totsize );
 102    bh->system = system;
 103    bh->user = user;
 104    bh->max_per_leaf = max_per_leaf;
 105    bh_rebuild( bh, item_count );
 106
 107    if( lin_alloc ){
 108       totsize = vg_align8(sizeof(bh_tree) + sizeof(bh_node) * bh->node_count);
 109       bh = vg_linear_resize( lin_alloc, bh, totsize );
 110    }
 111
 112    vg_success( "BVH done, size: %u/%u\n", bh->node_count, (alloc_count*2-1) );
 113    return bh;
 114 }
 115
 116 /*
 117  * Draw items in this leaf node.
 118  * *item_debug() must be set!
 119  */
 120 void bh_debug_leaf( bh_tree *bh, bh_node *node )
 121 {
 122    vg_line_boxf( node->bbx, 0xff00ff00 );
 123
 124    if( bh->system->item_debug ){
 125       for( u32 i=0; i<node->count; i++ ){
 126          u32 idx = node->start+i;
 127          bh->system->item_debug( bh->user, idx );
 128       }
 129    }
 130 }
 131
 132 /*
 133  * Trace the bh tree all the way down to the leaf nodes where pos is inside
 134  */
 135 void bh_debug_trace( bh_tree *bh, u32 inode, v3f pos, u32 colour )
 136 {
 137    bh_node *node = &bh->nodes[ inode ];
 138
 139    if( (pos[0] >= node->bbx[0][0] && pos[0] <= node->bbx[1][0]) &&
 140        (pos[2] >= node->bbx[0][2] && pos[2] <= node->bbx[1][2]) )
 141    {
 142       if( !node->count ){
 143          vg_line_boxf( node->bbx, colour );
 144
 145          bh_debug_trace( bh, node->il, pos, colour );
 146          bh_debug_trace( bh, node->ir, pos, colour );
 147       }
 148       else{
 149          if( bh->system->item_debug )
 150             bh_debug_leaf( bh, node );
 151       }
 152    }
 153 }
 154
 155 void bh_iter_init_generic( i32 root, bh_iter *it )
 156 {
 157    it->stack[0].id = root;
 158    it->stack[0].depth = 0;
 159    it->depth = 0;
 160    it->i = 0;
 161 }
 162
 163 void bh_iter_init_box( i32 root, bh_iter *it, boxf box )
 164 {
 165    bh_iter_init_generic( root, it );
 166    it->query = k_bh_query_box;
 167
 168    box_copy( box, it->box.box );
 169 }
 170
 171 void bh_iter_init_ray( i32 root, bh_iter *it, v3f co, v3f dir, f32 max_dist )
 172 {
 173    bh_iter_init_generic( root, it );
 174    it->query = k_bh_query_ray;
 175
 176    v3_div( (v3f){1.0f,1.0f,1.0f}, dir, it->ray.inv_dir );
 177    v3_copy( co, it->ray.co );
 178    it->ray.max_dist = max_dist;
 179 }
 180
 181 void bh_iter_init_range( i32 root, bh_iter *it, v3f co, f32 range )
 182 {
 183    bh_iter_init_generic( root, it );
 184    it->query = k_bh_query_range;
 185
 186    v3_copy( co, it->range.co );
 187    it->range.dist_sqr = range*range;
 188 }
 189
 190 /* NOTE: does not compute anything beyond the leaf level. element level tests
 191  *       should be implemented by the users code.
 192  *
 193  *       this is like a 'broad phase only' deal.
 194  */
 195 i32 bh_next( bh_tree *bh, bh_iter *it, i32 *em )
 196 {
 197    while( it->depth >= 0 ){
 198       bh_node *inode = &bh->nodes[ it->stack[it->depth].id ];
 199
 200       /* Only process overlapping nodes */
 201       i32 q = 0;
 202
 203       if( it->i ) /* already checked */
 204          q = 1;
 205       else{
 206          if( it->query == k_bh_query_box )
 207             q = box_overlap( inode->bbx, it->box.box );
 208          else if( it->query == k_bh_query_ray )
 209             q = ray_aabb1( inode->bbx, it->ray.co,
 210                            it->ray.inv_dir, it->ray.max_dist );
 211          else {
 212             v3f nearest;
 213             closest_point_aabb( it->range.co, inode->bbx, nearest );
 214
 215             if( v3_dist2( nearest, it->range.co ) <= it->range.dist_sqr )
 216                q = 1;
 217          }
 218       }
 219
 220       if( !q ){
 221          it->depth --;
 222          continue;
 223       }
 224
 225       if( inode->count ){
 226          if( it->i < inode->count ){
 227             *em = inode->start+it->i;
 228             it->i ++;
 229             return 1;
 230          }
 231          else{
 232             it->depth --;
 233             it->i = 0;
 234          }
 235       }
 236       else{
 237          if( it->depth+1 >= vg_list_size(it->stack) ){
 238             vg_error( "Maximum stack reached!\n" );
 239             return 0;
 240          }
 241
 242          it->stack[it->depth  ].id = inode->il;
 243          it->stack[it->depth+1].id = inode->ir;
 244          it->depth ++;
 245          it->i = 0;
 246       }
 247    }
 248
 249    return 0;
 250 }
 251
 252 int bh_closest_point( bh_tree *bh, v3f pos, v3f closest, float max_dist )
 253 {
 254    if( bh->node_count < 2 )
 255       return -1;
 256
 257    max_dist = max_dist*max_dist;
 258
 259    int queue[ 128 ],
 260        depth = 0,
 261        best_item = -1;
 262
 263    queue[0] = 0;
 264
 265    while( depth >= 0 ){
 266       bh_node *inode = &bh->nodes[ queue[depth] ];
 267
 268       v3f p1;
 269       closest_point_aabb( pos, inode->bbx, p1 );
 270
 271       /* branch into node if its closer than current best */
 272       float node_dist = v3_dist2( pos, p1 );
 273       if( node_dist < max_dist ){
 274          if( inode->count ){
 275             for( int i=0; i<inode->count; i++ ){
 276                v3f p2;
 277                bh->system->item_closest( bh->user, inode->start+i, pos, p2 );
 278
 279                float item_dist = v3_dist2( pos, p2 );
 280                if( item_dist < max_dist ){
 281                   max_dist = item_dist;
 282                   v3_copy( p2, closest );
 283                   best_item = inode->start+i;
 284                }
 285             }
 286
 287             depth --;
 288          }
 289          else{
 290             queue[depth] = inode->il;
 291             queue[depth+1] = inode->ir;
 292
 293             depth ++;
 294          }
 295       }
 296       else
 297          depth --;
 298    }
 299
 300    return best_item;
 301 }