quad-tree

quad-tree.c

#include "quad-tree.h"
#include <math.h>
#include <stdlib.h>

struct qnode_t {
        struct point_t pt;
        struct qnode_t *children[4];
};

struct qtree_t {
        //double rangexy[2];
        struct qnode_t *head;
};

struct qnode_t *
qnode_new(struct point_t pt) {
        struct qnode_t *node = malloc(sizeof(*node));
        node->pt = pt;
        for (int i = 0; i < 4; ++i) {
                node->children[i] = NULL;
        }
        return node;
}

void 
qnode_free(struct qnode_t **node) {
        if (!node) return ;
        free(*node);
        *node = NULL;
}

struct qtree_t * 
qtree_new(void) {
        struct qtree_t *qt = malloc(sizeof(*qt));
        qt->head = NULL;
        return qt;
}

static void 
qtree_free_nodes(struct qnode_t *node) {
        if (node) {
                for (int i = 0; i < 4; ++i) {
                        if (node->children[i])
                                qtree_free_nodes(node->children[i]);
                }
                qnode_free(&node);
        }
}

void 
qtree_free(struct qtree_t **qt) {        
        if (!qt || !(*qt)) return ;
        qtree_free_nodes((*qt)->head);
        free(*qt);
        *qt = NULL;
}

static double 
point_distance(struct point_t a, struct point_t b) {
        return sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y)*(a.y - b.y));
}

/**
 * Find the location relationship. 
 * If point a is the same with o,     return 0.
 * If point a in the north-east of o, return 1
 * If point a in the north-west of o, return 2
 * If point a in the north-west of o, return 3
 * If point a in the north-west of o, return 4
 * @a point a
 * @o point o
 */
static int 
qnode_compare(struct point_t a, struct point_t o) {
        if (point_distance(a, o) < 1E-10) { /* assume 1E-10 is small enogh */
                return 0;
        }

        const int d[2][2] = {{1,2},{3,4}};
        return d[a.y < o.y][a.x < o.x];
}

int 
qtree_insert(struct qtree_t *qt, struct point_t pt) {
        if (!qt) return -1;
        if (!qt->head) {
                qt->head = qnode_new(pt);
        }
        else {
                struct qnode_t * head = qt->head;
                int d = qnode_compare(pt, head->pt);
                while (d && head->children[d-1] != NULL) {
                        head = head->children[d-1];
                        d = qnode_compare(pt, head->pt);
                }
                if (d != 0) {
                        head->children[d-1] = qnode_new(pt);
                        return 1;
                }
                return 0;
        }

        /* avoid warning */
        return 0;
}

static struct point_t 
point_nearest(struct point_t a, struct point_t b, struct point_t o) {
        return point_distance(a, o) > point_distance(b, o) ? b : a;
}

struct point_t 
qtree_nn(const struct qtree_t *qt, struct point_t pt) {
        struct qnode_t *head = qt->head;
        int d = qnode_compare(head->pt, pt);

        if (d == 0) {
                return head->pt;
        }
        
        head = head->children[d-1];
        while (head) {
                int t = qnode_compare(pt, head->pt);
                if (t == 0) {
                        return head->pt;
                }
                else if ((t+1)%4 == d-1) {
                        return point_nearest(head->pt, head->children[t-1]->pt, pt);
                }
                else {
                        head = head->children[t-1];     
                        d = t;
                }
        }

        /* avoid wanring */
        struct point_t dum;
        return dum;
}

quad-tree.h

#ifndef QUADTREE_H
#define QUADTREE_H

struct point_t {
        double x,y;
};

struct qtree_t;
struct qtree_t * qtree_new();
void qtree_free(struct qtree_t **qt);

/**
 * Insert 2D points into quatree, this could cause a really unbalanced tree.
 * @return 1 if inserted, 0 if a 'same' point exists. -1 for error.
 */
int qtree_insert(struct qtree_t *qt, struct point_t pt);

/**
 * Find the nearest point in built quad-tree.
 * @return the exact nearest point.
 */
struct point_t qtree_nn(const struct qtree_t *qt, struct point_t query);

#endif