corner[1].x = quadtree_get_max_dimension(node, 3);
if (debug) {
- printf("Initial Search rectangle (%f %f) (%f %f), (%f %f)\n",
+ printf("\nInitial Search rectangle (%f %f) (%f %f), (%f %f)\n",
corner[0].x, corner[0].y,
corner[1].x, corner[1].y,
node->pos.x, node->pos.y);
struct vector *pos,
struct vector *corner)
{
+ struct vector tmp;
+ struct quadtree *parent = tree;
+ double distance, dist;
int ret;
if (!is_within(pos, corner)) {
return NULL;
}
+ vector_sub(pos, &tree->pos, &tmp);
+ distance = vector_abs(&tmp);
+
+ printf("Starting from parent %p (%f %f), target (%f %f)\n", tree,
+ tree->pos.x, tree->pos.y, pos->x, pos->y);
+
while (1) {
+
ret = quadtree_compare_coord(&tree->pos, pos);
+ printf("Compared: %d\n", ret);
+
if (tree->child[ret]) {
tree = tree->child[ret];
continue;
}
- if (is_within(&tree->pos, corner))
- return tree;
+ printf("break\n");
+ break;
+ }
+ if (!is_within(&tree->pos, corner)) {
if (debug)
printf("Node %p (%f %f) is not within "
"search window (%f %f) (%f %f)\n",
tree, tree->pos.x, tree->pos.y,
corner[0].x, corner[0].y,
corner[1].x, corner[1].y);
+ return NULL;
+ }
- return 0;
+ vector_sub(pos, &tree->pos, &tmp);
+ dist = vector_abs(&tmp);
+
+ printf("dist: %f, distance: %f\n", dist, distance);
+ if (dist < distance)
+ return tree;
+
+ printf("Return parent\n");
+ return parent;
+}
+
+static struct quadtree *quadtree_find_nearest_noparent(struct quadtree *tree,
+ struct vector *pos,
+ struct vector *corner)
+{
+ struct quadtree *nearest;
+ struct vector sub;
+ struct quadtree *node;
+ double dist = 0, dist2;
+ int i;
+
+ nearest = quadtree_find_nearest(tree, pos, corner);
+
+ if (nearest != tree)
+ return nearest;
+
+ if (debug)
+ printf("Avoiding parent node %p\n", tree);
+
+ /*
+ * oops, we don't want to pick the parent node, let's choose
+ * another one
+ */
+ nearest = NULL;
+ for (i = 0; i < 4; i++) {
+ if (!tree->child[i])
+ continue;
+
+ if (!nearest) {
+ nearest = quadtree_find_nearest(tree->child[i], pos,
+ corner);
+
+ if (nearest == NULL)
+ continue;
+
+ vector_sub(pos, &nearest->pos, &sub);
+ dist = vector_abs(&sub);
+ printf("Setting nearest from %d\n", i);
+ continue;
+ }
+ node = quadtree_find_nearest(tree->child[i],
+ pos, corner);
+
+ if (node == NULL)
+ continue;
+
+ vector_sub(pos, &node->pos, &sub);
+ dist2 = vector_abs(&sub);
+
+ if (dist2 < dist) {
+ nearest = node;
+ dist = dist2;
+ }
+ printf("Re-setting nearest from %d\n", i);
}
+
+ return nearest;
}
static void get_middle_point(struct vector *corner, struct vector *middle)
{
- middle->x = (corner[0].x + corner[1].x) / 2;
- middle->y = (corner[0].y + corner[1].y) / 2;
+ middle->x = (corner[0].x + corner[1].x) / (double)2;
+ middle->y = (corner[0].y + corner[1].y) / (double)2;
}
static int quadtree_split_by_node(struct quadtree *node,
{
struct vector corner[2], mid;
struct quadtree *t, *tmp;
- int ret, moved = 0;
+ int moved = 0;
get_middle_point(corner_orig, &mid);
- t = quadtree_find_nearest(tree, &mid, corner_orig);
+ t = quadtree_find_nearest_noparent(tree, &mid, corner_orig);
if (!t) {
if (debug)
goto out;
}
- /* oops, we don't want to remove this node, let's pick another
- * one
- */
- if (t == tree) {
- printf("t == tree\n");
- ret = quadtree_compare_coord(&tree->pos, &mid);
-
- if (tree->child[ret]) {
- t = quadtree_find_nearest(tree->child[ret], &mid,
- corner_orig);
- } else {
- printf("Second nearest not found in direction %d\n",
- ret);
- /*
- * No suitable match from the right
- * direction. Try to find a second nearest
- * node.
- */
- struct vector sub;
- struct quadtree *node;
- double dist = 0, dist2;
- int i;
-
- t = NULL;
- for (i = 0; i < 4; i++) {
- if (!tree->child[i])
- continue;
- if (!t) {
- t = quadtree_find_nearest(
- tree->child[i], &mid,
- corner_orig);
-
- if (t == NULL)
- continue;
-
- vector_sub(&mid, &t->pos, &sub);
- dist = vector_abs(&sub);
- printf("Setting nearest from %d\n", i);
- continue;
- }
- node = quadtree_find_nearest(tree->child[i],
- &mid, corner_orig);
-
- if (node == NULL)
- continue;
-
- vector_sub(&mid, &node->pos, &sub);
- dist2 = vector_abs(&sub);
-
- if (dist2 < dist) {
- t = node;
- dist = dist2;
- }
- printf("Re-setting nearest from %d\n", i);
- }
-
- if (t == NULL)
- goto out;
- }
- }
-
/*
* Now we have the t node which contains the object of the
* spatial middle coordinates of the tree.
*/
if (debug) {
- printf("Relocating node %p under parent %p\n", t, parent);
+ printf("Relocating node %p (%f %f) under parent %p\n", t,
+ t->pos.x, t->pos.y, parent);
printf("There are %ld child nodes left\n", tree->children);
fflush(stdout);
}
out:
if (debug)
- printf("Now moved %d nodes, %d left\n", moved, tree->children);
+ printf("Now moved %d nodes, %ld left\n", moved, tree->children);
return moved;
}
if (moved != children) {
printf("Got %d children but %d were moved\n",
children, moved);
+ printf("nearest children left:\n");
+ for (i = 0 ; i < 4; i++)
+ if (node->child[i])
+ printf(" node %d %p, (%f %f)\n",
+ i, node->child[i],
+ node->child[i]->pos.x,
+ node->child[i]->pos.y);
fflush(stdout);
trap();
}