vert_tree.c

Go to the documentation of this file.
00001 /*                     V E R T _ T R E E . C
00002  * BRL-CAD
00003  *
00004  * Copyright (c) 2002-2012 United States Government as represented by
00005  * the U.S. Army Research Laboratory.
00006  *
00007  * This library is free software; you can redistribute it and/or
00008  * modify it under the terms of the GNU Lesser General Public License
00009  * version 2.1 as published by the Free Software Foundation.
00010  *
00011  * This library is distributed in the hope that it will be useful, but
00012  * WITHOUT ANY WARRANTY; without even the implied warranty of
00013  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00014  * Lesser General Public License for more details.
00015  *
00016  * You should have received a copy of the GNU Lesser General Public
00017  * License along with this file; see the file named COPYING for more
00018  * information.
00019  */
00020 /** @addtogroup vtree */
00021 /** @{ */
00022 /** @file libbn/vert_tree.c
00023  *
00024  * @brief
00025  * Routines to manage a binary search tree of vertices.
00026  *
00027  * The actual vertices are stored in an array
00028  * for convenient use by routines such as "mk_bot".
00029  * The binary search tree stores indices into the array.
00030  *
00031  */
00032 
00033 #include "common.h"
00034 
00035 #include <stdlib.h>
00036 #include <stdio.h>
00037 #include <math.h>
00038 #include <string.h>
00039 #include <ctype.h>
00040 #include <errno.h>
00041 
00042 #include "bn.h"
00043 
00044 
00045 /**
00046  * Structure to make vertex searching fast.
00047  *
00048  * Each leaf represents a vertex, and has an index into
00049  *   the vertices array ("the_array")
00050  *
00051  * Each node is a cutting plane at the "cut_val" on
00052  *   the "coord" (0, 1, or 2) axis.
00053  *
00054  * All vertices with "coord" value less than the "cut_val" are in the "lower"
00055  * subtree, others are in the "higher".
00056  */
00057 union vert_tree {
00058     char type;          /**< type - leaf or node */
00059     struct vert_leaf {
00060         char type;
00061         int index;      /**< index into the array */
00062     } vleaf;
00063     struct vert_node {
00064         char type;
00065         double cut_val; /**< cutting value */
00066         int coord;      /**< cutting coordinate */
00067         union vert_tree *higher, *lower;        /**< subtrees */
00068     } vnode;
00069 };
00070 
00071 /* types for the above "vert_tree" */
00072 #define VERT_LEAF       'l'
00073 #define VERT_NODE       'n'
00074 
00075 
00076 /**             C R E A T E _ V E R T _ T R E E
00077  *@brief
00078  *      routine to create a vertex tree.
00079  *
00080  *      Possible refinements include specifying an initial size
00081  */
00082 struct vert_root *
00083 create_vert_tree()
00084 {
00085     struct vert_root *tree;
00086 
00087     tree = (struct vert_root *)bu_calloc( 1, sizeof( struct vert_root ), "vert_tree_root" );
00088     tree->magic = VERT_TREE_MAGIC;
00089     tree->tree_type = TREE_TYPE_VERTS;
00090     tree->the_tree = (union vert_tree *)NULL;
00091     tree->curr_vert = 0;
00092     tree->max_vert = VERT_BLOCK;
00093     tree->the_array = (fastf_t *)bu_malloc( tree->max_vert * 3 * sizeof( fastf_t ), "vert tree array" );
00094 
00095     return tree;
00096 }
00097 
00098 /**             C R E A T E _ V E R T _ T R E E _ W _ N O R M S
00099  *@brief
00100  *      routine to create a vertex tree.
00101  *
00102  *      Possible refinements include specifying an initial size
00103  */
00104 struct vert_root *
00105 create_vert_tree_w_norms()
00106 {
00107     struct vert_root *tree;
00108 
00109     tree = (struct vert_root *)bu_calloc( 1, sizeof( struct vert_root ), "vert_tree_root" );
00110     tree->magic = VERT_TREE_MAGIC;
00111     tree->tree_type = TREE_TYPE_VERTS_AND_NORMS;
00112     tree->the_tree = (union vert_tree *)NULL;
00113     tree->curr_vert = 0;
00114     tree->max_vert = VERT_BLOCK;
00115     tree->the_array = (fastf_t *)bu_malloc( tree->max_vert * 6 * sizeof( fastf_t ), "vert tree array" );
00116 
00117     return tree;
00118 }
00119 
00120 /**             C L E A N _ V E R T_ T R E E _ R E C U R S E
00121  *@brief
00122  *      static recursion routine used by "clean_vert_tree"
00123  */
00124 static void
00125 clean_vert_tree_recurse( union vert_tree *ptr )
00126 {
00127     if ( ptr->type == VERT_NODE ) {
00128         clean_vert_tree_recurse( ptr->vnode.higher );
00129         clean_vert_tree_recurse( ptr->vnode.lower );
00130     }
00131 
00132     bu_free( (char *)ptr, "vert_tree" );
00133 
00134 }
00135 
00136 /**             C L E A N _ V E R T _ T R E E
00137  *@brief
00138  *      Routine to free the binary search tree and reset the current number of vertices.
00139  *      The vertex array is left untouched, for re-use later.
00140  */
00141 void
00142 clean_vert_tree( struct vert_root *tree_root )
00143 {
00144     BN_CK_VERT_TREE( tree_root );
00145 
00146     if ( !tree_root->the_tree ) return;
00147 
00148     clean_vert_tree_recurse( tree_root->the_tree );
00149     tree_root->the_tree = (union vert_tree *)NULL;
00150     tree_root->curr_vert = 0;
00151 }
00152 
00153 
00154 /**             F R E E _ V E R T_ T R E E_ R E C U R S E
00155  *@brief
00156  *      Static recursive routine used by "free_vert_tree"
00157  */
00158 static void
00159 free_vert_tree_recurse( union vert_tree *ptr )
00160 {
00161     if ( ptr->type == VERT_NODE ) {
00162         free_vert_tree_recurse( ptr->vnode.higher );
00163         free_vert_tree_recurse( ptr->vnode.lower );
00164     }
00165 
00166     bu_free( (char *)ptr, "vert_tree" );
00167 
00168 }
00169 
00170 /**             F R E E _ V E R T_ T R E E
00171  *@brief
00172  *      Routine to free a vertex tree and all associated dynamic memory
00173  */
00174 void
00175 free_vert_tree( struct vert_root *vert_root )
00176 {
00177     union vert_tree *ptr;
00178 
00179     if ( !vert_root )
00180         return;
00181 
00182     BN_CK_VERT_TREE( vert_root );
00183 
00184     ptr = vert_root->the_tree;
00185     if ( !ptr )
00186         return;
00187 
00188     free_vert_tree_recurse( ptr );
00189 
00190     if ( vert_root->the_array ) {
00191         bu_free( (char *)vert_root->the_array, "vertex array" );
00192     }
00193 
00194     vert_root->the_tree = (union vert_tree *)NULL;
00195     vert_root->the_array = (fastf_t *)NULL;
00196     vert_root->curr_vert = 0;
00197     vert_root->max_vert = 0;
00198 }
00199 
00200 /**             A D D _ V E R T
00201  *@brief
00202  *      Routine to add a vertex to the current list of part vertices.
00203  *      The array is re-alloc'd if needed.
00204  *      Returns index into the array of vertices where this vertex is stored
00205  */
00206 int
00207 Add_vert( double x, double y, double z, struct vert_root *vert_root, fastf_t local_tol_sq )
00208 {
00209     union vert_tree *ptr, *prev=NULL, *new_leaf, *new_node;
00210     vect_t diff = VINIT_ZERO;
00211     vect_t vertex;
00212 
00213     BN_CK_VERT_TREE( vert_root );
00214 
00215     if ( vert_root->tree_type != TREE_TYPE_VERTS ) {
00216         bu_bomb( "Error: Add_vert() called for a tree containing vertices and normals\n" );
00217     }
00218 
00219     VSET( vertex, x, y, z );
00220 
00221     /* look for this vertex already in the list */
00222     ptr = vert_root->the_tree;
00223     while ( ptr ) {
00224         if ( ptr->type == VERT_NODE ) {
00225             prev = ptr;
00226             if ( vertex[ptr->vnode.coord] >= ptr->vnode.cut_val ) {
00227                 ptr = ptr->vnode.higher;
00228             } else {
00229                 ptr = ptr->vnode.lower;
00230             }
00231         } else {
00232             int ij;
00233 
00234             ij = ptr->vleaf.index*3;
00235             diff[0] = fabs( vertex[0] - vert_root->the_array[ij] );
00236             diff[1] = fabs( vertex[1] - vert_root->the_array[ij+1] );
00237             diff[2] = fabs( vertex[2] - vert_root->the_array[ij+2] );
00238             if ( (diff[0]*diff[0] + diff[1]*diff[1] + diff[2]*diff[2]) <= local_tol_sq ) {
00239                 /* close enough, use this vertex again */
00240                 return ptr->vleaf.index;
00241             }
00242             break;
00243         }
00244     }
00245 
00246     /* add this vertex to the list */
00247     if ( vert_root->curr_vert >= vert_root->max_vert ) {
00248         /* allocate more memory for vertices */
00249         vert_root->max_vert += VERT_BLOCK;
00250 
00251         vert_root->the_array = (fastf_t *)bu_realloc( vert_root->the_array, sizeof( fastf_t ) * vert_root->max_vert * 3,
00252                                                       "vert_root->the_array" );
00253     }
00254 
00255     VMOVE( &vert_root->the_array[vert_root->curr_vert*3], vertex );
00256 
00257     /* add to the tree also */
00258     new_leaf = (union vert_tree *)bu_malloc( sizeof( union vert_tree ), "new_leaf" );
00259     new_leaf->vleaf.type = VERT_LEAF;
00260     new_leaf->vleaf.index = vert_root->curr_vert++;
00261     if ( !vert_root->the_tree ) {
00262         /* first vertex, it becomes the root */
00263         vert_root->the_tree = new_leaf;
00264     } else if ( ptr && ptr->type == VERT_LEAF ) {
00265         /* search above ended at a leaf, need to add a node above this leaf and the new leaf */
00266         new_node = (union vert_tree *)bu_malloc( sizeof( union vert_tree ), "new_node" );
00267         new_node->vnode.type = VERT_NODE;
00268 
00269         /* select the cutting coord based on the biggest difference */
00270         if ( diff[0] >= diff[1] && diff[0] >= diff[2] ) {
00271             new_node->vnode.coord = 0;
00272         } else if ( diff[1] >= diff[2] && diff[1] >= diff[0] ) {
00273             new_node->vnode.coord = 1;
00274         } else if ( diff[2] >= diff[1] && diff[2] >= diff[0] ) {
00275             new_node->vnode.coord = 2;
00276         }
00277 
00278         /* set the cut value to the mid value between the two vertices */
00279         new_node->vnode.cut_val = (vertex[new_node->vnode.coord] +
00280                                    vert_root->the_array[ptr->vleaf.index * 3 + new_node->vnode.coord]) * 0.5;
00281 
00282         /* set the node "lower" nad "higher" pointers */
00283         if ( vertex[new_node->vnode.coord] >=
00284              vert_root->the_array[ptr->vleaf.index * 3 + new_node->vnode.coord] ) {
00285             new_node->vnode.higher = new_leaf;
00286             new_node->vnode.lower = ptr;
00287         } else {
00288             new_node->vnode.higher = ptr;
00289             new_node->vnode.lower = new_leaf;
00290         }
00291 
00292         if ( ptr == vert_root->the_tree ) {
00293             /* if the above search ended at the root, redefine the root */
00294             vert_root->the_tree =  new_node;
00295         } else {
00296             /* set the previous node to point to our new one */
00297             if ( prev->vnode.higher == ptr ) {
00298                 prev->vnode.higher = new_node;
00299             } else {
00300                 prev->vnode.lower = new_node;
00301             }
00302         }
00303     } else if ( ptr && ptr->type == VERT_NODE ) {
00304         /* above search ended at a node, just add the new leaf */
00305         prev = ptr;
00306         if ( vertex[prev->vnode.coord] >= prev->vnode.cut_val ) {
00307             if ( prev->vnode.higher ) {
00308                 bu_bomb("higher vertex node already exists in Add_vert()?\n");
00309             }
00310             prev->vnode.higher = new_leaf;
00311         } else {
00312             if ( prev->vnode.lower ) {
00313                 bu_bomb("lower vertex node already exists in Add_vert()?\n");
00314             }
00315             prev->vnode.lower = new_leaf;
00316         }
00317     } else {
00318         fprintf( stderr, "*********ERROR********\n" );
00319     }
00320 
00321     /* return the index into the vertex array */
00322     return new_leaf->vleaf.index;
00323 }
00324 
00325 /**             A D D _ V E R T _ A N D _ N O R M
00326  *@brief
00327  *      Routine to add a vertex and a normal to the current list of part vertices.
00328  *      The array is re-alloc'd if needed.
00329  *      Returns index into the array of vertices where this vertex and normal is stored
00330  */
00331 int
00332 Add_vert_and_norm( double x, double y, double z, double nx, double ny, double nz, struct vert_root *vert_root, fastf_t local_tol_sq )
00333 {
00334     union vert_tree *ptr, *prev=NULL, *new_leaf, *new_node;
00335     fastf_t diff[6];
00336     fastf_t vertex[6];
00337     double d1_sq=0.0, d2_sq=0.0;
00338 
00339     BN_CK_VERT_TREE( vert_root );
00340 
00341     if ( vert_root->tree_type != TREE_TYPE_VERTS_AND_NORMS ) {
00342         bu_bomb( "Error: Add_vert_and_norm() called for a tree containing just vertices\n" );
00343     }
00344 
00345     VSET( vertex, x, y, z );
00346     VSET( &vertex[3], nx, ny, nz );
00347 
00348     /* look for this vertex and normal already in the list */
00349     ptr = vert_root->the_tree;
00350     while ( ptr ) {
00351         int i;
00352 
00353         if ( ptr->type == VERT_NODE ) {
00354             prev = ptr;
00355             if ( vertex[ptr->vnode.coord] >= ptr->vnode.cut_val ) {
00356                 ptr = ptr->vnode.higher;
00357             } else {
00358                 ptr = ptr->vnode.lower;
00359             }
00360         } else {
00361             int ij;
00362 
00363             ij = ptr->vleaf.index*6;
00364             for ( i=0; i<6; i++ ) {
00365                 diff[i] = fabs( vertex[i] - vert_root->the_array[ij+i] );
00366             }
00367             d1_sq = VDOT( diff, diff );
00368             d2_sq = VDOT( &diff[3], &diff[3] );
00369             if ( d1_sq <= local_tol_sq && d2_sq <= 0.0001 ) {
00370                 /* close enough, use this vertex and normal again */
00371                 return ptr->vleaf.index;
00372             }
00373             break;
00374         }
00375     }
00376 
00377     /* add this vertex and normal to the list */
00378     if ( vert_root->curr_vert >= vert_root->max_vert ) {
00379         /* allocate more memory for vertices */
00380         vert_root->max_vert += VERT_BLOCK;
00381 
00382         vert_root->the_array = (fastf_t *)bu_realloc( vert_root->the_array,
00383                                                       sizeof( fastf_t ) * vert_root->max_vert * 6,
00384                                                       "vert_root->the_array" );
00385     }
00386 
00387     VMOVE( &vert_root->the_array[vert_root->curr_vert*6], vertex );
00388     VMOVE( &vert_root->the_array[vert_root->curr_vert*6+3], &vertex[3] );
00389 
00390     /* add to the tree also */
00391     new_leaf = (union vert_tree *)bu_malloc( sizeof( union vert_tree ), "new_leaf" );
00392     new_leaf->vleaf.type = VERT_LEAF;
00393     new_leaf->vleaf.index = vert_root->curr_vert++;
00394     if ( !vert_root->the_tree ) {
00395         /* first vertex, it becomes the root */
00396         vert_root->the_tree = new_leaf;
00397     } else if ( ptr && ptr->type == VERT_LEAF ) {
00398         fastf_t max;
00399         int i;
00400 
00401         /* search above ended at a leaf, need to add a node above this leaf and the new leaf */
00402         new_node = (union vert_tree *)bu_malloc( sizeof( union vert_tree ), "new_node" );
00403         new_node->vnode.type = VERT_NODE;
00404 
00405         /* select the cutting coord based on the biggest difference */
00406         if ( d1_sq <= local_tol_sq ) {
00407             /* cut based on normal */
00408             new_node->vnode.coord = 3;
00409         } else {
00410             new_node->vnode.coord = 0;
00411         }
00412         max = diff[new_node->vnode.coord];
00413         for ( i=new_node->vnode.coord+1; i<6; i++ ) {
00414             if ( diff[i] > max ) {
00415                 new_node->vnode.coord = i;
00416                 max = diff[i];
00417             }
00418         }
00419 
00420         /* set the cut value to the mid value between the two vertices or normals */
00421         new_node->vnode.cut_val = (vertex[new_node->vnode.coord] +
00422                                    vert_root->the_array[ptr->vleaf.index * 3 + new_node->vnode.coord]) * 0.5;
00423 
00424         /* set the node "lower" nad "higher" pointers */
00425         if ( vertex[new_node->vnode.coord] >=
00426              vert_root->the_array[ptr->vleaf.index * 3 + new_node->vnode.coord] ) {
00427             new_node->vnode.higher = new_leaf;
00428             new_node->vnode.lower = ptr;
00429         } else {
00430             new_node->vnode.higher = ptr;
00431             new_node->vnode.lower = new_leaf;
00432         }
00433 
00434         if ( ptr == vert_root->the_tree ) {
00435             /* if the above search ended at the root, redefine the root */
00436             vert_root->the_tree =  new_node;
00437         } else {
00438             /* set the previous node to point to our new one */
00439             if ( prev->vnode.higher == ptr ) {
00440                 prev->vnode.higher = new_node;
00441             } else {
00442                 prev->vnode.lower = new_node;
00443             }
00444         }
00445     } else if ( ptr && ptr->type == VERT_NODE ) {
00446         /* above search ended at a node, just add the new leaf */
00447         prev = ptr;
00448         if ( vertex[prev->vnode.coord] >= prev->vnode.cut_val ) {
00449             if ( prev->vnode.higher ) {
00450                 bu_bomb("higher vertex node already exists in Add_vert_and_norm()?\n");
00451             }
00452             prev->vnode.higher = new_leaf;
00453         } else {
00454             if ( prev->vnode.lower ) {
00455                 bu_bomb("lower vertex node already exists in Add_vert_and_norm()?\n");
00456             }
00457             prev->vnode.lower = new_leaf;
00458         }
00459     } else {
00460         fprintf( stderr, "*********ERROR********\n" );
00461     }
00462 
00463     /* return the index into the vertex array */
00464     return new_leaf->vleaf.index;
00465 }
00466 /** @} */
00467 /*
00468  * Local Variables:
00469  * mode: C
00470  * tab-width: 8
00471  * indent-tabs-mode: t
00472  * c-file-style: "stroustrup"
00473  * End:
00474  * ex: shiftwidth=4 tabstop=8
00475  */
Generated on Tue Dec 11 13:14:28 2012 for LIBBN by  doxygen 1.6.3