00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
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
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057 union vert_tree {
00058 char type;
00059 struct vert_leaf {
00060 char type;
00061 int index;
00062 } vleaf;
00063 struct vert_node {
00064 char type;
00065 double cut_val;
00066 int coord;
00067 union vert_tree *higher, *lower;
00068 } vnode;
00069 };
00070
00071
00072 #define VERT_LEAF 'l'
00073 #define VERT_NODE 'n'
00074
00075
00076
00077
00078
00079
00080
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
00099
00100
00101
00102
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
00121
00122
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
00137
00138
00139
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
00155
00156
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
00171
00172
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
00201
00202
00203
00204
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
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
00240 return ptr->vleaf.index;
00241 }
00242 break;
00243 }
00244 }
00245
00246
00247 if ( vert_root->curr_vert >= vert_root->max_vert ) {
00248
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
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
00263 vert_root->the_tree = new_leaf;
00264 } else if ( ptr && ptr->type == VERT_LEAF ) {
00265
00266 new_node = (union vert_tree *)bu_malloc( sizeof( union vert_tree ), "new_node" );
00267 new_node->vnode.type = VERT_NODE;
00268
00269
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
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
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
00294 vert_root->the_tree = new_node;
00295 } else {
00296
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
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
00322 return new_leaf->vleaf.index;
00323 }
00324
00325
00326
00327
00328
00329
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
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
00371 return ptr->vleaf.index;
00372 }
00373 break;
00374 }
00375 }
00376
00377
00378 if ( vert_root->curr_vert >= vert_root->max_vert ) {
00379
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
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
00396 vert_root->the_tree = new_leaf;
00397 } else if ( ptr && ptr->type == VERT_LEAF ) {
00398 fastf_t max;
00399 int i;
00400
00401
00402 new_node = (union vert_tree *)bu_malloc( sizeof( union vert_tree ), "new_node" );
00403 new_node->vnode.type = VERT_NODE;
00404
00405
00406 if ( d1_sq <= local_tol_sq ) {
00407
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
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
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
00436 vert_root->the_tree = new_node;
00437 } else {
00438
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
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
00464 return new_leaf->vleaf.index;
00465 }
00466
00467
00468
00469
00470
00471
00472
00473
00474
00475