#include "polyPairTest.h"
#include "matrix.h"
#include "database.h"	/* for SQR define, and normalize() */

static ConNode *buildContactList(ConNode *, ConNode *);
static ConNode *edgesVsPoly(TuppleArray, PolyNode *, Tupple *, ConNode *,
					Status *, Transform *);
static ConNode *calcConNodeInfo(ConNode *, Ulong, Ulong, int, int);
static ConNode *edgeVsEdges(Tupple, Tupple, PolyNode *, Tupple *, 
					ConNode *, Boolean *, Transform *);
static Boolean pointInBoundary(Tupple, PolyNode *, Tupple *);
static Real buildParaLine(Tupple, Tupple, Tupple *);

#ifdef STATS
extern Ulong polyTestCount;
#endif

/*
 * Given a PairNode list, checks each pair and returns intersection, contact
 * (as well as the contact or intersectin information), or nothing for no 
 * intersection
 *
 * Params	p		PairNode list of polygons to be checked
 *			obja	object A
 *			objb	object B
 *			tab		transform from A to B frame
 *			tba		transform from B to A frame
 *			cn		contact list to add to.  Contains the contact information
 *					between other objects (if any so far)
 *			s		used to return status
 *
 * Globals	none
 *
 * Returns	contact list and status
 */
ConNode *polyPairTest(p, obja, objb, tab, tba, cn, s)
PairNode *p;
ObjNode *obja, *objb;
Transform *tab, *tba;
ConNode *cn;
Status *s;
{
	int i, preva, pa, pb, nv, *v;
	PairNode *pt = p;
	ConNode *ctemp = NULL;
	ConNode *cp;
	Tupple *verts;
	TuppleArray abVerts, baVerts;
	PolyNode *pp;
	Status stemp = Nothing;

	if (p == NULL) {
		return cn;
	}

	abVerts.total = 0; abVerts.v = NULL;
	baVerts.total = 0; baVerts.v = NULL;
	*s = Nothing;

	/* Because the PairNode list is ordered, there is a good chance that the
	 * the current polygon in consideration of object A, is the same as in the 
	 * previous iteration, so keep track of previous A polygon and save on
	 * recalculating its edges in B's ref frame
	 */
	preva = -1;

	do {

		#ifdef STATS
		polyTestCount++;
		#endif

		if (pt->prev != NULL)
			free(pt->prev);
		pt->prev = NULL;
		ctemp = NULL;

		pa = pt->a; pb = pt->b;

		pp = &(obja->polys.p[pa]);
		v = pp->vertices; nv = pp->numVertices; verts = obja->va.v;
		if (pa != preva) {
			/* current polygon from A is different from the previous
			 * step so transform each vertex of A into B's ref frame
			 */
			
			if (abVerts.v != NULL)
				free (abVerts.v); /* free previous data */
			
			abVerts.total = nv;
			if ((abVerts.v=(Tupple *)malloc(sizeof(Tupple)*nv)) == NULL) {
				fprintf(stderr, "Error: couldn't malloc Tupple Array\n");
				exit(1);
			}

			for (i=0; i<nv; i++)
				applyTransform(tab, verts[v[i]], &(abVerts.v[i]));

			preva = pa;
		}

		/* have the vertices of polygon from A transformed into B's ref frame
		 * so determine the intersection or contact points and lines, if any 
		 * (building up of the contact info in ctemp).
		 */
		ctemp = edgesVsPoly(abVerts, &(objb->polys.p[pb]), objb->va.v, ctemp, 
					&stemp, objb->loc2glob);
		if (stemp == Intersection) {
			*s = Intersection;
		} else if (stemp == Contact && *s != Intersection) {
			*s = Contact;
		}
		/* transform the vertices of polygon from B into A's ref frame */
		pp = &(objb->polys.p[pb]);
		v = pp->vertices; nv = pp->numVertices; verts = objb->va.v;
		if (baVerts.v != NULL)
			free (baVerts.v);
		
		baVerts.total = nv;
		if ((baVerts.v=(Tupple *)malloc(sizeof(Tupple)*nv)) == NULL) {
			fprintf(stderr, "Error: couldn't malloc Tupple Array\n");
			exit(1);
		}
		for (i=0; i<nv; i++)
			applyTransform(tba, verts[v[i]], &(baVerts.v[i]));
		
		/* have the vertices of polygon from B transformed into A's ref frame
		 * so determine the intersection or contact points and lines, if any 
		 * (building up of the contact info in ctemp).
		 */
		ctemp = edgesVsPoly(baVerts, &(obja->polys.p[pa]), obja->va.v, ctemp, 
					&stemp, obja->loc2glob);
		if (stemp == Intersection) {
			*s = Intersection;
		} else if (stemp == Contact && *s != Intersection) {
			*s = Contact;
		}

		/* calculate the contact information (pull together the pieces), or
		 * set up the intersection ConNodes properly
		 */
		ctemp = calcConNodeInfo(ctemp, obja->id, objb->id, pa, pb);

		/* add it to the list */
		if (ctemp != NULL) {
			for (cp=ctemp; cp->next != NULL; cp=cp->next);

			cp->next = cn;
			cn = ctemp;
		}

		if (pt->next == NULL) {
			free (pt);
			pt = NULL;
		} else
			pt = pt->next;
	} while (pt != NULL);

	if (abVerts.v != NULL) free (abVerts.v);
	if (baVerts.v != NULL) free (baVerts.v);

	return cn;
}

