quadtree——建立空间四分树源代码

//----------------------------------------------------------------------------------- /* * quadtree.c *¡¡¡¡¡¡¡¡¡¡ Quad tree implementation -- for spatial quick searching *¡¡¡¡¡¡¡¡¡¡ cheungmine *¡¡¡¡¡¡¡¡¡¡ Oct. 5, 2007.¡¡ All rights reserved. */ //----------------------------------------------------------------------------------- #include "quadtree.h" #include <assert.h> //----------------------------------------------------------------------------------- //----------------------------------------------------------------------------------- #define QUAD_MEM_BLOCK_SIZE 68288512 // 1024*1024*64=64M #define QUAD_NODE_MEM 0x0001 #define QUAD_DATA_MEM 0x0002 typedef struct{ struct list_head list_node; ems_uint8 quadmem[QUAD_MEM_BLOCK_SIZE]; }quad_block_t; //---------------------------------------------------------------------------------- static int quad_allocblock(quadtree_t *qtree, int type) { quad_block_t *mem_node =NULL; ems_int32 data_size, avali_size; ems_uint8 *block_mem; // 1 malloc memory mem_node = (quad_block_t*)malloc(sizeof(quad_block_t)); if (mem_node == NULL) return FALSE; list_add_after(&mem_node->list_node, &qtree->mem_list_head); // 2 init to free cache block_mem = mem_node->quadmem+0; avali_size = QUAD_MEM_BLOCK_SIZE; if (type == QUAD_NODE_MEM){ // for the quadnode data_size = ALIGN(sizeof(quadnode_t)); while(avali_size>=data_size){ quadnode_t *node = (quadnode_t*)block_mem; list_add_before( &node->list_node, &qtree->free_node_list_head); block_mem += data_size; avali_size -= data_size; } } else{ // for the key data data_size = ALIGN(sizeof(keydata_t)); while(avali_size>=data_size){ keydata_t *node = (keydata_t*)block_mem; list_add_before( &node->list_node, &qtree->free_data_list_head); block_mem += data_size; avali_size -= data_size; } } return TRUE; } static quadnode_t* quad_allocnode(quadtree_t *qtree) { quadnode_t* node =NULL; // malloc from current block if (!list_empty(qtree->free_node_list_head)){ node = list_entry(qtree->free_node_list_head.next, quadnode_t, list_node); list_del(qtree->free_node_list_head.next); return node; } // block is empty, make new block if (quad_allocblock(qtree, QUAD_NODE_MEM) == FALSE) return NULL; // if success, return number 1 node = list_entry(qtree->free_node_list_head.next, quadnode_t, list_node); list_del(qtree->free_node_list_head.next); return node; } static keydata_t* quad_allocdata(quadtree_t *qtree) { keydata_t* data =NULL; // malloc from current block if (!list_empty(qtree->free_node_list_head)){ data = list_entry(qtree->free_data_list_head.next, keydata_t, list_node); list_del(qtree->free_data_list_head.next); return data; } // block is empty, make new block if (quad_allocblock(qtree, QUAD_DATA_MEM) == FALSE) return NULL; // if success, return number 1 data = list_entry(qtree->free_data_list_head.next, keydata_t, list_node); list_del(qtree->free_data_list_head.next); return data; } //---------------------------------------------------------------------------------- static int quadbox_is_valid (quadbox_t *qb) { return (qb->xmin < qb->xmax && qb->ymin < qb->ymax)? TRUE : FALSE; } static double quadbox_width (const quadbox_t* qb) { return (qb->xmax - qb->xmin); } static double quadbox_height (const quadbox_t* qb) { return (qb->ymax - qb->ymin); } static void quadbox_init (quadbox_t*qb, double xmin, double ymin, double xmax, double ymax) { qb->xmin = xmin; qb->ymin = ymin; qb->xmax = xmax; qb->ymax = ymax; assert (quadbox_is_valid(qb) ); } static void quadbox_inflate(quadbox_t* qb, double dx, double dy) { assert (dx>0 && dy>0); qb->xmin -= (dx/2); qb->xmax += (dx/2); qb->ymin -= (dy/2); qb->ymax += (dy/2); } /* splits the quadrant such as below: ¡¡¡¡¡¡¡¡¡¡ nw(0010)¡¡ |¡¡ ne(0001) ¡¡¡¡¡¡¡¡¡¡ -----------|---------- ¡¡¡¡¡¡¡¡¡¡ sw(0100)¡¡ |¡¡ se(1000) */ static void quadbox_split(const quadbox_t* qb, quadbox_t* ne, quadbox_t* nw, quadbox_t* se, quadbox_t* sw, float overlap) { double dx = quadbox_width(qb)*(1.0 + overlap)/2; double dy = quadbox_height(qb)*(1.0 + overlap)/2; assert (overlap >= QBOX_OVERLAP_MIN-0.0001); assert (overlap <= QBOX_OVERLAP_MAX+0.0001); quadbox_init (ne, qb->xmax-dx, qb->ymax-dy, qb->xmax, qb->ymax); quadbox_init (nw, qb->xmin, qb->ymax-dy, qb->xmin+dx, qb->ymax); quadbox_init (sw, qb->xmin, qb->ymin, qb->xmin+dx, qb->ymin+dy); quadbox_init (se, qb->xmax-dx, qb->ymin, qb->xmax, qb->ymin+dy); } /* returns TRUE if the first is inside the second */ static int quadbox_is_inside(const quadbox_t* _first, const quadbox_t* _second) { return (_second->xmin < _first->xmin && _second->xmax > _first->xmax && _second->ymin < _first->ymin && _second->ymax > _first->ymax)? TRUE : FALSE; } /* returns TRUE if two quad_box is overlapped */ static int quadbox_is_overlapped(const quadbox_t* _first, const quadbox_t* _second) { return (_first->xmin > _second->xmax || _first->xmax < _second->xmin || _first->ymin > _second->ymax || _first->ymax < _second->ymin)? FALSE : TRUE; } static quadnode_t* quadnode_create(quadtree_t *qtree, const quadbox_t* box) { quadnode_t *node = quad_allocnode(qtree); if (node != NULL){ memcpy (&(node->box), box, sizeof(quadbox_t)); INIT_LIST_HEAD(&node->data_list_head); } return node; } static void quadnode_destroy(quadtree_t *qtree, quadnode_t* node) { keydata_t *data; struct list_head *datanode, *datahead; if (node->subnode[NE] != 0){ quadnode_destroy(qtree, node->subnode[NE]); quadnode_destroy(qtree, node->subnode[NW]); quadnode_destroy(qtree, node->subnode[SE]); quadnode_destroy(qtree, node->subnode[SW]); } // free the key data list datahead = &node->data_list_head; datanode = datahead->next; while(datanode != datahead){ data = list_entry(datanode, keydata_t, list_node); list_del(datanode); list_add_before(&data->list_node, &qtree->free_data_list_head); datanode = datahead->next; } // free current node list_del(&node->list_node); list_add_before(&node->list_node, &qtree->free_node_list_head); } static void quadnode_create_child(quadtree_t *qtree, quadnode_t* node, float overlap, int depth) { quadbox_t ne, nw, se, sw; assert (node); quadbox_split ( &(node->box), &ne, &nw, &se, &sw, overlap); node->subnode[NE] = quadnode_create(qtree, &ne); node->subnode[NW] = quadnode_create(qtree, &nw); node->subnode[SW] = quadnode_create(qtree, &sw); node->subnode[SE] = quadnode_create(qtree, &se); } static int quadnode_has_child(const quadnode_t* node) { return (node->subnode[NE] != NULL); } static int quadnode_has_data(const quadnode_t* node) { return (!list_empty(node->data_list_head))?TRUE: FALSE; } static void quadnode_add_data (quadtree_t *qtree, quadnode_t* node, long node_key ) { keydata_t *data; assert(qtree); assert(node); data = quad_allocdata(qtree); assert(data); data->key = node_key; list_add_before(&data->list_node, &node->data_list_head); } /* inserts a node to parent node of tree. returns pointer to node */ static quadnode_t* quadtree_insert_node(quadtree_t *qtree, quadnode_t *parent, long node_key, quadbox_t *node_box, int *depth) { if( quadbox_is_inside(node_box, &(parent->box))) { if( ++(*depth) < qtree->depth ) { if( !quadnode_has_child ( parent ) ) quadnode_create_child (qtree, parent, qtree->overlap, (*depth)); if( quadbox_is_inside (node_box, &(parent->subnode[NE]->box) ) ) return quadtree_insert_node (qtree, parent->subnode[NE], node_key, node_box, depth); if( quadbox_is_inside (node_box, &(parent->subnode[NW]->box) ) ) return quadtree_insert_node (qtree, parent->subnode[NW], node_key, node_box, depth); if ( quadbox_is_inside (node_box, &(parent->subnode[SW]->box) ) ) return quadtree_insert_node (qtree, parent->subnode[SW], node_key, node_box,