#include <stdio.h>
#include <string.h>
#include <math.h>
#include <stdlib.h>
#include <dist.h>

#define DEBUG 	0

#define	NCACHE	1007
#define	DEPTH	4

#define	HUGE	(1.0e100)	/* the square of this number should be < HUGE_VAL */
#define	TINY	(1.0/HUGE);


#define TYPE(p)	(((p)->right == 0)?SLEAF:((p)->left==0)?SSOBJ:SNODE)

typedef struct celem	Celem;

struct celem {
	int	callnum;
	Sprim2	*p[2];
	Ssobj2	*s[2];
};

int		snoline2 = 0;

static Sdist2	dist;

static double	err, cut;
static Frame2	f1, f2, f12, f21;
static Ssobj2	*sobj1, *sobj2;
static void	*carg;

/* contact stuff */
static int	ncon;		/* number of contants so far */
static int	maxncon;	/* max number of contants */
static Sdist2	*dcon;		/* contant distances */

/* cache stuff */
static Celem	cache[NCACHE][DEPTH];
static int	callnum;	/* used to reset cache for each call */
static Celem	ccache;		/* one element first level cache */
static int	ccacher;	/* result of first level cache */

static Sobj2	*obj;

static Snode2	*nfreelist;
static Sprim2	*pfreelist;

/* for debuging */
static int	level;
#define SPACE	{int i; for(i = 0; i < level; i++) putc(' ', stderr);}
#define SDB	if (DEBUG&1) { SPACE; fprintf(stderr, 
#define SDB2	if (DEBUG&2) { SPACE; fprintf(stderr, 
#define EDB	); }


static void	cvline(Point2 l[2], Sprim2 *prim, double w1, double w2, double rmax);
static Snode2	*build_hier(Snode2 *, Point2 min, Point2 max, Point2 avg);
static void	dodist(Sobj2 *o1, Frame2 *pf1, Sobj2 *o2, Frame2 *pf2);
static void	search(Snode2 *, Snode2 *);
static void	split1(Snode2 *, Snode2 *);
static void	split2(Snode2 *, Snode2 *);
static void	crack1(Snode2 *, Snode2 *);
static void	crack2(Snode2 *, Snode2 *);
static void	p2p(Snode2 *, Snode2 *);
static void	prim2prim(Point2 l[2], Sprim2 *p1, Sprim2 *p2);
static int	compared(Sprim2 *p1, Sprim2 *p2);
static int	contain(Snode2 *, Sprim2 *prim);
static Sobj2	*oalloc2(void);
static Snode2	*nalloc2(Point2, Sprim2 *, double, Snode2 *, Snode2 *);
static Sprim2	*palloc2(Point2 *p, int n, double r, void *a);
static void	nfree2(Snode2 *p);
static void	fatal(char *s);

void
bgnsobj2(void)
{
	obj = malloc(sizeof(Sobj2));
	if (obj == 0)
		fatal("bgnsobj2: out of memory");
	obj->p = 0;
}

void
spoint2(Point2 pt, double r, void *a)
{
	Snode2 *p;
	Sprim2 *prim;

	if (!obj)
		fatal("spoint2: no current object");

	prim = palloc2(&pt, 1, r, a);
	p = nalloc2(pt, prim, r, obj->p, 0);
	obj->p = p;
}

Ssobj2 *
ssobj2(Sobj2 *o, Frame2 *f, void *a)
{
	Snode2 *p;
	Ssobj2 *s;
	Point2 pt;

	if (!obj)
		fatal("sobj2: no current object");

	s = malloc(sizeof(Ssobj2));
	if (s == 0)
		fatal("salloc2: out of memory");
	s->ignore = 0;
	s->a = a;
	s->p = o->p;
	s->f = *f;

	pt = fmap2(f, o->p->p);
	p = nalloc2(pt, o->p->prim, o->p->r, obj->p, (Snode2 *)s);

	obj->p = p;

	return s;
}