/*
 * given an array of vertices (that represent a polygon) tests each of the edges
 * against another polygon and determines, interpenetration, contact, or
 * disjointness.
 *
 * Params	vertices	Array of vertices
 *			pp			polygon to test against
 *			verts		array of polygon's vertex points
 *			cn			contact points, lines and areas
 *			s			where to return status
 *			trans		transformation from local frame to global
 *
 * Globals	none
 *
 * Returns	the contact information in cn, as well as the status in s
 */
static ConNode *edgesVsPoly(vertices, pp, verts, cn, s, trans)
TuppleArray vertices;
PolyNode *pp;
Tupple *verts;
ConNode *cn;
Status *s;
Transform *trans;
{
	int p1, p2, limit;
	ConNode *ctemp;
	Real denom, t;
	Tupple dir, inter, *points;
	Boolean touch;
	float sign1, sign2;

	int *v, q;

	*s = Nothing;
	points = vertices.v;
	limit = vertices.total - 1;

	for (p1=0; p1 <= limit; p1++) {
		if (p1 == limit)
			p2 = 0;
		else
			p2 = p1+1;
		
		/* test the two end points of the current edge against the polygon's
		 * plane equation
		 */
		if (p1 == 0) {
			sign1 = points[p1].i * pp->normal.i + points[p1].j * pp->normal.j +
					points[p1].k * pp->normal.k + pp->d;
		} else {
			/* sign of this point already calculated for previous edge */
			sign1 = sign2;
		}
		sign2 = points[p2].i * pp->normal.i + points[p2].j * pp->normal.j +
				points[p2].k * pp->normal.k + pp->d;
	
		if (compareValues(sign1, 0.0) && compareValues(sign2, 0.0)) {
			/* the edge lies in the plane, so test it against all edges of
			 * polygon
			 */
			cn = edgeVsEdges(points[p1], points[p2], pp, verts, cn, &touch, 
						trans);
			if ((touch) && (s != Intersection)) *s = Contact;
		} else if (compareValues(sign1, 0.0)) {
			/* one point lies in the plane (and haven't already dealt with it
			 * during the checking of the previous edge) -- single point of 
			 * contact check if it lies withing the polygon
			 */
			if (p1 == 0) {
				if (pointInPoly(points[p1], pp, verts)) {
					if ((ctemp = (ConNode *)malloc(sizeof(ConNode))) == NULL) {
						fprintf(stderr, "Error: couldn't malloc ConNode\n");
						exit(1);
					}
					ctemp->statusType = Contact;
					ctemp->type = Cpoint;
					applyTransform(trans, points[p1], &(ctemp->data.point));
					ctemp->next = NULL;
					cn = buildContactList(ctemp, cn);
					if (*s != Intersection) *s = Contact;
				}
			}
		} else if (compareValues(sign2, 0.0)) {
			/* other point lies in the plane, check to see if it lies
			 * within the polygon
			 */
			if (pointInPoly(points[p2], pp, verts)) {
				if ((ctemp = (ConNode *)malloc(sizeof(ConNode))) == NULL) {
					fprintf(stderr, "Error: couldn't malloc ConNode\n");
					exit(1);
				}
				ctemp->statusType = Contact;
				ctemp->type = Cpoint;
				applyTransform(trans, points[p2], &(ctemp->data.point));
				ctemp->next = NULL;
				cn = buildContactList(ctemp, cn);
				if (*s != Intersection) *s = Contact;
			}

		} else if ((sign1 / fabs(sign1)) != (sign2 / fabs(sign2))) {
			/* the points lie on opposite sides of the plane. 3 cases:
			 * - the intersection point lies outside the polygon
			 * - the line hits the edge of the polygon -- contact
			 * - the line hits within the polygon -- intersection
			 */

			/* find intersection point with plane */
			dir.i = points[p2].i - points[p1].i;
			dir.j = points[p2].j - points[p1].j;
			dir.k = points[p2].k - points[p1].k;
			normalize(&dir);

			denom = pp->normal.i * dir.i + pp->normal.j * dir.j +
					pp->normal.k * dir.k;
			t = -(pp->d + pp->normal.i * points[p1].i +
					pp->normal.j * points[p1].j + pp->normal.k * 
					points[p1].k) / denom;

			inter.i = points[p1].i + t * dir.i;
			inter.j = points[p1].j + t * dir.j;
			inter.k = points[p1].k + t * dir.k;

			/* Check to see if touches the boundary */
			if (pointInBoundary(inter, pp, verts)) {
				if ((ctemp = (ConNode *)malloc(sizeof(ConNode))) == NULL) {
					fprintf(stderr, "Error: couldn't malloc ConNode\n");
					exit(1);
				}
				ctemp->statusType = Contact;
				ctemp->type = Cpoint;
				applyTransform(trans, inter, &(ctemp->data.point));
				ctemp->next = NULL;
				cn = buildContactList(ctemp, cn);
				if (*s != Intersection) *s = Contact;

			/* check to see if it lies within the polygon */
			} else if (pointInPoly(inter, pp, verts)) {
				*s = Intersection;
				if ((ctemp = (ConNode *)malloc(sizeof(ConNode))) == NULL) {
					fprintf(stderr, "Error: couldn't malloc ConNode\n");
					exit(1);
				}

				ctemp->statusType = Intersection;
				ctemp->type = Cpoint;
				applyTransform(trans, inter, &(ctemp->data.point));
				ctemp->next = NULL;
				cn = buildContactList(ctemp, cn);
			}

			/* point lies outside polygon so it doesn't intersect or contact
			 * the polygon.  Do nothing */
		}
		/* points lie above or below plane -- don't intersect or contact
		 * it.  Do nothing */
	}

	return cn;
}

