assorted crap

[carveJwlIkooP6JGAAIwe30JlM.git] / bvh.h

diff --git a/bvh.h b/bvh.h
index 9e4528604d59c8cf205f20d3d172b671102085e1..491756acb0db460df2d5e697002b9b5e5e560169 100644 (file)
--- a/bvh.h
+++ b/bvh.h
@@ -5,6 +5,7 @@
 #ifndef BVH_H
 #define BVH_H
 #include "common.h"
+#include "distq.h"
 
 /*
  * Usage:
@@ -39,8 +40,8 @@ struct bh_tree
       boxf bbx;
 
       /* if il is 0, this is a leaf */
-      u32 il, count;
-      union{ u32 ir, start; };
+      int il, count;
+      union{ int ir, start; };
    } 
    nodes[];
 };
@@ -49,6 +50,7 @@ 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 );
 
    /*
@@ -260,54 +262,127 @@ VG_STATIC int bh_ray( bh_tree *bh, v3f co, v3f dir, ray_hit *hit )
    return count;
 }
 
-VG_STATIC int bh_select( bh_tree *bh, boxf box, u32 *buffer, int len )
+typedef struct bh_iter bh_iter;
+struct bh_iter
 {
-   if( bh->node_count < 2 )
-      return 0;
+   struct 
+   {
+      int id, depth;
+   }
+   stack[64];
 
-   int count = 0;
-   u32 stack[100];
-   u32 depth = 2;
+   int depth, i;
+};
 
-   stack[0] = 0;
-   stack[1] = bh->nodes[0].il;
-   stack[2] = bh->nodes[0].ir;
-   
-   while(depth)
+VG_STATIC void bh_iter_init( int root, bh_iter *it )
+{
+   it->stack[0].id = root;
+   it->stack[0].depth = 0;
+   it->depth = 0;
+   it->i = 0;
+}
+
+VG_STATIC int bh_next( bh_tree *bh, bh_iter *it, boxf box, int *em )
+{
+   while( it->depth >= 0 )
    {
-      bh_node *inode = &bh->nodes[ stack[depth] ];
+      bh_node *inode = &bh->nodes[ it->stack[it->depth].id ];
+      
       if( box_overlap( inode->bbx, box ) )
       {
          if( inode->count )
          {
-            if( count + inode->count >= len )
-               return count;
-
-            for( u32 i=0; i<inode->count; i++ )
-               buffer[ count ++ ] = inode->start+i;
-
-            depth --;
+            if( it->i < inode->count )
+            {
+               *em = inode->start+it->i;
+               it->i ++;
+               return 1;
+            }
+            else
+            {
+               it->depth --;
+               it->i = 0;
+            }
          }
          else
          {
-            if( depth+1 >= vg_list_size(stack) )
+            if( it->depth+1 >= vg_list_size(it->stack) )
             {
                vg_error( "Maximum stack reached!\n" );
-               return count;
+               return 0;
             }
 
-            stack[depth] = inode->il;
-            stack[depth+1] = inode->ir;
-            depth ++;
+            it->stack[it->depth  ].id = inode->il;
+            it->stack[it->depth+1].id = inode->ir;
+            it->depth ++;
+            it->i = 0;
          }
       }
       else
       {
-         depth --;
+         it->depth --;
       }
    }
 
-   return count;
+   return 0;
+}
+
+VG_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;
 }
 
 #endif /* BVH_H */