void
sline2(Point2 l[2], double r, double rmax, void *a)
{
	Sprim2 *prim;

	if (!obj)
		fatal("sline2: no current object");

	prim = palloc2(l, 2, r, a);

	cvline(l, prim, r, 0, rmax);
}

void
sbezier2(Point2 b[4], double r, double ld, double rmax, void *a)
{
	Point2 b0[4], b1[4];
	Point2 l[2];

	if (bldev2(b) <= ld) {
		l[0] = b[0];
		l[1] = b[3];
		sline2(l, r/*+ld*/, rmax, a);
	} else {
		bsubdiv2(b, b0, b1);
		sbezier2(b0, r, ld, rmax, a);
		sbezier2(b1, r, ld, rmax, a);
	}
}

void
sobjfree2(Sobj2 *o)
{
	nfree2(o->p);
	free(o);
}

	
Sobj2 *
endsobj2(void)
{
	Snode2 *p;
	int n;
	Point2 min, max, avg;

	if (!obj)
		fatal("endsobj2: no current object");

	n = 0;
	avg = point2(0,0);
	min = point2(HUGE, HUGE);	
	max = point2(-HUGE, -HUGE);

	for (p = obj->p; p; p = p->left, n++) {
		min = pmin2(min, p->p);
		max = pmax2(max, p->p);
		avg = padd2(avg, p->p);
	}

	obj->n = n;
	if (n == 0)
		return obj;

	avg = pmul2(1.0/n, avg);
			
	obj->p = build_hier(obj->p, min, max, avg);
	return obj;
}

Sdist2
sdist2(Sobj2 *o1, Frame2 *f1, Sobj2 *o2, Frame2 *f2, double e)
{

	err = e;
	cut = HUGE;
	dcon = 0;

	dodist(o1, f1, o2, f2);

	return dist;
}

Sdist2
scollide2(Sobj2 *o1, Frame2 *f1, Sobj2 *o2, Frame2 *f2)
{

	err = 0;
	cut = TINY;
	dcon = 0;

	dodist(o1, f1, o2, f2);

	return dist;
}

int	
scontact2(Sdist2 *d, int n, Sobj2 *o1, Frame2 *f1,
		Sobj2 *o2, Frame2 *f2, double c)
{

	err = 0;
	cut = c;
	dcon = d;
	maxncon = n;
	ncon = 0;

	dodist(o1, f1, o2, f2);

	return ncon;
}


/***************** local functions *********************/

static void
cvline(Point2 l[2], Sprim2 *prim, double w1, double w2, double rmax)
{
	Snode2 *p;
	double r, r2;
	int n, i;
	Point2 step, p0, p1, pt;

	r = dpp2(l[0], l[1])/2;

	r2 = sqrt(rmax*rmax - w2*w2);

	n = r/r2;
	if (n*r2 < r || n == 0)
		n++;
	r = r/n;
	r = sqrt(r*r + w2*w2) + w1;

	step = pmul2(1.0/n, psub2(l[1], l[0]));
	p0 = l[0];
	p1 = padd2(p0, step);

	for (i = 0; i < n; i++, p0 = p1, p1 = padd2(p1, step)) {
		pt = padd2(p0, pmul2(0.5, step));
		p = nalloc2(pt, prim, r, obj->p, 0);

		obj->p = p;
	}
}