/* 
 * Given a line segment (defined by point1 and point2 and lying in the plane,
 * although both may not be lying exaclty in the plane because we are allowing
 * for floating point arithmetic inaccuracies),
 * tests to see if it lies within the polygon, crosses an edge, crosses two 
 * edges, or lies outside the polygon.  Returns the type of contact in cn.  
 * If there is only one point of contact (the edge just touches the polygon 
 * at its boundary at one place), the contact is of type Cpoint.  Else if 
 * there are two points (the polygon is convex, so there is a maximum of two 
 * points which includes the case where part/all of the line segment overlaps 
 * with an edge).
 *
 * Params	p1			defines starting point of line segment
 *			p2			defines ending point of the line segment
 *			pp			pointer to polygon to test line segment against
 *			verts		vertices of the polygon
 *			cn			where to return the contact information
 *			touch		used to indicate whether the edge contacts the polygon
 *			trans		transform from local frame to global frame
 *
 * Globals	none
 *
 * Returns	the contact information, and also whether the edge contacts the
 *			polygon (in "touch")
 */
static ConNode *edgeVsEdges(p1, p2, pp, verts, cn, touch, trans)
Tupple p1, p2, *verts;
PolyNode *pp;
ConNode *cn;
Boolean *touch;
Transform *trans;
{
	int i, *v;
	Boolean yes1, yes2;
	ConNode *ctemp;
	Real endt, ends, t, s, magd1xd2;
	Tupple point1, d1, d2, pstart, pend, d1xd2, ttemp, p2_p1;
	
	*touch = False;

	/* test to see if either of the end points lie within the polygon */
	/* because we are allowing points not lying exactly on the plane, may need
	 * to find the corresponding points (to p1 and p2) that actually lie on
	 * the plane (shift p1 and p2 in the reverse direction to the normal)
	 */
	yes1 = pointInPoly(p1, pp, verts);
	yes2 = pointInPoly(p2, pp, verts);
	if (yes1 && yes2) {
		/* both points lie within the polygon, so the line segment forms a
		 * part of the boundary of the contact area
		 */
		if ((ctemp = (ConNode *)malloc(sizeof(ConNode))) == NULL) {
			fprintf(stderr, "Error: couldn't malloc ConNode\n");
			exit(1);
		}
		*touch = True;
		applyTransform(trans, p1, &(ctemp->data.line[0]));
		applyTransform(trans, p2, &(ctemp->data.line[1]));
		ctemp->statusType = Contact;
		ctemp->type = Cline;
		ctemp->next = NULL;
		return buildContactList(ctemp, cn);
	} else if (yes1) {
		/* p1 lies in the polygon */
		*touch = True;
		point1 = p1;
	} else if (yes2) {
		/* p2 lies in the polygon */
		*touch = True;
		point1 = p2;
	}

	/* either have one contact point already, or no contact point.  Check
	 * line segment against all edges of the polygon.  Will either find
	 * a point (and if we already have one we then have a line segment), or
	 * 2 points (a line), or nothing
	 */

	/* define parametric representation of the line segment:
	 * l1 = p1 + d1 * t
	 */
	if ((endt = buildParaLine(p1, p2, &d1)) == -1) {
		fprintf(stderr, "Error: couldn't create parametric line rep.\n");
		exit(2);
	}

	v = pp->vertices;
	/* go through all the polygon's edges looking for intersections */
	for (i=0; i < pp->numVertices; i++) {

		pstart = verts[v[i]];
		if (i == pp->numVertices-1)
			pend = verts[v[0]];
		else
			pend = verts[v[i+1]];
		
		if ((ends = buildParaLine(pstart, pend, &d2)) == -1) {
			fprintf(stderr, "Error: couldn't create parametric line rep.\n");
			exit(2);
		}

		/* have parametric rep of both line segments, find point of
		 * intersection.  Both lines lie in the plane so either they are
		 * parallel (ignore then, any contact will be picked up when testing
		 * other edges) or they intersect
		 *
		 * solve:
		 *
		 * t = Det{(p2 - p1), d2, d1 X d2} / |d1 X d2|^2
		 */

		/* calculate d1 X d2 */
		d1xd2.i = d1.j * d2.k - d1.k * d2.j;
		d1xd2.j = d1.k * d2.i - d1.i * d2.k;
		d1xd2.k = d1.i * d2.j - d1.j * d2.i;

		/* calculate |d1 X d2|^2 */
		magd1xd2 = SQR(d1xd2.i) + SQR(d1xd2.j) + SQR(d1xd2.k);

		if ((float)magd1xd2 == 0.0) {
			/* line segments are parallel so continue. Any contact in this
			 * case will be picked up by the other cases
			 */
			continue;
		}

		p2_p1.i = pstart.i - p1.i; p2_p1.j = pstart.j - p1.j;
		p2_p1.k = pstart.k - p1.k;
		t =  p2_p1.i * (d2.j * d1xd2.k - d2.k * d1xd2.j);
		t += d2.i * (d1xd2.j * p2_p1.k - p2_p1.j * d1xd2.k);
		t += d1xd2.i * (p2_p1.j * d2.k - d2.j * p2_p1.k);
		t /= magd1xd2;

		/* found the point of intersection (or closest approach at least), 
		 * check to see if it lies within the first line segment
		 */
		if (t < 0.0 || t > endt) {
			/* intersects outside of line */
			continue;
		}

		/* now solve for s:
		 *
		 * s = Det{(p2 - p1), d1, d1 X d2} / |d1 X d2|^2
		 */
		s =  p2_p1.i * (d1.j * d1xd2.k - d1.k * d1xd2.j);
		s += d1.i * (p2_p1.j * d1xd2.k - p2_p1.k * d1xd2.j);
		s += d1xd2.i * (p2_p1.j * d1.k - p2_p1.k * d1.j);
		s /= magd1xd2;

		/* found point of intersection (or closest approach), check to see if
		 * it lies within this line segment
		 */
		if (s < 0.0 || s > ends) {
			/* intersects outside of line */
			continue;
		}

		*touch = True;

		/* calculate the point of intersection.  Because the lines may not
		 * be exactly intersecting (skew), calculate the point of intersection
		 * by averaging the points from each line (if the lines do cross, then
		 * average is the exact point).
		 */
		ttemp.i = (p1.i + t * d1.i + pstart.i + s * d2.i) / 2.0;
		ttemp.j = (p1.j + t * d1.j + pstart.j + s * d2.j) / 2.0;
		ttemp.k = (p1.k + t * d1.k + pstart.k + s * d2.k) / 2.0;

		if (yes1 || yes2) {
			/* already have the first point of intersection/contact
			 * make sure this isn't the same one
			 */
			if (comparePoints(ttemp, point1))
				continue;

			/* already have the first point of intersection/contact
			 * which is different to this one so we now have the
			 * second point and can define the line
			 */
			ctemp = (ConNode *)malloc(sizeof(ConNode));
			if (ctemp == NULL) {
				fprintf(stderr,"Error: couldn't malloc ConNode\n");
				exit(1);
			}
			ctemp->statusType = Contact;
			ctemp->type = Cline;
			applyTransform(trans, point1, &(ctemp->data.line[0]));
			applyTransform(trans, ttemp, &(ctemp->data.line[1]));
			ctemp->next = NULL;
			return buildContactList(ctemp, cn);
		}

		/* first point of intersection/contact */
		yes1 = True;
		point1 = ttemp;
	}

	if (*touch) {
		/* have found only one point of intersection/contact so add it as a
		 * point to the cn list
		 */
		if ((ctemp = (ConNode *)malloc(sizeof(ConNode))) == NULL) {
			fprintf(stderr, "Error: couldn't malloc ConNode\n");
			exit(1);
		}
		ctemp->statusType = Contact;
		ctemp->type = Cpoint;
		applyTransform(trans, point1, &(ctemp->data.point));
		ctemp->next = NULL;
		return buildContactList(ctemp, cn);
	}

	/* edge lies outside of polygon */
	*touch = False;
	return cn;
}

