--- /dev/null
+#pragma once
+#include "vg_mem.h"
+#include "vg_m.h"
+#include "vg_lines.h"
+
+/*
+ * Usage:
+ *
+ * create a bh_system with functions filled out for expand, centroid, and swap.
+ * optionally include item_debug and cast_ray functions if needed, otherwise,
+ * set them to null
+ *
+ * create a bh_tree struct with:
+ * user: a pointer back the base of the data you are ordering
+ * system: the system we created above which will deal with the data
+ *
+ * call bh_create( bh_tree *bh, u32 item_count )
+ * static int bh_ray( bh_tree *bh, u32 inode, v3f co, v3f dir, ray_hit *hit )
+ * static int bh_select( bh_tree *bh, boxf box, u32 *buffer, int len )
+ */
+
+typedef struct bh_node bh_node;
+typedef struct bh_tree bh_tree;
+typedef struct bh_system bh_system;
+
+typedef struct ray_hit ray_hit;
+struct ray_hit{
+ float dist;
+ u32 *tri;
+ v3f pos, normal;
+};
+
+struct bh_tree{
+ u32 node_count;
+
+ bh_system *system;
+ void *user;
+ u32 max_per_leaf;
+
+ struct bh_node{
+ boxf bbx;
+
+ /* if il is 0, this is a leaf */
+ int il, count;
+ union{ int ir, start; };
+ }
+ nodes[];
+};
+
+struct bh_system{
+ void (*expand_bound)( void *user, boxf bound, u32 item_index );
+ float (*item_centroid)( void *user, u32 item_index, int axis );
+ void (*item_closest)( void *user, u32 item_index, v3f point, v3f closest );
+ void (*item_swap)( void *user, u32 ia, u32 ib );
+
+ /*
+ * Optional:
+ * item_debug - draw this item quickly usually with lines
+ * cast_ray - shoot a ray against the object, if this is not set,
+ * raycasts will simply return the hit on the bvh node
+ */
+
+ void (*item_debug)( void *user, u32 item_index );
+ int (*cast_ray)( void *user, u32 index, v3f co, v3f dir, ray_hit *hit );
+};
+
+#define BVH_FIXED_MODE
+#ifdef BVH_FIXED_MODE
+static f32 shape_bvh_centroid( void *user, u32 item_index, int axis );
+static void shape_bvh_swap( void *user, u32 ia, u32 ib );
+static void shape_bvh_expand_bound( void *user, boxf bound, u32 item_index );
+#endif
+
+static void bh_update_bounds( bh_tree *bh, u32 inode ){
+ bh_node *node = &bh->nodes[ inode ];
+
+ box_init_inf( node->bbx );
+ for( u32 i=0; i<node->count; i++ ){
+ u32 idx = node->start+i;
+#ifdef BVH_FIXED_MODE
+ shape_bvh_expand_bound( bh->user, node->bbx, idx );
+#else
+ bh->system->expand_bound( bh->user, node->bbx, idx );
+#endif
+ }
+}
+
+static void bh_subdivide( bh_tree *bh, u32 inode ){
+ bh_node *node = &bh->nodes[ inode ];
+
+ if( node->count <= bh->max_per_leaf )
+ return;
+
+ v3f extent;
+ v3_sub( node->bbx[1], node->bbx[0], extent );
+
+ int axis = 0;
+ if( extent[1] > extent[0] ) axis = 1;
+ if( extent[2] > extent[axis] ) axis = 2;
+
+ float split = node->bbx[0][axis] + extent[axis]*0.5f;
+ float avg = 0.0;
+ for( u32 t=0; t<node->count; t++ ){
+ u32 idx = node->start+t;
+#ifdef BVH_FIXED_MODE
+ avg += shape_bvh_centroid( bh->user, idx, axis );
+#else
+ avg += bh->system->item_centroid( bh->user, idx, axis );
+#endif
+ }
+ avg /= (float)node->count;
+ split = avg;
+
+
+ i32 i = node->start,
+ j = i + node->count-1;
+
+ while( i <= j ){
+#ifdef BVH_FIXED_MODE
+ f32 centroid = shape_bvh_centroid( bh->user, i, axis );
+#else
+ f32 centroid = bh->system->item_centroid( bh->user, i, axis );
+#endif
+
+ if( centroid < split )
+ i ++;
+ else{
+#ifdef BVH_FIXED_MODE
+ shape_bvh_swap( bh->user, i, j );
+#else
+ bh->system->item_swap( bh->user, i, j );
+#endif
+ j --;
+ }
+ }
+
+ u32 left_count = i - node->start;
+ if( left_count == 0 || left_count == node->count ) return;
+
+ u32 il = bh->node_count ++,
+ ir = bh->node_count ++;
+
+ bh_node *lnode = &bh->nodes[il],
+ *rnode = &bh->nodes[ir];
+
+ lnode->start = node->start;
+ lnode->count = left_count;
+ rnode->start = i;
+ rnode->count = node->count - left_count;
+
+ node->il = il;
+ node->ir = ir;
+ node->count = 0;
+
+ bh_update_bounds( bh, il );
+ bh_update_bounds( bh, ir );
+ bh_subdivide( bh, il );
+ bh_subdivide( bh, ir );
+}
+
+static void bh_rebuild( bh_tree *bh, u32 item_count ){
+ bh_node *root = &bh->nodes[0];
+ bh->node_count = 1;
+
+ root->il = 0;
+ root->ir = 0;
+ root->count = item_count;
+ root->start = 0;
+
+ bh_update_bounds( bh, 0 );
+
+ if( item_count > 2 )
+ bh_subdivide( bh, 0 );
+}
+
+static bh_tree *bh_create( void *lin_alloc, bh_system *system,
+ void *user, u32 item_count, u32 max_per_leaf ){
+ assert( max_per_leaf > 0 );
+
+ u32 alloc_count = VG_MAX( 1, item_count );
+
+ u32 totsize = sizeof(bh_tree) + sizeof(bh_node)*(alloc_count*2-1);
+ bh_tree *bh = lin_alloc? vg_linear_alloc( lin_alloc, vg_align8(totsize) ):
+ malloc( totsize );
+ bh->system = system;
+ bh->user = user;
+ bh->max_per_leaf = max_per_leaf;
+ bh_rebuild( bh, item_count );
+
+ if( lin_alloc ){
+ totsize = vg_align8(sizeof(bh_tree) + sizeof(bh_node) * bh->node_count);
+ bh = vg_linear_resize( lin_alloc, bh, totsize );
+ }
+
+ vg_success( "BVH done, size: %u/%u\n", bh->node_count, (alloc_count*2-1) );
+ return bh;
+}
+
+/*
+ * Draw items in this leaf node.
+ * *item_debug() must be set!
+ */
+static void bh_debug_leaf( bh_tree *bh, bh_node *node ){
+ vg_line_boxf( node->bbx, 0xff00ff00 );
+
+ if( bh->system->item_debug ){
+ for( u32 i=0; i<node->count; i++ ){
+ u32 idx = node->start+i;
+ bh->system->item_debug( bh->user, idx );
+ }
+ }
+}
+
+/*
+ * Trace the bh tree all the way down to the leaf nodes where pos is inside
+ */
+static void bh_debug_trace( bh_tree *bh, u32 inode, v3f pos, u32 colour ){
+ bh_node *node = &bh->nodes[ inode ];
+
+ if( (pos[0] >= node->bbx[0][0] && pos[0] <= node->bbx[1][0]) &&
+ (pos[2] >= node->bbx[0][2] && pos[2] <= node->bbx[1][2]) )
+ {
+ if( !node->count ){
+ vg_line_boxf( node->bbx, colour );
+
+ bh_debug_trace( bh, node->il, pos, colour );
+ bh_debug_trace( bh, node->ir, pos, colour );
+ }
+ else{
+ if( bh->system->item_debug )
+ bh_debug_leaf( bh, node );
+ }
+ }
+}
+
+typedef struct bh_iter bh_iter;
+struct bh_iter{
+ struct {
+ i32 id, depth;
+ }
+ stack[64];
+
+ enum bh_query_type{
+ k_bh_query_box,
+ k_bh_query_ray,
+ k_bh_query_range
+ }
+ query;
+
+ union{
+ struct{
+ boxf box;
+ }
+ box;
+
+ struct{
+ v3f co, inv_dir;
+ f32 max_dist;
+ }
+ ray;
+
+ struct {
+ v3f co;
+ f32 dist_sqr;
+ }
+ range;
+ };
+
+ i32 depth, i;
+};
+
+static void bh_iter_init_generic( i32 root, bh_iter *it ){
+ it->stack[0].id = root;
+ it->stack[0].depth = 0;
+ it->depth = 0;
+ it->i = 0;
+}
+
+static void bh_iter_init_box( i32 root, bh_iter *it, boxf box ){
+ bh_iter_init_generic( root, it );
+ it->query = k_bh_query_box;
+
+ box_copy( box, it->box.box );
+}
+
+static void bh_iter_init_ray( i32 root, bh_iter *it, v3f co,
+ v3f dir, f32 max_dist ){
+ bh_iter_init_generic( root, it );
+ it->query = k_bh_query_ray;
+
+ v3_div( (v3f){1.0f,1.0f,1.0f}, dir, it->ray.inv_dir );
+ v3_copy( co, it->ray.co );
+ it->ray.max_dist = max_dist;
+}
+
+static void bh_iter_init_range( i32 root, bh_iter *it, v3f co, f32 range ){
+ bh_iter_init_generic( root, it );
+ it->query = k_bh_query_range;
+
+ v3_copy( co, it->range.co );
+ it->range.dist_sqr = range*range;
+}
+
+/* NOTE: does not compute anything beyond the leaf level. element level tests
+ * should be implemented by the users code.
+ *
+ * this is like a 'broad phase only' deal.
+ */
+static i32 bh_next( bh_tree *bh, bh_iter *it, i32 *em ){
+ while( it->depth >= 0 ){
+ bh_node *inode = &bh->nodes[ it->stack[it->depth].id ];
+
+ /* Only process overlapping nodes */
+ i32 q = 0;
+
+ if( it->i ) /* already checked */
+ q = 1;
+ else{
+ if( it->query == k_bh_query_box )
+ q = box_overlap( inode->bbx, it->box.box );
+ else if( it->query == k_bh_query_ray )
+ q = ray_aabb1( inode->bbx, it->ray.co,
+ it->ray.inv_dir, it->ray.max_dist );
+ else {
+ v3f nearest;
+ closest_point_aabb( it->range.co, inode->bbx, nearest );
+
+ if( v3_dist2( nearest, it->range.co ) <= it->range.dist_sqr )
+ q = 1;
+ }
+ }
+
+ if( !q ){
+ it->depth --;
+ continue;
+ }
+
+ if( inode->count ){
+ if( it->i < inode->count ){
+ *em = inode->start+it->i;
+ it->i ++;
+ return 1;
+ }
+ else{
+ it->depth --;
+ it->i = 0;
+ }
+ }
+ else{
+ if( it->depth+1 >= vg_list_size(it->stack) ){
+ vg_error( "Maximum stack reached!\n" );
+ return 0;
+ }
+
+ it->stack[it->depth ].id = inode->il;
+ it->stack[it->depth+1].id = inode->ir;
+ it->depth ++;
+ it->i = 0;
+ }
+ }
+
+ return 0;
+}
+
+static int bh_closest_point( bh_tree *bh, v3f pos,
+ v3f closest, float max_dist )
+{
+ if( bh->node_count < 2 )
+ return -1;
+
+ max_dist = max_dist*max_dist;
+
+ int queue[ 128 ],
+ depth = 0,
+ best_item = -1;
+
+ queue[0] = 0;
+
+ while( depth >= 0 ){
+ bh_node *inode = &bh->nodes[ queue[depth] ];
+
+ v3f p1;
+ closest_point_aabb( pos, inode->bbx, p1 );
+
+ /* branch into node if its closer than current best */
+ float node_dist = v3_dist2( pos, p1 );
+ if( node_dist < max_dist ){
+ if( inode->count ){
+ for( int i=0; i<inode->count; i++ ){
+ v3f p2;
+ bh->system->item_closest( bh->user, inode->start+i, pos, p2 );
+
+ float item_dist = v3_dist2( pos, p2 );
+ if( item_dist < max_dist ){
+ max_dist = item_dist;
+ v3_copy( p2, closest );
+ best_item = inode->start+i;
+ }
+ }
+
+ depth --;
+ }
+ else{
+ queue[depth] = inode->il;
+ queue[depth+1] = inode->ir;
+
+ depth ++;
+ }
+ }
+ else
+ depth --;
+ }
+
+ return best_item;
+}
projects / vg.git / blobdiff