cell/source/quadtree.c

/*
  quadtree.c
  2014 JSK (kutani@projectkutani.com)

  Part of the Panic Panic project.

  Released to the public domain. See LICENSE for details.
*/
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <stdint.h>

#include "aabb.h"

/// Default node size cap
#define QTREE_STDCAP 4

/// A function pointer def for determining if an element exists in a range
typedef int (*qtree_fnc)(void *ptr, aabb *range);

/// Quadtree node
typedef struct qnode {
	uint16_t cnt;     ///< Number of elements in this node
	aabb bound;       ///< Area this node covers
	void **elist;     ///< List of element pointers
	struct qnode *nw; ///< NW quadrant of this node
	struct qnode *ne; ///< NE quadrant of this node
	struct qnode *sw; ///< SW quadrant of this node
	struct qnode *se; ///< SE quadrant of this node
} qnode;

/// Quadtree container
typedef struct _qtree {
	uint16_t maxnodecap; ///< Maximum element count per node
	qnode *root;         ///< Root node
	qtree_fnc cmpfnc;    ///< Element range compare function pointer
} _qtree;

typedef struct _qtree* qtree;

/// Simple container for returning found elements
typedef struct retlist {
	uint32_t cnt; ///< Number of elements found
	aabb range;   ///< Range to use for searching
	void **list;  ///< Array of pointers to found elements
} retlist;

static void
retlist_add(retlist *r, void *p) {
	r->list = realloc(r->list, sizeof(void*)*(r->cnt+1));
	r->list[r->cnt] = p;
	r->cnt++;
}

static uint16_t
qtree_getMaxNodeCnt(qtree q) {
	uint16_t r;
	r = q->maxnodecap;
	return r;
}

static qnode*
qnode_new(qtree p, float x, float y, float hW, float hH) {
	qnode *q = malloc(sizeof(qnode));
	memset(q, 0, sizeof(qnode));
	q->bound.center.x = x;
	q->bound.center.y = y;
	q->bound.dims.w = hW;
	q->bound.dims.h = hH;

	return q;
}

static void
qnode_free(qtree q, qnode *qn) {

	if(qn->cnt)
		free(qn->elist);

	qn->cnt = 0;

	if(qn->nw) {
		qnode_free(q, qn->nw);
		qnode_free(q, qn->ne);
		qnode_free(q, qn->sw);
		qnode_free(q, qn->se);
	}

	free(qn);
}

static void
add(qnode *q, void *p) {
	q->elist = realloc(q->elist, sizeof(void*)*(q->cnt+1));
	q->elist[q->cnt] = p;
	q->cnt++;
}

static void
drop(qnode *q, uint16_t idx) {
	void **narry = malloc(sizeof(void*)*(q->cnt-1));
	
	// This is a little (lot) ugly; a pair of memcpy's would be
	// better, but I had some problems with it
	for(uint16_t i=0,skip=0; i<q->cnt; i++) {
		if(i == idx) { skip++; continue; }
		narry[i-skip] = q->elist[i];
	}
	
	void **old = q->elist;
	q->elist = narry;
	free(old);
	q->cnt--;
}

static void
subdivide(qtree p, qnode *q) {
	float cx = q->bound.center.x;
	float cy = q->bound.center.y;
	float hw = q->bound.dims.w/2;
	float hh = q->bound.dims.h/2;

	q->nw = qnode_new(p, cx-hw, cy-hh, hw, hh);
	q->ne = qnode_new(p, cx+hw, cy-hh, hw, hh);
	q->sw = qnode_new(p, cx-hw, cy+hh, hw, hh);
	q->se = qnode_new(p, cx+hw, cy+hh, hw, hh);
}

static int
qnode_insert(qtree q, qnode *qn, void *ptr) {
	int ret = 0;
	
	if(! (q->cmpfnc)(ptr, &qn->bound))
		goto QN_INS_EXIT;

	if(qn->cnt < qtree_getMaxNodeCnt(q)) {
		add(qn, ptr);
		ret = 1;
		goto QN_INS_EXIT;
	}

	if(! qn->nw)
		subdivide(q, qn);

	if(qnode_insert(q,qn->nw,ptr))
		return 1;
	else if(qnode_insert(q,qn->ne,ptr))
		return 1;
	else if(qnode_insert(q,qn->sw,ptr))
		return 1;
	else if(qnode_insert(q,qn->se,ptr))
		return 1;

  QN_INS_EXIT:
	return ret;
}

static void* 
qnode_remove(qtree q, qnode *qn, void *ptr) {
	if(qn->cnt) {
		for(uint16_t i=0; i<qn->cnt; i++) {
			if(qn->elist[i] == ptr) {
				drop(qn, i);
				ptr = NULL;
				goto QN_REM_EXIT;
			}
		}
	}

	if(! qn->nw)
		return NULL;

	if(qnode_remove(q, qn->nw, ptr)) return ptr;
	if(qnode_remove(q, qn->ne, ptr)) return ptr;
	if(qnode_remove(q, qn->sw, ptr)) return ptr;
	if(qnode_remove(q, qn->se, ptr)) return ptr;

	return NULL;
	QN_REM_EXIT:
	return ptr;
}

static void
qnode_getInRange(qtree q, qnode *qn, retlist *r) {
	if(qn->cnt) {
		if(! aabb_intersects(&qn->bound, &r->range))
			goto QN_GET_EXIT;

		for(uint16_t i=0; i<qn->cnt; i++)
			if((q->cmpfnc)(qn->elist[i], &r->range))
				retlist_add(r, qn->elist[i]);
	}

	if(! qn->nw)
		goto QN_GET_EXIT;

	qnode_getInRange(q, qn->nw, r);
	qnode_getInRange(q, qn->ne, r);
	qnode_getInRange(q, qn->sw, r);
	qnode_getInRange(q, qn->se, r);

	QN_GET_EXIT:
	return;
}

/* exports */

qtree
qtree_new(float x, float y, float w, float h, qtree_fnc fnc) {
	qtree q = malloc(sizeof(_qtree));
	memset(q, 0, sizeof(_qtree));

	q->maxnodecap = QTREE_STDCAP;
	q->cmpfnc = fnc;
	q->root = qnode_new(q, x+(w/2),y+(h/2),w/2,h/2);

	return q;
}

void
qtree_destroy(qtree q) {
	void *m;
	if(q->root) qnode_free(q, q->root);
	
	memset(q, 0, sizeof(_qtree));
	
	free(q);
}

void
qtree_insert(qtree q, void *ptr) {
	qnode_insert(q, q->root, ptr);
}

void
qtree_remove(qtree q, void *ptr) {
	qnode_remove(q, q->root, ptr);
}

void
qtree_setMaxNodeCnt(qtree q, uint16_t cnt) {
	q->maxnodecap = cnt || 1;
}

void
qtree_clear(qtree q) {
	float x = q->root->bound.center.x;
	float y = q->root->bound.center.y;
	float w = q->root->bound.dims.w;
	float h = q->root->bound.dims.h;
	qnode *qn = q->root;
	
	q->root = qnode_new(q, x, y, w, h);

	qnode_free(q, qn);
}

void**
qtree_findInArea(qtree q, float x, float y, float w, float h, uint32_t *cnt) {
	float hw = w/2;
	float hh = h/2;

	retlist ret;
	memset(&ret, 0, sizeof(retlist));

	ret.range.center.x = x+hw;
	ret.range.center.y = y+hh;
	ret.range.dims.w = hw;
	ret.range.dims.h = hh;

	qnode_getInRange(q, q->root, &ret);

	*cnt = ret.cnt;
	return ret.list;
}