/*
 * Used to help set up the contact return information.  Points and lines
 * are passed to this function, and a list is built.  After all intersections
 * etc for a given polygon pair are discovered, the list can then be used
 * to determine the contact area involved (or just a single line or point).
 * Thus, this function shouldn't have to deal with "area" type ConNodes.
 *
 * Params	ct		item to add
 *			cn		list to add to
 *
 * Globals	none
 *
 * returns	pointer to the head of the contact list
 */
static ConNode *buildContactList(ct, cn)
ConNode *ct, *cn;
{
	ConNode *cp, *cpp;

	for (cp=cn; cp != NULL; cp=cp->next) {
		if (cp->statusType == Intersection) continue;
		/* ignore the intersection points */

		switch (cp->type) {
			case Cpoint:
				switch (ct->type) {
					case Cpoint:
						if (comparePoints(ct->data.point, cp->data.point)) {
							/* point is already in list, ignore it */
							free (ct);
							return cn;
						}
						break;
					case Cline:
						if (comparePoints(ct->data.line[0],cp->data.point)) {
							/* point is one end of the line, so replace it
							 * with the line
							 */
							cp->type = Cline;
							cp->data.line[0] = ct->data.line[0];
							cp->data.line[1] = ct->data.line[1];
							
							/* check to see if the other end point of the line
							 * already exists (get rid of it if it does)
							 */
							for (cp=cn; cp->next != NULL; cp=cp->next) {
								cpp = cp->next;
								if (cpp->type == Cpoint) {
									if (comparePoints(cpp->data.point,
											ct->data.line[1])) {
										cp->next = cpp->next;
										free (cpp);
										free (ct);
										return cn;
									}
								}
							}

							free (ct);
							return cn;
						} else if (comparePoints(ct->data.line[1],
									cp->data.point)) {
							/* point is one end of the line, so replace it
							 * with the line
							 */
							cp->type = Cline;
							cp->data.line[0] = ct->data.line[0];
							cp->data.line[1] = ct->data.line[1];
							
							/* check to see if the other end point of the line
							 * already exists (get rid of it if it does)
							 */
							for (cp=cn; cp->next != NULL; cp=cp->next) {
								cpp = cp->next;
								if (cpp->type == Cpoint) {
									if (comparePoints(cpp->data.point,
											ct->data.line[0])) {
										cp->next = cpp->next;
										free (cpp);
										free (ct);
										return cn;
									}
								}
							}

							free (ct);
							return cn;
						}
						break;
					case Carea:
						/* shouldn't get to this case */
						break;
				}
				break;
			case Cline:
				switch (ct->type) {
					case Cpoint:
						if (comparePoints(cp->data.line[0],ct->data.point) ||
							comparePoints(cp->data.line[1],ct->data.point)) {
							/* point is one of the ends of this line,
							 * ignore it */
							free (ct);
							return cn;
						}
						break;
					case Cline:
						if ((comparePoints(cp->data.line[0],ct->data.line[0]) &&
							comparePoints(cp->data.line[1],ct->data.line[1])) ||
							(comparePoints(cp->data.line[1],ct->data.line[0]) &&
							comparePoints(cp->data.line[0],ct->data.line[1]))) {
							/* if the two lines are the same ignore the
							 * extra one
							 */
							free (ct);
							return cn;
						}
						break;
					case Carea:
						/* shouldn't get this case either */
						break;
				}
				break;
			case Carea:
				switch (ct->type) {
					case Cpoint:
						break;
					case Cline:
						break;
					case Carea:
						break;
				}
				break;
		}
	}

	/* went through entire list without finding the item pointed to by ct
	 * so add it to the head of the list.
	 */
	ct->next = cn;
	return ct;
}

