#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)
#define PA(p)	((p)->prim?(p)->prim->a:0)

typedef struct celem	Celem;

struct celem {
	int	callnum;
	Sprim3	*p[2];
	Ssobj3	*s[2];
};

int		snopoly3 = 0;

static Sdist3	dist;

static double	err, cut;
static Frame3	f1, f2, f12, f21;
static Ssobj3	*sobj1, *sobj2;
static void	*carg;

/* contact stuff */
static int	ncon;		/* number of contants so far */
static int	maxncon;	/* max number of contants */
static Sdist3	*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 Sobj3	*obj;

static Snode3	*nfreelist;
static Sprim3	*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(Point3 l[2], Sprim3 *prim, double w1, double w2, double rmax);
static void	cvtri(Point3 t[3], Sprim3 *prim, double width, double rmax);
static void	cvpoly(Point3 *p, int n, Sprim3 *prim, double width, double rmax);
static void	cvstrip(int c, Point3 p0, Point3 p1, Point3 q0, Point3 q1,
			Sprim3 *prim, double width, double rmax);
static Snode3	*build_hier(Snode3 *, Point3 min, Point3 max, Point3 avg);
static void	dodist(Sobj3 *o1, Frame3 *pf1, Sobj3 *o2, Frame3 *pf2);
static void	search(Snode3 *, Snode3 *);
static void	split1(Snode3 *, Snode3 *);
static void	split2(Snode3 *, Snode3 *);
static void	crack1(Snode3 *, Snode3 *);
static void	crack2(Snode3 *, Snode3 *);
static void	p2p(Snode3 *, Snode3 *);
static void	prim2prim(Point3 l[2], Sprim3 *p1, Sprim3 *p2);
static int	compared(Sprim3 *p1, Sprim3 *p2);
static int	contain(Snode3 *, Sprim3 *prim);
static Sobj3	*oalloc3(void);
static Snode3	*nalloc3(Point3, Sprim3 *, double, Snode3 *, Snode3 *);
static Sprim3	*palloc3(Point3 *p, int n, double r, void *a);
static void	nfree3(Snode3 *p);
static void	fatal(char *s);

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