static Snode2 *
build_hier(Snode2 *p, Point2 min, Point2 max, Point2 avg)
{
	Snode2 *p1, *p2, *p3;
	Point2 min1, max1, avg1;
	Point2 min2, max2, avg2;
	Point2 ctr;
	int n1, n2;
	double c, r, r1, r2, rmax;


	if (p->left == 0) {
		return p;
	}

	if (DEBUG) level++;

	avg1 = avg2 = point2(0,0);
	min1 = min2 = point2(HUGE, HUGE);	
	max1 = max2 = point2(-HUGE, -HUGE);
	p1 = p2 = 0;
	n1 = n2 = 0;
	if (peq2(min,max)) {
/* printf("split in two\n"); */
		/* split in two */ 
		while (p) {
			n1++;
			p3 = p->left;
			p->left = p1;
			p1 = p;
			if (!p3)
				break;
			n2++;
			p = p3->left;
			p3->left = p2;
			p2 = p3;
		}
		avg1 = pmul2(n1, avg);
		avg2 = pmul2(n2, avg);
		min1 = min2 = min;
		max1 = max2 = max;
	} else if (max.x-min.x > max.y-min.y) {
		c = avg.x;
		if (c <= min.x || c > max.x)
			c = (min.x+max.x)/2.0;
		if (c == min.x)
			c = max.x;

		SDB2 "split at x = %f %f\n", c, avg.x EDB

		while(p) {
			p3 = p->left;
			if (p->p.x < c) {
				p->left = p1;
				p1 = p;
				min1 = pmin2(min1, p->p);
				max1 = pmax2(max1, p->p);
				avg1 = padd2(avg1, p->p);
				n1++;
			} else {
				p->left = p2;
				p2 = p;
				min2 = pmin2(min2, p->p);
				max2 = pmax2(max2, p->p);
				avg2 = padd2(avg2, p->p);
				n2++;
			}
			p = p3;
		}
	} else {
		c = avg.y;
		if (c <= min.y || c > max.y)
			c = (min.y+max.y)/2.0;
		if (c == min.y)
			c = max.y;

		SDB2 "split at y = %f %f\n", c, avg.y EDB

		while(p) {
			p3 = p->left;
			if (p->p.y < c) {
				p->left = p1;
				p1 = p;
				min1 = pmin2(min1, p->p);
				max1 = pmax2(max1, p->p);
				avg1 = padd2(avg1, p->p);
				n1++;
			} else {
				p->left = p2;
				p2 = p;
				min2 = pmin2(min2, p->p);
				max2 = pmax2(max2, p->p);
				avg2 = padd2(avg2, p->p);
				n2++;
			}
			p = p3;
		}
	}

	SDB2 "n1 = %d n2 = %d\n", n1, n2 EDB

	if (n1 == 0)
		fatal("build_hier: n1 = 0");
	if (n2 == 0)
		fatal("build_hier: n2 = 0");
	
	avg1 = pmul2(1.0/n1, avg1);
	avg2 = pmul2(1.0/n2, avg2);

	r = 0;
	rmax = 0;
	ctr = pmid2(0.5, min, max);


	for (p = p1; p; p = p->left) {
		if (p->r > r)
			r = p->r;
		r2 = (p->p.x-ctr.x)*(p->p.x-ctr.x) +
			(p->p.y-ctr.y)*(p->p.y-ctr.y);

		/* a bit messy to avoid the sqrt */
		c = rmax-p->r;

		if (c < 0 || r2 > c*c)
			rmax = sqrt(r2) + p->r;
	}
	for (p = p2; p; p = p->left) {
		if (p->r > r)
			r = p->r;
		r2 = (p->p.x-ctr.x)*(p->p.x-ctr.x) +
			(p->p.y-ctr.y)*(p->p.y-ctr.y);

		/* a bit messy to avoid the sqrt */
		c = rmax-p->r;

		if (c < 0 || r2 > c*c)
			rmax = sqrt(r2) + p->r;
	}

	p1 = build_hier(p1, min1, max1, avg1);
	p2 = build_hier(p2, min2, max2, avg2);

	p = nalloc2(ctr, 0, 0, p1, p2);

	r = dpp2(p1->p, p2->p);
	
	if (p1->r+r <= p2->r) {
		p->r = p2->r;
		p->p = p2->p;
	} else if (p1->r >= p2->r + r) {
		p->r = p1->r;
		p->p = p1->p;
	} else {
		p->r = 0.5*(p1->r + p2->r + r);
		r1 = p->r - p1->r;
		r2 = p->r - p2->r;
		p->p = pmid2(r1/r, p1->p, p2->p);
	}
	if (p->r > rmax) {
		p->p = ctr;
		p->r = rmax;
	}

	if (p1->prim == p2->prim)
		p->prim = p1->prim;
	else
		p->prim = 0;

	if (DEBUG) level--;

	return p;
}

