/
quadtree.c
108 lines (97 loc) · 3.54 KB
/
quadtree.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
#include "ct-head/random.h"
#include "ct-head/test.h"
#include "data/quadtree.h"
#include "geom/isec.h"
CT_TEST_DECLS
struct bounds_t {
CT_Vec2f min, max;
size_t num;
};
struct isec_t {
CT_Vec2f c;
float r;
CT_Vec2f *sel[4];
size_t num;
};
static int ct_qtree_bounds(const CT_QTNode *node, void *state) {
struct bounds_t *bounds = (struct bounds_t *)state;
ct_min2fv_imm(&bounds->min, node->point);
ct_max2fv_imm(&bounds->max, node->point);
bounds->num++;
return 0;
}
static int ct_qtree_select(const CT_QTNode *node, void *state) {
struct isec_t *isec = (struct isec_t *)state;
CT_Vec2f p = {node->x + node->w, node->y + node->h};
int i = ct_intersect_rect_circle(&node->pos, &p, &isec->c, isec->r);
//CT_INFO("isec: %d, n: %1.3f,%1.3f -> %1.3f,%1.3f c: %f,%f, r: %f", i, node->x, node->y, p.x, p.y, isec->c.x, isec->c.y, isec->r);
if (i) {
if (node->type == CT_TREE_LEAF) {
if (ct_dist2fv(node->point, &isec->c) <= isec->r) {
isec->sel[isec->num++] = node->point;
}
}
return 0;
}
return 1;
}
static void print_isec_results(struct isec_t *isec) {
for (size_t i = 0; i < isec->num; i++) {
CT_INFO("isec %zu: (%f, %f)", i, isec->sel[i]->x, isec->sel[i]->y);
}
}
int test_quadtree() {
CT_INFO("CT_Quadtree size: %zu", sizeof(CT_Quadtree));
CT_INFO("CT_QTNode size: %zu", sizeof(CT_QTNode));
CT_Quadtree t;
ct_qtree_init(&t, 0, 0, 100, 100, 0x10000);
CT_DEF_MPOOL(vpool, 0x10000, CT_Vec2f);
CT_Vec2f *a = ct_vec2f(10, 10, &vpool);
CT_Vec2f *b = ct_vec2f(10, 11, &vpool);
CT_Vec2f *b2 = ct_vec2f(10.1, 11, &vpool);
CT_Vec2f *c = ct_vec2f(50, 12, &vpool);
CT_IS(!ct_qtree_insert(&t, a, NULL), "add a");
CT_IS(!ct_qtree_insert(&t, b, NULL), "add b");
CT_IS(!ct_qtree_insert(&t, c, NULL), "add c");
//ct_qtree_trace(&t);
struct isec_t isec = {{50, 50}, 70.f, {NULL, NULL, NULL, NULL}, 0};
ct_qtree_visit(&t, ct_qtree_select, &isec);
CT_IS(3 == isec.num, "wrong isec count: %zu", isec.num);
//print_isec_results(&isec);
struct isec_t isec2 = {{10, 10}, 0.9f, {NULL, NULL, NULL, NULL}, 0};
ct_qtree_visit(&t, ct_qtree_select, &isec2);
CT_IS(1 == isec2.num, "wrong isec2 count: %zu", isec2.num);
struct isec_t isec3 = {{52, 12}, 5.f, {NULL, NULL, NULL, NULL}, 0};
ct_qtree_visit(&t, ct_qtree_select, &isec3);
CT_IS(1 == isec3.num, "wrong isec3 count: %zu", isec3.num);
CT_IS(ct_qtree_find_leaf(&t, a), "can't find a");
CT_IS(ct_qtree_find_leaf(&t, b), "can't find b");
CT_IS(!ct_qtree_find_leaf(&t, b2), "shouldn't find b2");
ct_qtree_remove(&t, a);
CT_IS(!ct_qtree_find_leaf(&t, a), "shouldn't find a");
CT_IS(ct_qtree_remove(&t, a), "remove a again");
ct_qtree_remove(&t, b);
CT_IS(!ct_qtree_find_leaf(&t, b), "shouldn't find b");
CT_IS(ct_qtree_remove(&t, b), "remove b again");
ct_qtree_remove(&t, c);
CT_IS(!ct_qtree_find_leaf(&t, c), "shouldn't find c");
CT_IS(ct_qtree_remove(&t, c), "remove c again");
CT_IS(CT_TREE_EMPTY == t.root.type, "root is not empty: %zu", t.root.type);
//srand(time(0));
int num = 1e5;
for (int i = 0; i < num; i++) {
CT_Vec2f *p =
ct_vec2f(ct_rand_normpos() * 100, ct_rand_normpos() * 100, &vpool);
ct_qtree_insert(&t, p, NULL);
}
struct bounds_t bounds = {{1000, 1000}, {-1000, -1000}, 0};
ct_qtree_visit_leaves(&t, ct_qtree_bounds, &bounds);
CT_IS(num == bounds.num, "wrong leaf count: %zu", bounds.num);
CT_INFO("%f,%f -> %f, %f, %zu", bounds.min.x, bounds.min.y, bounds.max.x,
bounds.max.y, bounds.num);
ct_mpool_free(&vpool);
ct_qtree_free(&t);
return 0;
fail:
return 1;
}