/*
 * Given a list of contact information, amalgamate and set up the proper
 * ConNode that this info represents.  cn either contains a single Point or
 * Line, or a list of Lines which represent the boundary of a contact area.
 *
 * Otherwise, it contains at most two intersection points, which don't have 
 * to be altered.  The list is from the testing for intersection between 
 * a pair of polygons, so there is either intersection, contact, or nothing.
 *
 * Params	cn		the list of data to work on
 *			objaID	unique ID of object A
 *			objbID	unique ID of object B
 *			pai		A's polygon involved in this contact
 *			pbi		B's polygon involved in this contact
 *
 * Globals	none
 *
 * Returns	the properly setup ConNode
 */
static ConNode *calcConNodeInfo(cn, objaID, objbID, pai, pbi)
ConNode *cn;
Ulong objaID, objbID;
int pai, pbi;
{
	ConNode *ctemp, *cnew, *count;
	Tupple *t, *t1, *t2;
	int i;

	if (cn == NULL) return NULL;

	if (cn->statusType == Intersection) {
		/* should only be intersection points, so just set their object and
		 * polygon numbers
		 */
		for (ctemp=cn; ctemp != NULL; ctemp=ctemp->next) {
			ctemp->objs[0] = objaID; ctemp->objs[1] = objbID;
			ctemp->polys[0] = pai; ctemp->polys[1] = pbi;
		}
		return cn;
	}

	/* must be a list containing info concerning contact */
	if (cn->next != NULL) {
		/* have more than one ConNode so this must be an area of contact.
		 * All the ConNodes must be of type Cline (all ConNodes of type 
		 * Cpoint should've been eliminated)
		 */

		if ((cnew=(ConNode *)malloc(sizeof(ConNode))) == NULL) {
			fprintf(stderr, "Error: couldn't malloc ConNode\n");
			exit(1);
		}

		cnew->type = Carea;
		cnew->next = NULL;
		if ((t=(Tupple *)malloc(sizeof(Tupple)*2)) == NULL) {
			fprintf(stderr, "Error: couldn't malloc Tupple\n");
			exit(1);
		}

		t[0] = cn->data.line[0]; t[1] = cn->data.line[1];
		ctemp = cn->next; free (cn); cn = ctemp;

		i = 1;
		for (count=cn; count != NULL; count=count->next) {
			/* this first loop is used as a counter */

			for (ctemp=cn; ctemp != NULL; ctemp=ctemp->next) {
				if (ctemp->type == Cline) {
					/* current ConNode is a Cline, so it hasn't been entered
					 * into the area array yet
					 */

					t1 = &(ctemp->data.line[0]); t2 = &(ctemp->data.line[1]);

					if (comparePoints(t[i], *t1)) {
						/* found the next line */
						t = (Tupple *)realloc(t, sizeof(Tupple) * (++i + 1));
						if (t == NULL) {
							fprintf(stderr, "Error: Couldn't malloc Tupple\n");
							exit(1);
						}

						t[i] = *t2;
						ctemp->type = Carea;
						/* mark it as being dealt with */
						break;

					} else if (comparePoints(t[i], *t2)) {
						/* found the next line */
						t = (Tupple *)realloc(t, sizeof(Tupple) * (++i + 1));
						if (t == NULL) {
							fprintf(stderr, "Error: Couldn't malloc Tupple\n");
							exit(1);
						}

						t[i] = *t1;
						ctemp->type = Carea;
						/* mark it as being dealt with */
						break;
					}
				}
			}
		}

		/* Array of vertices defining the area of contact contains the first
		 * vertex duplicated in the last position, so remove it */
		if ((t=(Tupple *)realloc(t, sizeof(Tupple) * i)) == NULL) {
			fprintf(stderr, "Error: couldn't realloc Tupple array\n");
			exit(1);
		}
		cnew->data.area.total = i;
		cnew->data.area.v = t;

		/* destroy unwanted list pointed to by cn */
		destroyConNodeList(cn);

		cn = cnew;
	}

	cn->objs[0] = objaID; cn->objs[1] = objbID;
	cn->polys[0] = pai; cn->polys[1] = pbi;
	return cn;
}