static void
dodist(Sobj2 *o1, Frame2 *pf1, Sobj2 *o2, Frame2 *pf2)
{
	f1 = *pf1;
	f2 = *pf2;

	finv2(&f12, &f2);
	fmul2(&f12, &f12, &f1);

	finv2(&f21, &f1);
	fmul2(&f21, &f21, &f2);

	sobj1 = sobj2 = 0;

	dist.d = HUGE;
	dist.p[0] = dist.p[1] = point2(0,0);
	dist.sobj[0] = dist.sobj[1] = 0;
	dist.f[0] = dist.f[1] = *iframe2;
	dist.prim[0] = dist.prim[1] = 0;
	dist.n = dist.npp = 0;

	/* reset cache */
	callnum++;
	if (callnum == 0) {
		/* callnum has wrapped - a lot of calls! */
		callnum = 1;
		memset(cache, 0, sizeof(cache));
	}

	/* reset first level cache */
	ccache.p[0] = 0;

	if (DEBUG) level = 0;

	SDB "------\n" EDB

	if (o1->p != 0 && o2->p != 0)
		search(o1->p, o2->p);

}

static void
search(Snode2 *p1, Snode2 *p2)
{
	int t1, t2;

	if (DEBUG) level++;

	SDB "search %d:%x %d:%x\n", TYPE(p1), p1->prim, TYPE(p2), p2->prim EDB

	if (!snoline2 && p1->prim && p2->prim && compared(p1->prim, p2->prim)) {
		SDB "matched\n" EDB
		return;
	}

	dist.n++;

	t1 = TYPE(p1);
	t2 = TYPE(p2);

	if (t1 != SLEAF && (p1->r > p2->r || t2 == SLEAF))
		if (t1 == SNODE)
			split1(p1, p2);
		else
			crack1(p1, p2);
		
	else if (t2 != SLEAF)
		if (t2 == SNODE)
			split2(p1, p2);
		else
			crack2(p1, p2);
	else
		p2p(p1, p2);

	if (DEBUG) level--;
}

static void
split1(Snode2 *p1, Snode2 *p2)
{
	Snode2 *p11, *p12;
	double r1, r2, c;
	Point2 p, pt;

	SDB "split1\n" EDB

	/* this code has been expaned out for speed */

	pt.x = f21.r[0][0]*p2->p.x + f21.r[0][1]*p2->p.y + f21.p.x;
	pt.y = f21.r[1][0]*p2->p.x + f21.r[1][1]*p2->p.y + f21.p.y;

	p11 = p1->left;
	p12 = p1->right;
		
	p.x = p11->p.x - pt.x;
	p.y = p11->p.y - pt.y;
	r1 = p.x*p.x + p.y*p.y;

	p.x = p12->p.x - pt.x;
	p.y = p12->p.y - pt.y;
	r2 = p.x*p.x + p.y*p.y;

	/* the heuristic has been modified to avoid the sqrt */

	if (r1 - (p11->r)*(p11->r) < r2 - (p12->r)*(p12->r)) {

		SDB "min1 = %f\n", sqrt(r1) - p11->r - p2->r EDB
		c = cut + p11->r + p2->r;
		if (c > 0 && r1 < c*c)
			search(p11, p2);

		SDB "min2 = %f\n", sqrt(r2) - p12->r - p2->r EDB
		c = cut + p12->r + p2->r;
		if (c > 0 && r2 < c*c)
			search(p12, p2);

	} else {

		SDB "min2 = %f\n", sqrt(r2) - p12->r - p2->r EDB
		c = cut + p12->r + p2->r;
		if (c > 0 && r2 < c*c)
			search(p12, p2);

		SDB "min1 = %f\n", sqrt(r1) - p11->r - p2->r EDB
		c = cut + p11->r + p2->r;
		if (c > 0 && r1 < c*c)
			search(p11, p2);
	}
}