void
spoint3(Point3 pt, double r, void *a)
{
	Snode3 *p;
	Sprim3 *prim;

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

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

Ssobj3 *
ssobj3(Sobj3 *o, Frame3 *f, void *a)
{
	Snode3 *p;
	Ssobj3 *s;
	Point3 pt;

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

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

	pt = fmap3(f, o->p->p);
	p = nalloc3(pt, o->p->prim, o->p->r, obj->p, (Snode3 *)s);

	obj->p = p;

	return s;
}

void
sline3(Point3 l[2], double r, double rmax, void *a)
{
	Sprim3 *prim;

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

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

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

void
stri3(Point3 p[3], double r, double rmax, void *a)
{
	Sprim3 *prim;

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

	prim = palloc3(p, 3, r, a);

	cvtri(p, prim, r, rmax);
}

void
spoly3(Point3 *p, int n, double r, double rmax, void *a)
{
	Sprim3 *prim;

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

	switch(n) {
	
	case 0:
		break;
	case 1:
		spoint3(p[0], r, a);
		break;
	case 2:
		sline3(p, r, rmax, a);
		break;
	case 3:
		stri3(p, r, rmax, a);
		break;
	default:
		prim = palloc3(p, n, r, a);
		cvpoly(p, n, prim, r, rmax);
		break;
	}
}

void
sobjfree3(Sobj3 *o)
{
	nfree3(o->p);
	free(o);
}

	
Sobj3 *
endsobj3(void)
{
	Snode3 *p;
	int n;
	Point3 min, max, avg;

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

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

	for (p = obj->p; p; p = p->left, n++) {
		min = pmin3(min, p->p);
		max = pmax3(max, p->p);
		avg = padd3(avg, p->p);
	}

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

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

Sdist3
sdist3(Sobj3 *o1, Frame3 *f1, Sobj3 *o2, Frame3 *f2, double e)
{

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

	dodist(o1, f1, o2, f2);

	return dist;
}

Sdist3
scollide3(Sobj3 *o1, Frame3 *f1, Sobj3 *o2, Frame3 *f2)
{

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

	dodist(o1, f1, o2, f2);

	return dist;
}

int	
scontact3(Sdist3 *d, int n, Sobj3 *o1, Frame3 *f1,
		Sobj3 *o2, Frame3 *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(Point3 l[2], Sprim3 *prim, double w1, double w2, double rmax)
{
	Snode3 *p;
	double r, r2;
	int n, i;
	Point3 step, p0, p1, pt;

	r = dpp3(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 = pmul3(1.0/n, psub3(l[1], l[0]));
	p0 = l[0];
	p1 = padd3(p0, step);

	for (i = 0; i < n; i++, p0 = p1, p1 = padd3(p1, step)) {
		pt = padd3(p0, pmul3(0.5, step));
		p = nalloc3(pt, prim, r, obj->p, 0);

		obj->p = p;
	}
}

static void
cvtri(Point3 t[3], Sprim3 *prim, double width, double rmax)
{
	Snode3 *p;
	Point3 e[3], c;
	double r00, r11, r22, r02, den, u, v, r;
	Point3 l[2];
	Point3 t0[3], t1[3];

	if (!obj)
		fatal("stri3: no current object");
	/* find center point */

	e[0] = psub3(t[1], t[0]);
	e[1] = psub3(t[2], t[1]);
	e[2] = psub3(t[0], t[2]);

	r00 = pdot3(e[0], e[0]);
	r11 = pdot3(e[1], e[1]);
	r22 = pdot3(e[2], e[2]);
	r02 = pdot3(e[0], e[2]);

	den = r00*r22 - r02*r02;

	if (den == 0) {
		/* colinear */

		if (r00 > r11 && r00 > r22) {
			l[0] = t[0];
			l[1] = t[1];
		} else if (r11 > r22) {
			l[0] = t[1];
			l[1] = t[2];
		} else {
			l[0] = t[2];
			l[1] = t[0];
		}
		cvline(l, prim, width, 0, rmax);
		return;
	}

	u = 0.5*(r22*r02+r22*r00)/den;
	v = 0.5*(r00*r02+r00*r22)/den;

	/* will centroid be in the triangle */
	if (u >= 0 && v >= 0 && u + v <= 1) {
		c.x = t[0].x + u*e[0].x - v*e[2].x;
		c.y = t[0].y + u*e[0].y - v*e[2].y;
		c.z = t[0].z + u*e[0].z - v*e[2].z;
		r = dpp3(c, t[0]);
	} else {
		if (r00 > r11 && r00 > r22) {
			c.x = 0.5*(t[0].x + t[1].x);
			c.y = 0.5*(t[0].y + t[1].y);
			c.z = 0.5*(t[0].z + t[1].z);
			r = dpp3(c, t[0]);
		} else if (r11 > r22) {
			c.x = 0.5*(t[1].x + t[2].x);
			c.y = 0.5*(t[1].y + t[2].y);
			c.z = 0.5*(t[1].z + t[2].z);
			r = dpp3(c, t[1]);
		} else {	
			c.x = 0.5*(t[2].x + t[0].x);
			c.y = 0.5*(t[2].y + t[0].y);
			c.z = 0.5*(t[2].z + t[0].z);
			r = dpp3(c, t[2]);
		}
	}

	if (r > rmax) {
		cvpoly(t, 3, prim, width, rmax);
	} else if (r > rmax) {
		/* divide into two */
		if (r00 > r11 && r00 > r22) {
			t0[0] = t[0];
			t1[0] = t[1];
			t0[1].x = t1[1].x = t[0].x + 0.5*e[0].x;
			t0[1].y = t1[1].y = t[0].y + 0.5*e[0].y;
			t0[1].z = t1[1].z = t[0].z + 0.5*e[0].z;
			t0[2] = t1[2] = t[2];
		} else if (r11 > r22) {
			t0[0] = t[1];
			t1[0] = t[2];
			t0[1].x = t1[1].x = t[1].x + 0.5*e[1].x;
			t0[1].y = t1[1].y = t[1].y + 0.5*e[1].y;
			t0[1].z = t1[1].z = t[1].z + 0.5*e[1].z;
			t0[2] = t1[2] = t[0];
		} else {
			t0[0] = t[2];
			t1[0] = t[0];
			t0[1].x = t1[1].x = t[2].x + 0.5*e[2].x;
			t0[1].y = t1[1].y = t[2].y + 0.5*e[2].y;
			t0[1].z = t1[1].z = t[2].z + 0.5*e[2].z;
			t0[2] = t1[2] = t[1];
		}


		cvtri(t0, prim, width, rmax);
		cvtri(t1, prim, width, rmax);
	} else {
		p = nalloc3(c, prim, r+width, obj->p, 0);
		obj->p = p;
	}
}

static void
cvpoly(Point3 *pt, int n, Sprim3 *prim, double width, double rmax)
{
	int i, i0, i1, c;
	Point3 p0, p1, q0, q1;
	Point3 avg, q, min, max;
	double r, r2;
	Snode3 *p;
	double s;

	if (n < 2)
		fatal("cvpoly: small n");

	/* find centroid to see if poly will fit in one sphere */
	avg = point3(0,0,0);
	min = point3(HUGE, HUGE, HUGE);
	max = point3(-HUGE, -HUGE, -HUGE);
	for (i = 0; i < n; i++) {
		avg = padd3(avg, pt[i]);
		min = pmin3(min, pt[i]);
		max = pmax3(max, pt[i]);
	}
	avg = pmul3(1.0/n, avg);
	r = 0;
	for (i = 0; i < n; i++) {
		q = psub3(pt[i], avg);
		r2 = pdot3(q, q);
		if (r2 > r)
			r = r2;
	}
	if (r <= rmax*rmax) {
		/* easy case */
		p = nalloc3(avg, prim, sqrt(r), obj->p, 0);
		obj->p = p;
		
		return;
	}

	max = psub3(max, min);

	if (max.x > max.y && max.x > max.z)
		c = 0;	/* x cooord */
	else if (max.y > max.z)
		c = 1;	/* y coord */
	else
		c = 2;	/* z coord */

	s = HUGE;
	for (i = 0; i < n; i++) {
		if (PC(pt[i],c) < s) {
			s = PC(pt[i],c);
			i0 = i;
		}
	}
	i1 = i0;
	p0 = p1 = pt[i0];
	if (--i0 < 0)
		i0 = n-1;
	if (++i1 >= n)
		i1 = 0;
	for(;;) {
		q0 = pt[i0];
		q1 = pt[i1];
		if (PC(q0,c) < PC(q1,c)) {
			q1 = pmid3((PC(q0,c)-s)/(PC(q1,c)-s), p1, q1);
			cvstrip(c, p0, p1, q0, q1, prim, width, rmax);

			s = PC(q0,c);
			if (--i0 < 0)
				i0 = n-1;
		} else if (PC(q0,c) > PC(q1,c)) {

			q0 = pmid3((PC(q1,c)-s)/(PC(q0,c)-s), p0, q0);
			cvstrip(c, p0, p1, q0, q1, prim, width, rmax);

			s = PC(q1,c);
			if (++i1 >= n)
				i1 = 0;
		} else {
			cvstrip(c, p0, p1, q0, q1, prim, width, rmax);

			if (i0 == i1)
				break;
			s = PC(q1,c);
			if (--i0 < 0)
				i0 = n-1;
			if (i0 == i1)
				break;
			if (++i1 >= n)
				i1 = 0;
		}


		p0 = q0;
		p1 = q1;
	}
}

static void
cvstrip(int c, Point3 p0, Point3 p1, Point3 q0, Point3 q1,
		Sprim3 *prim, double width, double rmax)
{
	Point3 u, e0, e1;
	double r, w2;
	int n, i;
	Point3 l[2];

	u = psub3(p1, p0);
	r = pabs3(u);
	if (r == 0) {
		u = p0;
		p0 = q0;
		q0 = u;

		u = p1;
		p1 = q1;
		q1 = u;

		u = psub3(p1, p0);
		r = pabs3(u);

		if (r == 0) {
			l[0] = p0;
			l[1] = q0;
			cvline(l, prim, width, 0,  rmax);
			return;
		}
	}
	u = pmul3(1/r, u);

	w2 = 0.70711*rmax;	
	r = fabs((PC(q0,c)-PC(p0,c))/2);
	if (r == 0)
		return;
	n = r/w2;
	if (w2*n < r)
		n++;
	w2 = r/n;
	
	r = pdot3(u, psub3(q0, p0));
	if (r < 0)
		r = -r;
	r /= 2*n;
	e0 = pmul3(-r, u);

	r = pdot3(u, psub3(q1, p1));
	if (r < 0)
		r = -r;
	r /= 2*n;
	e1 = pmul3(r, u);
	for (i = 0; i < n; i++) {
		r = (i+0.5)/n;
		l[0] = padd3(pmid3(r, p0, q0), e0);
		l[1] = padd3(pmid3(r, p1, q1), e1);

		cvline(l, prim, width, w2,  rmax);
	}
}


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


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

	if (DEBUG) level++;

	avg1 = avg2 = point3(0,0,0);
	min1 = min2 = point3(HUGE, HUGE, HUGE);	
	max1 = max2 = point3(-HUGE, -HUGE, -HUGE);
	p1 = p2 = 0;
	n1 = n2 = 0;
	if (peq3(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 = pmul3(n1, avg);
		avg2 = pmul3(n2, avg);
		min1 = min2 = min;
		max1 = max2 = max;
	} else if (max.x-min.x > max.y-min.y &&
		   max.x-min.x > max.z-min.z) {
		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 = pmin3(min1, p->p);
				max1 = pmax3(max1, p->p);
				avg1 = padd3(avg1, p->p);
				n1++;
			} else {
				p->left = p2;
				p2 = p;
				min2 = pmin3(min2, p->p);
				max2 = pmax3(max2, p->p);
				avg2 = padd3(avg2, p->p);
				n2++;
			}
			p = p3;
		}
	} else if (max.y-min.y > max.z-min.z){
		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 = pmin3(min1, p->p);
				max1 = pmax3(max1, p->p);
				avg1 = padd3(avg1, p->p);
				n1++;
			} else {
				p->left = p2;
				p2 = p;
				min2 = pmin3(min2, p->p);
				max2 = pmax3(max2, p->p);
				avg2 = padd3(avg2, p->p);
				n2++;
			}
			p = p3;
		}
	} else {
		c = avg.z;
		if (c <= min.z || c > max.z)
			c = (min.z+max.z)/2.0;
		if (c == min.z)
			c = max.z;

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

		while(p) {
			p3 = p->left;
			if (p->p.z < c) {
				p->left = p1;
				p1 = p;
				min1 = pmin3(min1, p->p);
				max1 = pmax3(max1, p->p);
				avg1 = padd3(avg1, p->p);
				n1++;
			} else {
				p->left = p2;
				p2 = p;
				min2 = pmin3(min2, p->p);
				max2 = pmax3(max2, p->p);
				avg2 = padd3(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 = pmul3(1.0/n1, avg1);
	avg2 = pmul3(1.0/n2, avg2);

	r = 0;
	rmax = 0;
	ctr = pmid3(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) +
			(p->p.z-ctr.z)*(p->p.z-ctr.z);

		/* 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) +
			(p->p.z-ctr.z)*(p->p.z-ctr.z);

		/* 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 = nalloc3(ctr, 0, 0, p1, p2);

	r = dpp3(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 = pmid3(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(Sobj3 *o1, Frame3 *pf1, Sobj3 *o2, Frame3 *pf2)
{
	f1 = *pf1;
	f2 = *pf2;

	finv3(&f12, &f2);
	fmul3(&f12, &f12, &f1);

	finv3(&f21, &f1);
	fmul3(&f21, &f21, &f2);

	sobj1 = sobj2 = 0;

	dist.d = HUGE;
	dist.p[0] = dist.p[1] = point3(0,0,0);
	dist.sobj[0] = dist.sobj[1] = 0;
	dist.f[0] = dist.f[1] = *iframe3;
	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(Snode3 *p1, Snode3 *p2)
{
	int t1, t2;

	if (DEBUG) level++;

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


	if (!snopoly3 && 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(Snode3 *p1, Snode3 *p2)
{
	Snode3 *p11, *p12;
	double r1, r2, c;
	Point3 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.r[0][2]*p2->p.z + f21.p.x;
	pt.y = f21.r[1][0]*p2->p.x + f21.r[1][1]*p2->p.y +
		f21.r[1][2]*p2->p.z + f21.p.y;
	pt.z = f21.r[2][0]*p2->p.x + f21.r[2][1]*p2->p.y +
		f21.r[2][2]*p2->p.z + f21.p.z;

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

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

	/* 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(Snode3 *p1, Snode3 *p2)
{
	Snode3 *p21, *p22;
	double r1, r2, c;
	Point3 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.r[0][2]*p1->p.z + f12.p.x;
	pt.y = f12.r[1][0]*p1->p.x + f12.r[1][1]*p1->p.y +
		f12.r[1][2]*p1->p.z + f12.p.y;
	pt.z = f12.r[2][0]*p1->p.x + f12.r[2][1]*p1->p.y +
		f12.r[2][2]*p1->p.z + f12.p.z;

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

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

	/* 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(Snode3 *p1, Snode3 *p2)
{
	Ssobj3 *os;
	Frame3 g1, g2, g12, g21;

	SDB "crack1\n" EDB

	os = sobj1;

	sobj1 = (Ssobj3 *)p1->right;

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

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

	fmul3(&f1, &f1, &sobj1->f);
	fmul3(&f12, &f12, &sobj1->f);
	finv3(&f21, &f12);

	search(sobj1->p, p2);

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

	sobj1 = os;
}

static void
crack2(Snode3 *p1, Snode3 *p2)
{
	Ssobj3 *os;
	Frame3 g1, g2, g12, g21;

	SDB "crack2\n" EDB

	os = sobj2;

	sobj2 = (Ssobj3 *)p2->right;

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

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

	fmul3(&f2, &f2, &sobj2->f);
	fmul3(&f21, &f21, &sobj2->f);
	finv3(&f12, &f21);

	search(p1, sobj2->p);

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

	sobj2 = os;
}


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

	SDB "p2p\n" EDB

	r1 = p1->r;
	r2 = p2->r;
	if (snopoly3 || ((p1->prim == 0 || p1->prim->n == 1) &&
		       (p2->prim == 0 || p2->prim->n == 1))) {
		l[0] = fmap3(&f12, p1->p);
		l[1] = p2->p;
	} else {
		p = fmap3(&f12, p1->p);
		r = dpp3(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 = psub3(l[1], l[0]);
	r = sqrt(p.x*p.x + p.y*p.y + p.z*p.z);

	min = r - r1 - r2;

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

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

	d.d = min;
	d.p[0] = fmap3(&f2, lp0);
	d.p[1] = fmap3(&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(Point3 l[2], Sprim3 *p1, Sprim3 *p2)
{
	Point3 *q1, *q2, sp[100];
	int i, n1, n2;
	int h;
	Celem c, *cp;

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

	dist.npp++;

	n1 = p1->n;
	if (n1 <= 100)
		q1 = sp;
	else
		q1 = malloc(n1 * sizeof(Point3));
	for (i = 0; i < n1; i++)
		q1[i] = fmap3(&f12, p1->p[i]);

	n2 = p2->n;
	q2 = p2->p;

	vdist3(l, q1, n1, q2, n2);

	if (q1 != sp)
		free(q1);

	/* 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(Sprim3 *p1, Sprim3 *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(Snode3 *p, Sprim3 *prim)
{
	if (p == 0)
		return 0;

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

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



static Snode3 *
nalloc3(Point3 pt, Sprim3 *prim, double r, Snode3 *left, Snode3 *right)
{
	int i;
	Snode3 *p;

	if (nfreelist) {
		p = nfreelist;
		nfreelist = p->left;
	} else {
		p = malloc(100*sizeof(Snode3));
		if (p == 0)
			fatal("nalloc3: 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 Sprim3 *
palloc3(Point3 *pt, int n, double r, void *a)
{
	int i;
	Sprim3 *p;

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

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

static void
nfree3(Snode3 *p)
{
	if (p == 0)
		return;

	switch(TYPE(p)) {

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

	case SNODE:
		nfree3(p->left);
		nfree3(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);
}