/*
 * Given a point (which lies in the supporting plane of the polygon) determines
 * if it lies within the bounds of the polygon.  Based on the function:
 *		An Efficient Ray/Polygon Intersection
 *		by Didier Badouel
 *		from "Graphics Gems", Academic Press, 1990
 *
 * Params	point		the point to test
 *
 * Globals	none
 *
 * Returns	True or False depending upon whether the point lies withing the
 *			polygon
 */
Boolean pointInPoly(point, pp, verts) 
Tupple point;
PolyNode *pp;
Tupple *verts;
{
	Real P[3], u0, v0, u1, v1, u2, v2, alpha, beta;
	Real V0[3], V[3];
	int i1, i2, i, *v;
	Boolean inter;

	i1 = pp->i1;
	i2 = pp->i2;
	v = pp->vertices;

	P[0] = point.i;
	P[1] = point.j;
	P[2] = point.k;
	V0[0] = verts[v[0]].i;
	V0[1] = verts[v[0]].j;
	V0[2] = verts[v[0]].k;

	u0 = P[i1] - V0[i1]; v0 = P[i2] - V0[i2];
	inter = False; i = 2;
	do {
		/* The polygon is viewed as (n-2) triangles. */
		V[0] = verts[v[i-1]].i;
		V[1] = verts[v[i-1]].j;
		V[2] = verts[v[i-1]].k;
    	u1 = V[i1] - V0[i1]; v1 = V[i2] - V0[i2];

		V[0] = verts[v[i]].i;
		V[1] = verts[v[i]].j;
		V[2] = verts[v[i]].k;
    	u2 = V[i1] - V0[i1]; v2 = V[i2] - V0[i2];

    	if (u1 == 0.0)    {
			beta = u0/u2;
			if ((beta >= 0.)&&(beta <= 1.)) {
           		alpha = (v0 - beta*v2)/v1;
            	inter = ((alpha >= 0.)&&((alpha+beta) <= 1.));
        	}
    	} else {
       		beta = (v0*u1 - u0*v1)/(v2*u1 - u2*v1);
        	if ((beta >= 0.)&&(beta <= 1.)) {
           		alpha = (u0 - beta*u2)/u1;
            	inter = ((alpha >= 0.)&&((alpha+beta) <= 1.));
        	}
    	}
	} while ((!inter)&&(++i < pp->numVertices));

	return inter;
}