static void
split2(Snode2 *p1, Snode2 *p2)
{
	Snode2 *p21, *p22;
	double r1, r2, c;
	Point2 p, pt;

	SDB "split2\n" EDB

	/* this code has been expaned out for speed */

	pt.x = f12.r[0][0]*p1->p.x + f12.r[0][1]*p1->p.y + f12.p.x;
	pt.y = f12.r[1][0]*p1->p.x + f12.r[1][1]*p1->p.y + f12.p.y;

	p21 = p2->left;
	p22 = p2->right;
		
	p.x = p21->p.x - pt.x;
	p.y = p21->p.y - pt.y;
	r1 = p.x*p.x + p.y*p.y;

	p.x = p22->p.x - pt.x;
	p.y = p22->p.y - pt.y;
	r2 = p.x*p.x + p.y*p.y;

	/* the heuristic has been modified to avoid the sqrt */

	if (r1 - (p21->r)*(p21->r) < r2 - (p22->r)*(p22->r)) {

		SDB "min1 = %f\n", sqrt(r1) - p21->r - p1->r EDB
		c = cut + p21->r + p1->r;
		if (c > 0 && r1 < c*c)
			search(p1, p21);

		SDB "min2 = %f\n", sqrt(r2) - p22->r - p1->r EDB
		c = cut + p22->r + p1->r;
		if (c > 0 && r2 < c*c)
			search(p1, p22);

	} else {

		SDB "min2 = %f\n", sqrt(r2) - p22->r - p1->r EDB
		c = cut + p22->r + p1->r;
		if (c > 0 && r2 < c*c)
			search(p1, p22);

		SDB "min1 = %f\n", sqrt(r1) - p21->r - p1->r EDB
		c = cut + p21->r + p1->r;
		if (c > 0 && r1 < c*c)
			search(p1, p21);
	}
}

static void
crack1(Snode2 *p1, Snode2 *p2)
{
	Ssobj2 *os;
	Frame2 g1, g2, g12, g21;

	SDB "crack1\n" EDB

	os = sobj1;

	sobj1 = (Ssobj2 *)p1->right;

	if (sobj1->ignore) {
		sobj1 = os;
		return;
	}

	g1 = f1;
	g2 = f2;
	g12 = f12;
	g21 = f21;

	fmul2(&f1, &f1, &sobj1->f);
	fmul2(&f12, &f12, &sobj1->f);
	finv2(&f21, &f12);

	search(sobj1->p, p2);

	f1 = g1;
	f2 = g2;
	f12 = g12;
	f21 = g21;

	sobj1 = os;
}

static void
crack2(Snode2 *p1, Snode2 *p2)
{
	Ssobj2 *os;
	Frame2 g1, g2, g12, g21;

	SDB "crack2\n" EDB

	os = sobj2;

	sobj2 = (Ssobj2 *)p2->right;

	if (sobj2->ignore) {
		sobj2 = os;
		return;
	}

	g1 = f1;
	g2 = f2;
	g12 = f12;
	g21 = f21;

	fmul2(&f2, &f2, &sobj2->f);
	fmul2(&f21, &f21, &sobj2->f);
	finv2(&f12, &f21);

	search(p1, sobj2->p);

	f1 = g1;
	f2 = g2;
	f12 = g12;
	f21 = g21;

	sobj2 = os;
}