/*
 * given a point lying in the plane of the polygon, determines if it lies on
 * the boundary or not
 *
 * Params	pnt		point to test
 *			pp		polygon to test against
 *			verts	vertices of polygon
 *
 * Globals	none
 *
 * Returns	True if the point lies on the boundary, otherwise False
 */
static Boolean pointInBoundary(pnt, pp, verts)
Tupple pnt;
PolyNode *pp;
Tupple *verts;
{
	int i, limit, *v, i1, i2;
	Real p[3], start[3], stop[3], length, dx, dy, tx, ty, endt;

	v = pp->vertices;
	i1 = pp->i1; i2 = pp->i2;
	p[0] = pnt.i; p[1] = pnt.j; p[2] = pnt.k;

	/* go though edges of the polygon */
	for (i=0, limit=pp->numVertices-1; i <= limit; i++) {

		/* set starting and stopping points of this edge */
		start[0] = verts[v[i]].i; start[1] = verts[v[i]].j;
		start[2] = verts[v[i]].k;
		if (i == limit) {
			stop[0] = verts[v[0]].i; stop[1] = verts[v[0]].j;
			stop[2] = verts[v[0]].k;
		} else {
			stop[0] = verts[v[i+1]].i; stop[1] = verts[v[i+1]].j;
			stop[2] = verts[v[i+1]].k;
		}
		dx = stop[i1] - start[i1];
		dy = stop[i2] - start[i2];
		length = sqrt (dx * dx + dy * dy);
		dx /= length; dy /= length;

		if (dx == 0.0) {			/* vertical line */
			endt = (stop[i2] - start[i2]) / dy;
			ty = (p[i2] - start[i2]) / dy;
			if (ty <= endt && ty >= 0.0 && compareValues(p[i1], start[i1]))
				return True;

		} else if (dy == 0.0) {		/* horizontal line */
			endt = (stop[i1] - start[i1]) / dx;
			tx = (p[i1] - start[i1]) / dx;
			if (tx <= endt && tx >= 0.0 && compareValues(p[i2], start[i2]))
				return True;
		} else {
			endt = (stop[i1] - start[i1]) / dx;
			tx = (p[i1] - start[i1]) / dx;
			ty = (p[i2] - start[i2]) / dy;

			if (compareValues(tx, ty) && tx <= endt && tx >= 0.0) return True;
		}
	}

	return False;
}