static void
p2p(Snode2 *p1, Snode2 *p2)
{
	double r, min, r1, r2, t;
	Point2 p, lp0, lp1;
	Point2 l[2];
	Sdist2 d;

	SDB "p2p\n" EDB

	r1 = p1->r;
	r2 = p2->r;
	if (snoline2 || ((p1->prim == 0 || p1->prim->n == 1) &&
		       (p2->prim == 0 || p2->prim->n == 1))) {
		l[0] = fmap2(&f12, p1->p);
		l[1] = p2->p;
	} else {
		p = fmap2(&f12, p1->p);
		r = dpp2(p, p2->p);
		min = r - p1->r - p2->r;

		if (min >= cut)
			return;

		prim2prim(l, p1->prim, p2->prim);

		r1 = p1->prim->r;
		r2 = p2->prim->r;
	}

	p = psub2(l[1], l[0]);
	r = sqrt(p.x*p.x + p.y*p.y);

	min = r - r1 - r2;

	SDB "min = %f\n", min EDB

	if (min >= dist.d && min >= cut)
		return;
	
	if (r > 0.0) {
		lp0 = pmid2(r1/r, l[0], l[1]);
		lp1 = pmid2(r2/r, l[1], l[0]);
		/* in case of numerical problems */
		if (min > 0)
			min = dpp2(lp0, lp1);
		else
			min = -dpp2(lp0, lp1);
	} else
		lp0 = lp1 = l[0];

	d.d = min;
	d.p[0] = fmap2(&f2, lp0);
	d.p[1] = fmap2(&f2, lp1);
	d.sobj[0] = sobj1;
	d.sobj[1] = sobj2;
	d.f[0] = f1;
	d.f[1] = f2;
	d.prim[0] = p1->prim;
	d.prim[1] = p2->prim;
	d.n = dist.n;
	d.npp = dist.npp;

	if (dcon == 0) {
		if (d.d <= 0.0) {
			cut = -HUGE;
		} else {
			cut = (1.0-err)*d.d;
			if (cut <= 0)
				cut = TINY;
		}

	} else {
		/* contact case */

		if (ncon >= maxncon) {
			cut = -HUGE;
			return;
		}

		dcon[ncon] = d;
		ncon++;
	}

	dist = d;
	
	SDB "d = %f cut = %f\n", dist.d, cut EDB
}

static void
prim2prim(Point2 l[2], Sprim2 *p1, Sprim2 *p2)
{
	Point2 *q1, *q2, sp[2];
	int i, n1, n2;
	int h;
	Celem c, *cp;

	SDB "prim2prim %x %x\n", p1, p2 EDB

	dist.npp++;

	n1 = p1->n;
	q1 = sp;
	if (n1 > 2)
		fatal("prim2prim: poly1 has more than 2 verteces");
	for (i = 0; i < n1; i++)
		q1[i] = fmap2(&f12, p1->p[i]);

	n2 = p2->n;
	if (n2 > 2)
		fatal("prim2prim: poly2 has more than 2 verteces");
	q2 = p2->p;

	if (n1 == 1) {
		if (n2 == 1) {
			/* trival case - should never occur! */
			fatal("poly2poly: both are points!");
		} else
			vpl2(l, *q1, q2);
	} else {
		if (n2 == 1) {
			vpl2(l, *q2, q1);
			sp[0] = l[0];
			l[0] = l[1];
			l[1] = sp[0];
		} else
			vll2(l, q1, q2);
	}

	/* update hash table */

	if (p1 < p2 || p1 == p2 && sobj1 < sobj2) {
		c.p[0] = p1;
		c.p[1] = p2;
		c.s[0] = sobj1;
		c.s[1] = sobj2;
	} else {
		c.p[1] = p1;
		c.p[0] = p2;
		c.s[1] = sobj1;
		c.s[0] = sobj2;
	}
	c.callnum = callnum;

	h = (int)c.p[0];
	h = (int)c.p[1] + h*47;
	h = (int)c.s[0] + h*47;
	h = (int)c.s[1] + h*47;

	h = h % NCACHE;
	if (h < 0)
		h += NCACHE;

	cp = cache[h];

	/* shift entries down one */

	for (i = DEPTH-1; i > 0; i--)
		cp[i] = cp[i-1];
	
	cp[0] = c;

	ccache = c;
	ccacher = 1;
}

static int
compared(Sprim2 *p1, Sprim2 *p2)
{
	int h, i;
	Celem c, *cp;

	SDB "compared %x:%x %x:%x\n", sobj1, p1, sobj2, p2 EDB

	if (p1 < p2 || p1 == p2 && sobj1 < sobj2) {
		c.p[0] = p1;
		c.p[1] = p2;
		c.s[0] = sobj1;
		c.s[1] = sobj2;
	} else {
		c.p[1] = p1;
		c.p[0] = p2;
		c.s[1] = sobj1;
		c.s[0] = sobj2;
	}

	/* first level cache */
	if (c.s[0] == ccache.s[0] &&
	    c.s[1] == ccache.s[1] &&
	    c.p[0] == ccache.p[0] &&
	    c.p[1] == ccache.p[1])
		return ccacher;

	h = (int)c.p[0];
	h = (int)c.p[1] + h*47;
	h = (int)c.s[0] + h*47;
	h = (int)c.s[1] + h*47;

	h = h % NCACHE;
	if (h < 0)
		h += NCACHE;

	cp = cache[h];

	ccacher = 0;
	for (i = 0; i < DEPTH && cp->callnum == callnum; i++, cp++)
		if (c.s[0] == cp->s[0] &&
		    c.s[1] == cp->s[1] &&
		    c.p[0] == cp->p[0] &&
		    c.p[1] == cp->p[1]) {
			ccacher = 1;
			break;
		}

	ccache = c;

	return ccacher;
}


static int
contain(Snode2 *p, Sprim2 *prim)
{
	if (p == 0)
		return 0;

	if (p->prim == prim)
		return 1;

	return contain(p->left, prim) | contain(p->right, prim);
}



static Snode2 *
nalloc2(Point2 pt, Sprim2 *prim, double r, Snode2 *left, Snode2 *right)
{
	int i;
	Snode2 *p;

	if (nfreelist) {
		p = nfreelist;
		nfreelist = p->left;
	} else {
		p = malloc(100*sizeof(Snode2));
		if (p == 0)
			fatal("nalloc2: out of memory");
		for (i = 0; i < 99; i++, p++) {
			p->left = nfreelist;
			nfreelist = p;
		}
	}
	p->p = pt;
	p->r = r;
	p->prim = prim;
	if (right == 0)
		prim->ref++;
	p->left = left;
	p->right = right;
	return p;
}

static Sprim2 *
palloc2(Point2 *pt, int n, double r, void *a)
{
	int i;
	Sprim2 *p;

	if (pfreelist) {
		p = pfreelist;
		pfreelist = p->a;
	} else {
		p = malloc(100*sizeof(Sprim2));
		if (p == 0)
			fatal("palloc2: out of memory");
		for (i = 0; i < 99; i++, p++) {
			p->a = pfreelist;
			pfreelist = p;
		}
	}

	p->ref = 0;
	p->p = malloc(n*sizeof(Point2));
	if (p->p == 0)
		fatal("palloc2: out of memory");
	memcpy(p->p, pt, n*sizeof(Point2));
	p->n = n;
	p->r = r;
	p->a = a;
		
	return p;
}

static void
nfree2(Snode2 *p)
{
	if (p == 0)
		return;

	switch(TYPE(p)) {

	case SSOBJ:
		free((Ssobj2*)p->right);
		break;

	case SNODE:
		nfree2(p->left);
		nfree2(p->right);
		break;

	case SLEAF:
		if (--p->prim->ref <= 0) {
			free(p->prim->p);
			p->prim->a = pfreelist;
			pfreelist = p->prim;
		}
		break;

	}
	p->left = nfreelist;
	nfreelist = p;
}

static void
fatal(char *s)
{
	fprintf(stderr, "%s\n", s);
	exit(1);
}