/*
 * frees the memory used by the list pointed to by cn
 *
 * Params	cn		pointer to linked list of ConNodes
 *
 * Globals	none
 *
 * Returns	nothing
 */
void destroyConNodeList(cn)
ConNode *cn;
{
	ConNode *ct;

	if (cn == NULL) return;

	while (cn != NULL) {
		if (cn->type == Carea)
			if (cn->data.area.total > 0) free (cn->data.area.v);
		ct = cn->next;
		free(cn);
		cn = ct;
	}
}

/*
 * Takes 2 points, and then creates the parametric representation:
 * point = origin + dir * t
 *
 * Params	p1		starting point
 *			p2		ending point
 *			dir		return the direction vector
 *
 * Globals	none
 *
 * Returns	the t value when point = p2 and the direction vector in dir
 *			-1 when there is a problem
 */
static Real buildParaLine(p1, p2, dir)
Tupple p1, p2, *dir;
{
	if (p1.i == p2.i && p1.j == p2.j && p1.k == p2.k) return -1.0;

	dir->i = p2.i - p1.i; dir->j = p2.j - p1.j; dir->k = p2.k - p1.k;
	if (!normalize(dir)) return -1.0;

	if (dir->i != 0.0) return (p2.i - p1.i) / dir->i;
	else if (dir->j != 0.0) return (p2.j - p1.j) / dir->j;
	else return (p2.i - p1.i) / dir->i;
}

/*
 * Given 2 points, test to see if they are actually the same, ie. they lie
 * within the given tolerance of each other and can be assumed to be the same.
 * The tolerance is defined by ETA
 *
 * Params	p1		point 1
 *			p2		point 2
 *
 * Globals	none
 *
 * Returns	True or False
 */
Boolean comparePoints(p1, p2)
Tupple p1, p2;
{
	Real a, b;
	int i;

	for (i=0; i<3 ;i++) {
		switch (i) {
			case 0: a = p1.i; b = p2.i; break;
			case 1: a = p1.j; b = p2.j; break;
			case 2: a = p1.k; b = p2.k; break;
		}

		if (!compareValues(a, b))
			return False;
	}

	return True;
}

/*
 * Given two values, tests to see whether they are close enough to be
 * considered equal.  The tolerance is defined by ETA.
 *
 * Params	v1		value 1
 *			v2		value 2
 *
 * Globals	none
 *
 * Returns	True if they lie within the allowable range, False otherwise
 */
Boolean compareValues(v1, v2)
Real v1, v2;
{
	Real Llim, Ulim;

	if (v1 < ETA && v1 > -ETA) {
		/* this value is close enough to zero */
		Llim = -ETA; Ulim = ETA;
	} else {
		Llim = v1 -  v1 * ETA; Ulim = v1 + v1 * ETA;
	}

	if (v2 >= Llim && v2 <= Ulim)
		return True;
	
	/* try around the other way */
	if (v2 < ETA && v2 > -ETA) {
		/* this value is close enough to zero */
		Llim = -ETA; Ulim = ETA;
	} else {
		Llim = v2 - v2 * ETA; Ulim = v2 + v2 * ETA;
	}

	if (v1 >= Llim && v1 <= Ulim)
		return True;
	
	return False;
}