/* Steven Andrews, 2/17/06
   Simple routines for 1D, 2D, and 3D geometry
   See documentation called Geometry_doc.doc and Geometry_doc.pdf */
/* Copyright 2003 by Steven Andrews.  Permission is granted
   for non-commercial use of and modifications to the code. */

#include <math.h>
#include <stdio.h>
#include "Geometry.h"
#include "math2.h"


/***************** ... Normal **************/

double Geo_LineNormal(double *pt1,double *pt2,double *ans) {
	double dx,dy,len;

	dx=pt2[0]-pt1[0];
	dy=pt2[1]-pt1[1];
	len=sqrt(dx*dx+dy*dy);
	if(len>0) {
		ans[0]=dy/len;
		ans[1]=-dx/len; }
	else {
		ans[0]=1;
		ans[1]=0; }
	return len; }


double Geo_LineNormal3D(double *pt1,double *pt2,double *point,double *ans) {
	double dot,line[3];

	line[0]=pt2[0]-pt1[0];
	line[1]=pt2[1]-pt1[1];
	line[2]=pt2[2]-pt1[2];
	dot=line[0]*line[0]+line[1]*line[1]+line[2]*line[2];
	if(dot==0) {								// pt1 == pt2
		ans[0]=point[0]-pt1[0];
		ans[1]=point[1]-pt1[1];
		ans[2]=point[2]-pt1[2];
		dot=ans[0]*ans[0]+ans[1]*ans[1]+ans[2]*ans[2];
		if(dot==0) {							// pt1 == pt2 and point == pt1 so return unit x
			ans[0]=1;
			ans[1]=0;
			ans[2]=0;
			return 0; }
		dot=sqrt(dot);						// pt1 == pt2 so return normalized point
		ans[0]/=dot;
		ans[1]/=dot;
		ans[2]/=dot;
		return dot; }
	dot=sqrt(dot);
	line[0]/=dot;
	line[1]/=dot;
	line[2]/=dot;

	ans[0]=point[0]-pt1[0];
	ans[1]=point[1]-pt1[1];
	ans[2]=point[2]-pt1[2];
	dot=ans[0]*line[0]+ans[1]*line[1]+ans[2]*line[2];
	ans[0]-=dot*line[0];
	ans[1]-=dot*line[1];
	ans[2]-=dot*line[2];
	dot=ans[0]*line[0]+ans[1]*line[1]+ans[2]*line[2];
	ans[0]-=dot*line[0];
	ans[1]-=dot*line[1];
	ans[2]-=dot*line[2];
	dot=ans[0]*ans[0]+ans[1]*ans[1]+ans[2]*ans[2];
	if(dot==0) {											// point is on line
		if(line[0]==0&&line[1]==0) {		// line parallel to z so return unit x
			ans[0]=1;
			ans[1]=0;
			ans[2]=0;
			return 0; }
		ans[0]=line[1];									// return right side perpendicular in x,y plane
		ans[1]=-line[0];
		ans[2]=0;
		dot=sqrt(ans[0]*ans[0]+ans[1]*ans[1]+ans[2]*ans[2]);
		ans[0]/=dot;
		ans[1]/=dot;
		ans[2]/=dot;
		return 0; }
	dot=sqrt(dot);
	ans[0]/=dot;
	ans[1]/=dot;
	ans[2]/=dot;
	return dot; }


double Geo_TriNormal(double *pt1,double *pt2,double *pt3,double *ans) {
	double dx1,dy1,dz1,dx2,dy2,dz2,area;

	dx1=pt2[0]-pt1[0];
	dy1=pt2[1]-pt1[1];
	dz1=pt2[2]-pt1[2];
	dx2=pt3[0]-pt2[0];
	dy2=pt3[1]-pt2[1];
	dz2=pt3[2]-pt2[2];
	ans[0]=dy1*dz2-dz1*dy2;			// v1 cross v2
	ans[1]=-dx1*dz2+dz1*dx2;
	ans[2]=dx1*dy2-dy1*dx2;
	area=sqrt(ans[0]*ans[0]+ans[1]*ans[1]+ans[2]*ans[2]);
	if(area>0) {
		ans[0]/=area;
		ans[1]/=area;
		ans[2]/=area; }
	else {
		Geo_LineNormal(pt1,pt2,ans);
		ans[2]=0; }
	return area/2; }


/********* Point In ... ***********/

int Geo_PtInTriangle(double *pt1,double *pt2,double *pt3,double *norm,double *test) {
	double dx1,dy1,dz1,dx2,dy2,dz2,crss[3],dot;

	dx1=pt2[0]-pt1[0];
	dy1=pt2[1]-pt1[1];
	dz1=pt2[2]-pt1[2];
	dx2=test[0]-pt2[0];
	dy2=test[1]-pt2[1];
	dz2=test[2]-pt2[2];
	crss[0]=dy1*dz2-dz1*dy2;
	crss[1]=-dx1*dz2+dz1*dx2;
	crss[2]=dx1*dy2-dy1*dx2;
	dot=crss[0]*norm[0]+crss[1]*norm[1]+crss[2]*norm[2];
	if(dot<0) return 0;

	dx1=pt3[0]-pt2[0];
	dy1=pt3[1]-pt2[1];
	dz1=pt3[2]-pt2[2];
	dx2=test[0]-pt3[0];
	dy2=test[1]-pt3[1];
	dz2=test[2]-pt3[2];
	crss[0]=dy1*dz2-dz1*dy2;
	crss[1]=-dx1*dz2+dz1*dx2;
	crss[2]=dx1*dy2-dy1*dx2;
	dot=crss[0]*norm[0]+crss[1]*norm[1]+crss[2]*norm[2];
	if(dot<0) return 0;

	dx1=pt1[0]-pt3[0];
	dy1=pt1[1]-pt3[1];
	dz1=pt1[2]-pt3[2];
	dx2=test[0]-pt1[0];
	dy2=test[1]-pt1[1];
	dz2=test[2]-pt1[2];
	crss[0]=dy1*dz2-dz1*dy2;
	crss[1]=-dx1*dz2+dz1*dx2;
	crss[2]=dx1*dy2-dy1*dx2;
	dot=crss[0]*norm[0]+crss[1]*norm[1]+crss[2]*norm[2];
	if(dot<0) return 0;
	return 1; }


int Geo_PtInSlab(double *pt1,double *pt2,double *test,int dim) {
	double dot;
	int d;

	dot=0;
	for(d=0;d<dim;d++) dot+=(test[d]-pt1[d])*(pt2[d]-pt1[d]);
	if(dot<0) return 0;
	dot=0;
	for(d=0;d<dim;d++) dot+=(pt2[d]-test[d])*(pt2[d]-pt1[d]);
	if(dot<0) return 0;
	return 1; }


int Geo_PtInSphere(double *test,double *cent,double rad,int dim) {
	int d;
	double r2;

	r2=0;
	for(d=0;d<dim;d++) r2+=(test[d]-cent[d])*(test[d]-cent[d]);
	return r2>rad*rad?0:1; }


/*************** To Rect ****************/

void Geo_Semic2Rect(double *cent,double rad,double *outvect,double *r1,double *r2,double *r3) {
	r1[0]=cent[0]+rad*outvect[1];
	r1[1]=cent[1]-rad*outvect[0];
	r2[0]=cent[0]-rad*outvect[1];
	r2[1]=cent[1]+rad*outvect[0];
	r3[0]=cent[0]+rad*outvect[1]-rad*outvect[0];
	r3[1]=cent[1]-rad*outvect[0]-rad*outvect[1];
	return; }


void Geo_Hemis2Rect(double *cent,double rad,double *outvect,double *r1,double *r2,double *r3,double *r4) {
	double v1[3],v2[3];

	v2[0]=v2[1]=v2[2]=0;
	Geo_LineNormal3D(v2,outvect,v2,v1);
	v2[0]=v1[1]*outvect[2]-v1[2]*outvect[1];		// v2 = v1 x outvect
	v2[1]=v1[2]*outvect[0]-v1[0]*outvect[2];
	v2[2]=v1[0]*outvect[1]-v1[1]*outvect[0];

	r1[0]=cent[0]+rad*(-v1[0]-v2[0]);
	r1[1]=cent[1]+rad*(-v1[1]-v2[1]);
	r1[2]=cent[2]+rad*(-v1[2]-v2[2]);
	r2[0]=cent[0]+rad*(+v1[0]-v2[0]);
	r2[1]=cent[1]+rad*(+v1[1]-v2[1]);
	r2[2]=cent[2]+rad*(+v1[2]-v2[2]);
	r3[0]=cent[0]+rad*(-v1[0]+v2[0]);
	r3[1]=cent[1]+rad*(-v1[1]+v2[1]);
	r3[2]=cent[2]+rad*(-v1[2]+v2[2]);
	r4[0]=cent[0]+rad*(-v1[0]-v2[0]-outvect[0]);
	r4[1]=cent[1]+rad*(-v1[1]-v2[1]-outvect[1]);
	r4[2]=cent[2]+rad*(-v1[2]-v2[2]-outvect[2]);
	return; }


void Geo_Cyl2Rect(double *pt1,double *pt2,double rad,double *r1,double *r2,double *r3,double *r4) {
	double v1[3],v2[3];

	Geo_LineNormal3D(pt1,pt2,pt1,v1);
	v2[0]=v1[1]*(pt2[2]-pt1[2])-v1[2]*(pt2[1]-pt1[1]);		// v2 = v1 x (pt2 - pt1)
	v2[1]=v1[2]*(pt2[0]-pt1[0])-v1[0]*(pt2[2]-pt1[2]);
	v2[2]=v1[0]*(pt2[1]-pt1[1])-v1[1]*(pt2[0]-pt1[0]);

	r1[0]=pt1[0]+rad*(-v1[0]-v2[0]);
	r1[1]=pt1[1]+rad*(-v1[1]-v2[1]);
	r1[2]=pt1[2]+rad*(-v1[2]-v2[2]);
	r2[0]=pt1[0]+rad*(+v1[0]-v2[0]);
	r2[1]=pt1[1]+rad*(+v1[1]-v2[1]);
	r2[2]=pt1[2]+rad*(+v1[2]-v2[2]);
	r3[0]=pt1[0]+rad*(-v1[0]+v2[0]);
	r3[1]=pt1[1]+rad*(-v1[1]+v2[1]);
	r3[2]=pt1[2]+rad*(-v1[2]+v2[2]);
	r4[0]=pt2[0]+rad*(-v1[0]-v2[0]);
	r4[1]=pt2[1]+rad*(-v1[1]-v2[1]);
	r4[2]=pt2[2]+rad*(-v1[2]-v2[2]);
	return; }


/**************** ...X...  ***************/

double Geo_LineXLine(double *l1p1,double *l1p2,double *l2p1,double *l2p2,double *crss2ptr) {
	double rxcx,rycy,sxrx,syry,dxcx,dycy,cross;

	rxcx=l2p1[0]-l1p1[0];		// rx-cx
	rycy=l2p1[1]-l1p1[1];		// ry-cy
	sxrx=l2p2[0]-l2p1[0];		// sx-rx
	syry=l2p2[1]-l2p1[1];		// sy-ry
	dxcx=l1p2[0]-l1p1[0];		// dx-cx
	dycy=l1p2[1]-l1p1[1];		// dy-cy
	cross=(rxcx*syry-rycy*sxrx)/(dxcx*syry-dycy*sxrx);
	if(crss2ptr) *crss2ptr=(rxcx*dycy-rycy*dxcx)/(dxcx*syry-dycy*sxrx);
	return cross; }


double Geo_LineXSphs(double *pt1,double *pt2,double *cent,double rad,int dim,double *crss2ptr) {
	double a,b,c,cross,cross2;
	int d;

	a=b=c=0;
	for(d=0;d<dim;d++) {
		a+=(pt2[d]-pt1[d])*(pt2[d]-pt1[d]);
		b+=2*(pt1[d]-cent[d])*(pt2[d]-pt1[d]);
		c+=(pt1[d]-cent[d])*(pt1[d]-cent[d]); }
	c-=rad*rad;
	cross=(-b-sqrt(b*b-4*a*c))/(2*a);
	cross2=(-b+sqrt(b*b-4*a*c))/(2*a);
	if(cross>=0&&cross<=1) {
		if(crss2ptr) *crss2ptr=cross2;
		return cross; }
	if(cross2>=0&&cross2<=1) {
		if(crss2ptr) *crss2ptr=cross;
		return cross2; }
	if(crss2ptr) *crss2ptr=cross2;
	return cross; }


double Geo_LineXCyl2s(double *pt1,double *pt2,double *cp1,double *cp2,double *norm,double rad,double *crss2ptr) {
	double edge0[2],edge1[2],crossl,crossr;

	edge0[0]=cp1[0]+norm[0]*rad;
	edge0[1]=cp1[1]+norm[1]*rad;
	edge1[0]=cp2[0]+norm[0]*rad;
	edge1[1]=cp2[1]+norm[1]*rad;
	crossr=Geo_LineXLine(pt1,pt2,edge0,edge1,NULL);
	edge0[0]=cp1[0]-norm[0]*rad;
	edge0[1]=cp1[1]-norm[1]*rad;
	edge1[0]=cp2[0]-norm[0]*rad;
	edge1[1]=cp2[1]-norm[1]*rad;
	crossl=Geo_LineXLine(pt1,pt2,edge0,edge1,NULL);
	if(0<=crossr&&crossr<=1&&(crossr<crossl||crossl<0)) {
		*crss2ptr=crossl;
		return crossr; }
	if(0<=crossl&&crossl<=1&&(crossl<crossr||crossr<0)) {
		*crss2ptr=crossr;
		return crossl; }
	if(crossr<crossl) {
		*crss2ptr=crossl;
		return crossr; }
	*crss2ptr=crossr;
	return crossl; }


double Geo_LineXCyls(double *pt1,double *pt2,double *cp1,double *cp2,double rad,double *crss2ptr) {
	double r1,r2,norm1[3],norm2[3],zero[3];
	int d;

	r1=Geo_LineNormal3D(cp1,cp2,pt1,norm1);
	r2=Geo_LineNormal3D(cp1,cp2,pt2,norm2);
	for(d=0;d<3;d++) {
		zero[d]=0;
		norm1[d]*=r1;
		norm2[d]*=r2; }
	return Geo_LineXSphs(norm1,norm2,zero,rad,3,crss2ptr); }


/******************* ...Xaabb ****************/

int Geo_LineXaabb2(double *pt1,double *pt2,double *norm,double *bpt1,double *bpt2) {
	double cmp,b1,b2,b3,b4;

	if(pt1[0]<bpt1[0]&&pt2[0]<bpt1[0]) return 0;
	if(pt1[0]>bpt2[0]&&pt2[0]>bpt2[0]) return 0;
	if(pt1[1]<bpt1[1]&&pt2[1]<bpt1[1]) return 0;
	if(pt1[1]>bpt2[1]&&pt2[1]>bpt2[1]) return 0;
	cmp=norm[0]*pt1[0]+norm[1]*pt1[1];
	b1=norm[0]*bpt1[0]+norm[1]*bpt1[1];
	b2=norm[0]*bpt1[0]+norm[1]*bpt2[1];
	b3=norm[0]*bpt2[0]+norm[1]*bpt1[1];
	b4=norm[0]*bpt2[0]+norm[1]*bpt2[1];
	if(b1<cmp&&b2<cmp&&b3<cmp&&b4<cmp) return 0;
	if(b1>cmp&&b2>cmp&&b3>cmp&&b4>cmp) return 0;
	return 1; }


int Geo_TriXaabb3(double *pt1,double *pt2,double *pt3,double *norm,double *bpt1,double *bpt2) {
	double cmp,b1,b2,b3,b4,b5,b6,b7,b8;

	if(pt1[0]<bpt1[0]&&pt2[0]<bpt1[0]&&pt3[0]<bpt1[0]) return 0;
	if(pt1[0]>bpt2[0]&&pt2[0]>bpt2[0]&&pt3[0]>bpt2[0]) return 0;
	if(pt1[1]<bpt1[1]&&pt2[1]<bpt1[1]&&pt3[1]<bpt1[1]) return 0;
	if(pt1[1]>bpt2[1]&&pt2[1]>bpt2[1]&&pt3[1]>bpt2[1]) return 0;
	if(pt1[2]<bpt1[2]&&pt2[2]<bpt1[2]&&pt3[2]<bpt1[2]) return 0;
	if(pt1[2]>bpt2[2]&&pt2[2]>bpt2[2]&&pt3[2]>bpt2[2]) return 0;
	cmp=norm[0]*pt1[0]+norm[1]*pt1[1]+norm[2]*pt1[2];
	b1=norm[0]*bpt1[0]+norm[1]*bpt1[1]+norm[2]*bpt1[2];
	b2=norm[0]*bpt1[0]+norm[1]*bpt1[1]+norm[2]*bpt2[2];
	b3=norm[0]*bpt1[0]+norm[1]*bpt2[1]+norm[2]*bpt1[2];
	b4=norm[0]*bpt1[0]+norm[1]*bpt2[1]+norm[2]*bpt2[2];
	b5=norm[0]*bpt2[0]+norm[1]*bpt1[1]+norm[2]*bpt1[2];
	b6=norm[0]*bpt2[0]+norm[1]*bpt1[1]+norm[2]*bpt2[2];
	b7=norm[0]*bpt2[0]+norm[1]*bpt2[1]+norm[2]*bpt1[2];
	b8=norm[0]*bpt2[0]+norm[1]*bpt2[1]+norm[2]*bpt2[2];
	if(b1<cmp&&b2<cmp&&b3<cmp&&b4<cmp&&b5<cmp&&b6<cmp&&b7<cmp&&b8<cmp) return 0;
	if(b1>cmp&&b2>cmp&&b3>cmp&&b4>cmp&&b5>cmp&&b6>cmp&&b7>cmp&&b8>cmp) return 0;
	return 1; }


int Geo_RectXaabb2(double *r1,double *r2,double *r3,double *bpt1,double *bpt2) {
	double v1[2],cmp,b1,b2,b3,b4;

	v1[0]=r2[0]+r3[0]-r1[0];		// point 4
	v1[1]=r2[1]+r3[1]-r1[1];
	if(r1[0]<bpt1[0]&&r2[0]<bpt1[0]&&r3[0]<bpt1[0]&&v1[0]<bpt1[0]) return 0;
	if(r1[0]>bpt2[0]&&r2[0]>bpt2[0]&&r3[0]>bpt2[0]&&v1[0]>bpt2[0]) return 0;
	if(r1[1]<bpt1[1]&&r2[1]<bpt1[1]&&r3[1]<bpt1[1]&&v1[1]<bpt1[1]) return 0;
	if(r1[1]>bpt2[1]&&r2[1]>bpt2[1]&&r3[1]>bpt2[1]&&v1[1]>bpt2[1]) return 0;
	v1[0]=r2[0]-r1[0];					// side 1,2
	v1[1]=r2[1]-r1[1];
	cmp=v1[0]*r1[0]+v1[1]*r1[1];
	b1=v1[0]*bpt1[0]+v1[1]*bpt1[1];
	b2=v1[0]*bpt1[0]+v1[1]*bpt2[1];
	b3=v1[0]*bpt2[0]+v1[1]*bpt1[1];
	b4=v1[0]*bpt2[0]+v1[1]*bpt2[1];
	if(b1<cmp&&b2<cmp&&b3<cmp&&b4<cmp) return 0;
	cmp=v1[0]*r2[0]+v1[1]*r2[1];
	if(b1>cmp&&b2>cmp&&b3>cmp&&b4>cmp) return 0;
	v1[0]=r3[0]-r1[0];					// side 2,3
	v1[1]=r3[1]-r1[1];
	cmp=v1[0]*r1[0]+v1[1]*r1[1];
	b1=v1[0]*bpt1[0]+v1[1]*bpt1[1];
	b2=v1[0]*bpt1[0]+v1[1]*bpt2[1];
	b3=v1[0]*bpt2[0]+v1[1]*bpt1[1];
	b4=v1[0]*bpt2[0]+v1[1]*bpt2[1];
	if(b1<cmp&&b2<cmp&&b3<cmp&&b4<cmp) return 0;
	cmp=v1[0]*r3[0]+v1[1]*r3[1];
	if(b1>cmp&&b2>cmp&&b3>cmp&&b4>cmp) return 0;
	return 1; }


int Geo_RectXaabb3(double *r1,double *r2,double *r3,double *r4,double *bpt1,double *bpt2) {
	double r5[3],r6[3],r7[3],r8[3],v1[3],b1,b2,b3,b4,b5,b6,b7,b8,cmp;

	r5[0]=r2[0]+r4[0]-r1[0];
	r5[1]=r2[1]+r4[1]-r1[1];
	r5[2]=r2[2]+r4[2]-r1[2];
	r6[0]=r3[0]+r4[0]-r1[0];
	r6[1]=r3[1]+r4[1]-r1[1];
	r6[2]=r3[2]+r4[2]-r1[2];
	r7[0]=r2[0]+r3[0]-r1[0];
	r7[1]=r2[1]+r3[1]-r1[1];
	r7[2]=r2[2]+r3[2]-r1[2];
	r8[0]=r7[0]+r4[0]-r1[0];
	r8[1]=r7[1]+r4[1]-r1[1];
	r8[2]=r7[2]+r4[2]-r1[2];
	if(r1[0]<bpt1[0]&&r2[0]<bpt1[0]&&r3[0]<bpt1[0]&&r4[0]<bpt1[0]&&r5[0]<bpt1[0]&&r6[0]<bpt1[0]&&r7[0]<bpt1[0]) return 0;
	if(r1[0]>bpt2[0]&&r2[0]>bpt2[0]&&r3[0]>bpt2[0]&&r4[0]>bpt2[0]&&r5[0]>bpt2[0]&&r6[0]>bpt2[0]&&r7[0]>bpt2[0]) return 0;
	if(r1[1]<bpt1[1]&&r2[1]<bpt1[1]&&r3[1]<bpt1[1]&&r4[1]<bpt1[1]&&r5[1]<bpt1[1]&&r6[1]<bpt1[1]&&r7[1]<bpt1[1]) return 0;
	if(r1[1]>bpt2[1]&&r2[1]>bpt2[1]&&r3[1]>bpt2[1]&&r4[1]>bpt2[1]&&r5[1]>bpt2[1]&&r6[1]>bpt2[1]&&r7[1]>bpt2[1]) return 0;
	if(r1[2]<bpt1[2]&&r2[2]<bpt1[2]&&r3[2]<bpt1[2]&&r4[2]<bpt1[2]&&r5[2]<bpt1[2]&&r6[2]<bpt1[2]&&r7[2]<bpt1[2]) return 0;
	if(r1[2]>bpt2[2]&&r2[2]>bpt2[2]&&r3[2]>bpt2[2]&&r4[2]>bpt2[2]&&r5[2]>bpt2[2]&&r6[2]>bpt2[2]&&r7[2]>bpt2[2]) return 0;
	v1[0]=r2[0]-r1[0];					// side 1,2
	v1[1]=r2[1]-r1[1];
	v1[2]=r2[2]-r1[2];
	cmp=v1[0]*r1[0]+v1[1]*r1[1]+v1[2]*r1[2];
	b1=v1[0]*bpt1[0]+v1[1]*bpt1[1]+v1[2]*bpt1[2];
	b2=v1[0]*bpt1[0]+v1[1]*bpt1[1]+v1[2]*bpt2[2];
	b3=v1[0]*bpt1[0]+v1[1]*bpt2[1]+v1[2]*bpt1[2];
	b4=v1[0]*bpt1[0]+v1[1]*bpt2[1]+v1[2]*bpt2[2];
	b5=v1[0]*bpt2[0]+v1[1]*bpt1[1]+v1[2]*bpt1[2];
	b6=v1[0]*bpt2[0]+v1[1]*bpt1[1]+v1[2]*bpt2[2];
	b7=v1[0]*bpt2[0]+v1[1]*bpt2[1]+v1[2]*bpt1[2];
	b8=v1[0]*bpt2[0]+v1[1]*bpt2[1]+v1[2]*bpt2[2];
	if(b1<cmp&&b2<cmp&&b3<cmp&&b4<cmp&&b5<cmp&&b6<cmp&&b7<cmp&&b8<cmp) return 0;
	cmp=v1[0]*r2[0]+v1[1]*r2[1]+v1[2]*r2[2];
	if(b1>cmp&&b2>cmp&&b3>cmp&&b4>cmp&&b5>cmp&&b6>cmp&&b7>cmp&&b8>cmp) return 0;
	v1[0]=r3[0]-r1[0];					// side 1,3
	v1[1]=r3[1]-r1[1];
	v1[2]=r3[2]-r1[2];
	cmp=v1[0]*r1[0]+v1[1]*r1[1]+v1[2]*r1[2];
	b1=v1[0]*bpt1[0]+v1[1]*bpt1[1]+v1[2]*bpt1[2];
	b2=v1[0]*bpt1[0]+v1[1]*bpt1[1]+v1[2]*bpt2[2];
	b3=v1[0]*bpt1[0]+v1[1]*bpt2[1]+v1[2]*bpt1[2];
	b4=v1[0]*bpt1[0]+v1[1]*bpt2[1]+v1[2]*bpt2[2];
	b5=v1[0]*bpt2[0]+v1[1]*bpt1[1]+v1[2]*bpt1[2];
	b6=v1[0]*bpt2[0]+v1[1]*bpt1[1]+v1[2]*bpt2[2];
	b7=v1[0]*bpt2[0]+v1[1]*bpt2[1]+v1[2]*bpt1[2];
	b8=v1[0]*bpt2[0]+v1[1]*bpt2[1]+v1[2]*bpt2[2];
	if(b1<cmp&&b2<cmp&&b3<cmp&&b4<cmp&&b5<cmp&&b6<cmp&&b7<cmp&&b8<cmp) return 0;
	cmp=v1[0]*r3[0]+v1[1]*r3[1]+v1[2]*r3[2];
	if(b1>cmp&&b2>cmp&&b3>cmp&&b4>cmp&&b5>cmp&&b6>cmp&&b7>cmp&&b8>cmp) return 0;
	v1[0]=r4[0]-r1[0];					// side 1,4
	v1[1]=r4[1]-r1[1];
	v1[2]=r4[2]-r1[2];
	cmp=v1[0]*r1[0]+v1[1]*r1[1]+v1[2]*r1[2];
	b1=v1[0]*bpt1[0]+v1[1]*bpt1[1]+v1[2]*bpt1[2];
	b2=v1[0]*bpt1[0]+v1[1]*bpt1[1]+v1[2]*bpt2[2];
	b3=v1[0]*bpt1[0]+v1[1]*bpt2[1]+v1[2]*bpt1[2];
	b4=v1[0]*bpt1[0]+v1[1]*bpt2[1]+v1[2]*bpt2[2];
	b5=v1[0]*bpt2[0]+v1[1]*bpt1[1]+v1[2]*bpt1[2];
	b6=v1[0]*bpt2[0]+v1[1]*bpt1[1]+v1[2]*bpt2[2];
	b7=v1[0]*bpt2[0]+v1[1]*bpt2[1]+v1[2]*bpt1[2];
	b8=v1[0]*bpt2[0]+v1[1]*bpt2[1]+v1[2]*bpt2[2];
	if(b1<cmp&&b2<cmp&&b3<cmp&&b4<cmp&&b5<cmp&&b6<cmp&&b7<cmp&&b8<cmp) return 0;
	cmp=v1[0]*r4[0]+v1[1]*r4[1]+v1[2]*r4[2];
	if(b1>cmp&&b2>cmp&&b3>cmp&&b4>cmp&&b5>cmp&&b6>cmp&&b7>cmp&&b8>cmp) return 0;
	return 1; }


int Geo_CircleXaabb2(double *cent,double rad,double *bpt1,double *bpt2) {
	double min,max,d2,r2;

	if(cent[0]+rad<bpt1[0]) return 0;					// test projections on x,y
	if(cent[0]-rad>bpt2[0]) return 0;
	if(cent[1]+rad<bpt1[1]) return 0;
	if(cent[1]-rad>bpt2[1]) return 0;
	r2=rad*rad;																// find closest, farthest corners
	d2=(bpt1[0]-cent[0])*(bpt1[0]-cent[0])+(bpt1[1]-cent[1])*(bpt1[1]-cent[1]);
	min=max=d2;
	d2=(bpt1[0]-cent[0])*(bpt1[0]-cent[0])+(bpt2[1]-cent[1])*(bpt2[1]-cent[1]);
	if(d2<min) min=d2;
	else if(d2>max) max=d2;
	d2=(bpt2[0]-cent[0])*(bpt2[0]-cent[0])+(bpt1[1]-cent[1])*(bpt1[1]-cent[1]);
	if(d2<min) min=d2;
	else if(d2>max) max=d2;
	d2=(bpt2[0]-cent[0])*(bpt2[0]-cent[0])+(bpt2[1]-cent[1])*(bpt2[1]-cent[1]);
	if(d2<min) min=d2;
	else if(d2>max) max=d2;
	if(r2>max) return 0;												// box totally inside circle
	if(r2>=min) return 1;												// at least 1 corner inside, but not all
	if(cent[0]>=bpt1[0]&&cent[0]<=bpt2[0]) return 1;	// circle crosses box side
	if(cent[1]>=bpt1[1]&&cent[1]<=bpt2[1]) return 1;
	return 0; }																	// circle near box corner, not crossing


int Geo_SphsXaabb3(double *cent,double rad,double *bpt1,double *bpt2) {
	double min,max,d2,r2;

	if(cent[0]+rad<bpt1[0]) return 0;
	if(cent[0]-rad>bpt2[0]) return 0;
	if(cent[1]+rad<bpt1[1]) return 0;
	if(cent[1]-rad>bpt2[1]) return 0;
	if(cent[2]+rad<bpt1[2]) return 0;
	if(cent[2]-rad>bpt2[2]) return 0;
	r2=rad*rad;
	d2=(bpt1[0]-cent[0])*(bpt1[0]-cent[0])+(bpt1[1]-cent[1])*(bpt1[1]-cent[1])+(bpt1[2]-cent[2])*(bpt1[2]-cent[2]);
	min=max=d2;
	d2=(bpt1[0]-cent[0])*(bpt1[0]-cent[0])+(bpt1[1]-cent[1])*(bpt1[1]-cent[1])+(bpt2[2]-cent[2])*(bpt2[2]-cent[2]);
	if(d2<min) min=d2;
	else if(d2>max) max=d2;
	d2=(bpt1[0]-cent[0])*(bpt1[0]-cent[0])+(bpt2[1]-cent[1])*(bpt2[1]-cent[1])+(bpt1[2]-cent[2])*(bpt1[2]-cent[2]);
	if(d2<min) min=d2;
	else if(d2>max) max=d2;
	d2=(bpt1[0]-cent[0])*(bpt1[0]-cent[0])+(bpt2[1]-cent[1])*(bpt2[1]-cent[1])+(bpt2[2]-cent[2])*(bpt2[2]-cent[2]);
	if(d2<min) min=d2;
	else if(d2>max) max=d2;
	d2=(bpt2[0]-cent[0])*(bpt2[0]-cent[0])+(bpt1[1]-cent[1])*(bpt1[1]-cent[1])+(bpt1[2]-cent[2])*(bpt1[2]-cent[2]);
	if(d2<min) min=d2;
	else if(d2>max) max=d2;
	d2=(bpt2[0]-cent[0])*(bpt2[0]-cent[0])+(bpt1[1]-cent[1])*(bpt1[1]-cent[1])+(bpt2[2]-cent[2])*(bpt2[2]-cent[2]);
	if(d2<min) min=d2;
	else if(d2>max) max=d2;
	d2=(bpt2[0]-cent[0])*(bpt2[0]-cent[0])+(bpt2[1]-cent[1])*(bpt2[1]-cent[1])+(bpt1[2]-cent[2])*(bpt1[2]-cent[2]);
	if(d2<min) min=d2;
	else if(d2>max) max=d2;
	d2=(bpt2[0]-cent[0])*(bpt2[0]-cent[0])+(bpt2[1]-cent[1])*(bpt2[1]-cent[1])+(bpt2[2]-cent[2])*(bpt2[2]-cent[2]);
	if(d2<min) min=d2;
	else if(d2>max) max=d2;
	if(r2>max) return 0;
	if(r2>=min) return 1;
	if(cent[0]>=bpt1[0]&&cent[0]<=bpt2[0]) return 1;
	if(cent[1]>=bpt1[1]&&cent[1]<=bpt2[1]) return 1;
	if(cent[2]>=bpt1[2]&&cent[2]<=bpt2[2]) return 1;
	return 1; }


int Geo_CylisXaabb3(double *pt1,double *pt2,double rad,double *bpt1,double *bpt2) {
	double v1[3],v2[3],v3[3];
	double cent[2],ab1[2],ab2[2],ab3[2],ab4[2],ab5[2],ab6[2],ab7[2],ab8[2];
	double nrm,b1,b2,b3,b4,b5,b6,b7,b8,cmp,r2,d2,min,max,crss1,crss2;
	int sgn;

	Geo_LineNormal3D(pt1,pt2,pt1,v1);		// v1 and v2 are normalized and perp. to cyl. axis
	v3[0]=pt2[0]-pt1[0];
	v3[1]=pt2[1]-pt1[1];
	v3[2]=pt2[2]-pt1[2];
	nrm=sqrt(v3[0]*v3[0]+v3[1]*v3[1]+v3[2]*v3[2]);
	if(!nrm) return 0;
	v3[0]/=nrm;
	v3[1]/=nrm;
	v3[2]/=nrm;
	v2[0]=v3[1]*v1[2]-v3[2]*v1[1];
	v2[1]=v3[2]*v1[0]-v3[0]*v1[2];
	v2[2]=v3[0]*v1[1]-v3[1]*v1[0];

	cent[0]=v1[0]*pt1[0]+v1[1]*pt1[1]+v1[2]*pt1[2];
	cent[1]=v2[0]*pt1[0]+v2[1]*pt1[1]+v2[2]*pt1[2];

	ab1[0]=v1[0]*bpt1[0]+v1[1]*bpt1[1]+v1[2]*bpt1[2];		// ab are box pts in 2-D
	ab1[1]=v2[0]*bpt1[0]+v2[1]*bpt1[1]+v2[2]*bpt1[2];
	ab2[0]=v1[0]*bpt1[0]+v1[1]*bpt1[1]+v1[2]*bpt2[2];
	ab2[1]=v2[0]*bpt1[0]+v2[1]*bpt1[1]+v2[2]*bpt2[2];
	ab3[0]=v1[0]*bpt1[0]+v1[1]*bpt2[1]+v1[2]*bpt1[2];
	ab3[1]=v2[0]*bpt1[0]+v2[1]*bpt2[1]+v2[2]*bpt1[2];
	ab4[0]=v1[0]*bpt1[0]+v1[1]*bpt2[1]+v1[2]*bpt2[2];
	ab4[1]=v2[0]*bpt1[0]+v2[1]*bpt2[1]+v2[2]*bpt2[2];
	ab5[0]=v1[0]*bpt2[0]+v1[1]*bpt1[1]+v1[2]*bpt1[2];
	ab5[1]=v2[0]*bpt2[0]+v2[1]*bpt1[1]+v2[2]*bpt1[2];
	ab6[0]=v1[0]*bpt2[0]+v1[1]*bpt1[1]+v1[2]*bpt2[2];
	ab6[1]=v2[0]*bpt2[0]+v2[1]*bpt1[1]+v2[2]*bpt2[2];
	ab7[0]=v1[0]*bpt2[0]+v1[1]*bpt2[1]+v1[2]*bpt1[2];
	ab7[1]=v2[0]*bpt2[0]+v2[1]*bpt2[1]+v2[2]*bpt1[2];
	ab8[0]=v1[0]*bpt2[0]+v1[1]*bpt2[1]+v1[2]*bpt2[2];
	ab8[1]=v2[0]*bpt2[0]+v2[1]*bpt2[1]+v2[2]*bpt2[2];

	v1[0]=ab2[0]-ab1[0];									// do SAT on the three box axes
	v1[1]=ab2[1]-ab1[1];
	nrm=sqrt(v1[0]*v1[0]+v1[1]*v1[1]);
	v1[0]/=nrm;
	v1[1]/=nrm;
	b1=v1[0]*ab1[0]+v1[1]*ab1[1];
	b2=v1[0]*ab2[0]+v1[1]*ab2[1];
	b3=v1[0]*ab3[0]+v1[1]*ab3[1];
	b4=v1[0]*ab4[0]+v1[1]*ab4[1];
	b5=v1[0]*ab5[0]+v1[1]*ab5[1];
	b6=v1[0]*ab6[0]+v1[1]*ab6[1];
	b7=v1[0]*ab7[0]+v1[1]*ab7[1];
	b8=v1[0]*ab8[0]+v1[1]*ab8[1];
	cmp=v1[0]*cent[0]+v1[1]*cent[1]-rad;
	if(b1<cmp&&b2<cmp&&b3<cmp&&b4<cmp&&b5<cmp&&b6<cmp&&b7<cmp&&b8<cmp) return 0;
	cmp=v1[0]*cent[0]+v1[1]*cent[1]+rad;
	if(b1>cmp&&b2>cmp&&b3>cmp&&b4>cmp&&b5>cmp&&b6>cmp&&b7>cmp&&b8>cmp) return 0;

	v1[0]=ab3[0]-ab1[0];
	v1[1]=ab3[1]-ab1[1];
	nrm=sqrt(v1[0]*v1[0]+v1[1]*v1[1]);
	v1[0]/=nrm;
	v1[1]/=nrm;
	b1=v1[0]*ab1[0]+v1[1]*ab1[1];
	b2=v1[0]*ab2[0]+v1[1]*ab2[1];
	b3=v1[0]*ab3[0]+v1[1]*ab3[1];
	b4=v1[0]*ab4[0]+v1[1]*ab4[1];
	b5=v1[0]*ab5[0]+v1[1]*ab5[1];
	b6=v1[0]*ab6[0]+v1[1]*ab6[1];
	b7=v1[0]*ab7[0]+v1[1]*ab7[1];
	b8=v1[0]*ab8[0]+v1[1]*ab8[1];
	cmp=v1[0]*cent[0]+v1[1]*cent[1]-rad;
	if(b1<cmp&&b2<cmp&&b3<cmp&&b4<cmp&&b5<cmp&&b6<cmp&&b7<cmp&&b8<cmp) return 0;
	cmp=v1[0]*cent[0]+v1[1]*cent[1]+rad;
	if(b1>cmp&&b2>cmp&&b3>cmp&&b4>cmp&&b5>cmp&&b6>cmp&&b7>cmp&&b8>cmp) return 0;

	v1[0]=ab5[0]-ab1[0];
	v1[1]=ab5[1]-ab1[1];
	nrm=sqrt(v1[0]*v1[0]+v1[1]*v1[1]);
	v1[0]/=nrm;
	v1[1]/=nrm;
	b1=v1[0]*ab1[0]+v1[1]*ab1[1];
	b2=v1[0]*ab2[0]+v1[1]*ab2[1];
	b3=v1[0]*ab3[0]+v1[1]*ab3[1];
	b4=v1[0]*ab4[0]+v1[1]*ab4[1];
	b5=v1[0]*ab5[0]+v1[1]*ab5[1];
	b6=v1[0]*ab6[0]+v1[1]*ab6[1];
	b7=v1[0]*ab7[0]+v1[1]*ab7[1];
	b8=v1[0]*ab8[0]+v1[1]*ab8[1];
	cmp=v1[0]*cent[0]+v1[1]*cent[1]-rad;
	if(b1<cmp&&b2<cmp&&b3<cmp&&b4<cmp&&b5<cmp&&b6<cmp&&b7<cmp&&b8<cmp) return 0;
	cmp=v1[0]*cent[0]+v1[1]*cent[1]+rad;
	if(b1>cmp&&b2>cmp&&b3>cmp&&b4>cmp&&b5>cmp&&b6>cmp&&b7>cmp&&b8>cmp) return 0;

	r2=rad*rad;														// check corners inside circle
	d2=(ab1[0]-cent[0])*(ab1[0]-cent[0])+(ab1[1]-cent[1])*(ab1[1]-cent[1]);
	min=max=d2;
	d2=(ab2[0]-cent[0])*(ab2[0]-cent[0])+(ab2[1]-cent[1])*(ab2[1]-cent[1]);
	if(d2<min) min=d2;
	else if(d2>max) max=d2;
	d2=(ab3[0]-cent[0])*(ab3[0]-cent[0])+(ab3[1]-cent[1])*(ab3[1]-cent[1]);
	if(d2<min) min=d2;
	else if(d2>max) max=d2;
	d2=(ab4[0]-cent[0])*(ab4[0]-cent[0])+(ab4[1]-cent[1])*(ab4[1]-cent[1]);
	if(d2<min) min=d2;
	else if(d2>max) max=d2;
	d2=(ab5[0]-cent[0])*(ab5[0]-cent[0])+(ab5[1]-cent[1])*(ab5[1]-cent[1]);
	if(d2<min) min=d2;
	else if(d2>max) max=d2;
	d2=(ab6[0]-cent[0])*(ab6[0]-cent[0])+(ab6[1]-cent[1])*(ab6[1]-cent[1]);
	if(d2<min) min=d2;
	else if(d2>max) max=d2;
	d2=(ab7[0]-cent[0])*(ab7[0]-cent[0])+(ab7[1]-cent[1])*(ab7[1]-cent[1]);
	if(d2<min) min=d2;
	else if(d2>max) max=d2;
	d2=(ab8[0]-cent[0])*(ab8[0]-cent[0])+(ab8[1]-cent[1])*(ab8[1]-cent[1]);
	if(d2<min) min=d2;
	else if(d2>max) max=d2;
	if(r2>max) return 0;
	if(r2>=min) return 1;

	if((crss1=Geo_LineXSphs(ab1,ab2,cent,rad,2,NULL))>=0&&crss1<=1) return 1;	// check edge crossing
	if((crss1=Geo_LineXSphs(ab2,ab4,cent,rad,2,NULL))>=0&&crss1<=1) return 1;
	if((crss1=Geo_LineXSphs(ab4,ab3,cent,rad,2,NULL))>=0&&crss1<=1) return 1;
	if((crss1=Geo_LineXSphs(ab3,ab1,cent,rad,2,NULL))>=0&&crss1<=1) return 1;
	if((crss1=Geo_LineXSphs(ab1,ab5,cent,rad,2,NULL))>=0&&crss1<=1) return 1;
	if((crss1=Geo_LineXSphs(ab2,ab6,cent,rad,2,NULL))>=0&&crss1<=1) return 1;
	if((crss1=Geo_LineXSphs(ab3,ab7,cent,rad,2,NULL))>=0&&crss1<=1) return 1;
	if((crss1=Geo_LineXSphs(ab4,ab8,cent,rad,2,NULL))>=0&&crss1<=1) return 1;
	if((crss1=Geo_LineXSphs(ab5,ab6,cent,rad,2,NULL))>=0&&crss1<=1) return 1;
	if((crss1=Geo_LineXSphs(ab6,ab8,cent,rad,2,NULL))>=0&&crss1<=1) return 1;
	if((crss1=Geo_LineXSphs(ab8,ab7,cent,rad,2,NULL))>=0&&crss1<=1) return 1;
	if((crss1=Geo_LineXSphs(ab7,ab5,cent,rad,2,NULL))>=0&&crss1<=1) return 1;

	v1[0]=cent[0]+1;			// check wholly inside vs. wholly outside
	v1[1]=cent[1];
	sgn=0;
	crss1=Geo_LineXLine(cent,v1,ab1,ab2,&crss2);
	if(crss2>=0&&crss2<=1) sgn=sign(crss1);
	crss1=Geo_LineXLine(cent,v1,ab2,ab4,&crss2);
	if(crss2>=0&&crss2<=1) {
		if(!sgn) sgn=sign(crss1);
		else if(sgn!=sign(crss1)) return 1; }
	crss1=Geo_LineXLine(cent,v1,ab4,ab3,&crss2);
	if(crss2>=0&&crss2<=1) {
		if(!sgn) sgn=sign(crss1);
		else if(sgn!=sign(crss1)) return 1; }
	crss1=Geo_LineXLine(cent,v1,ab3,ab1,&crss2);
	if(crss2>=0&&crss2<=1) {
		if(!sgn) sgn=sign(crss1);
		else if(sgn!=sign(crss1)) return 1; }
	crss1=Geo_LineXLine(cent,v1,ab1,ab5,&crss2);
	if(crss2>=0&&crss2<=1) {
		if(!sgn) sgn=sign(crss1);
		else if(sgn!=sign(crss1)) return 1; }
	crss1=Geo_LineXLine(cent,v1,ab2,ab6,&crss2);
	if(crss2>=0&&crss2<=1) {
		if(!sgn) sgn=sign(crss1);
		else if(sgn!=sign(crss1)) return 1; }
	crss1=Geo_LineXLine(cent,v1,ab3,ab7,&crss2);
	if(crss2>=0&&crss2<=1) {
		if(!sgn) sgn=sign(crss1);
		else if(sgn!=sign(crss1)) return 1; }
	crss1=Geo_LineXLine(cent,v1,ab4,ab8,&crss2);
	if(crss2>=0&&crss2<=1) {
		if(!sgn) sgn=sign(crss1);
		else if(sgn!=sign(crss1)) return 1; }
	crss1=Geo_LineXLine(cent,v1,ab5,ab6,&crss2);
	if(crss2>=0&&crss2<=1) {
		if(!sgn) sgn=sign(crss1);
		else if(sgn!=sign(crss1)) return 1; }
	crss1=Geo_LineXLine(cent,v1,ab6,ab8,&crss2);
	if(crss2>=0&&crss2<=1) {
		if(!sgn) sgn=sign(crss1);
		else if(sgn!=sign(crss1)) return 1; }
	crss1=Geo_LineXLine(cent,v1,ab8,ab7,&crss2);
	if(crss2>=0&&crss2<=1) {
		if(!sgn) sgn=sign(crss1);
		else if(sgn!=sign(crss1)) return 1; }
	crss1=Geo_LineXLine(cent,v1,ab7,ab5,&crss2);
	if(crss2>=0&&crss2<=1) {
		if(!sgn) sgn=sign(crss1);
		else if(sgn!=sign(crss1)) return 1; }
	return 0; }


/******************** approx. Xaabb ************/

int Geo_SemicXaabb2(double *cent,double rad,double *outvect,double *bpt1,double *bpt2) {
	double r1[2],r2[2],r3[2];

	if(!Geo_CircleXaabb2(cent,rad,bpt1,bpt2)) return 0;
	Geo_Semic2Rect(cent,rad,outvect,r1,r2,r3);
	return Geo_RectXaabb2(r1,r2,r3,bpt1,bpt2); }


int Geo_HemisXaabb3(double *cent,double rad,double *outvect,double *bpt1,double *bpt2) {
	double r1[3],r2[3],r3[3],r4[3];

	if(!Geo_SphsXaabb3(cent,rad,bpt1,bpt2)) return 0;
	Geo_Hemis2Rect(cent,rad,outvect,r1,r2,r3,r4);
	return Geo_RectXaabb3(r1,r2,r3,r4,bpt1,bpt2); }


int Geo_CylsXaabb3(double *pt1,double *pt2,double rad,double *bpt1,double *bpt2) {
	double r1[3],r2[3],r3[3],r4[3];

	if(!Geo_CylisXaabb3(pt1,pt2,rad,bpt1,bpt2)) return 0;
	Geo_Cyl2Rect(pt1,pt2,rad,r1,r2,r3,r4);
	return Geo_RectXaabb3(r1,r2,r3,r4,bpt1,bpt2); }


/******************** volumes ****************/

double Geo_SphVolume(double rad,int dim) {
	double vol;

	if(dim==0) vol=1;
	else if(dim==1) vol=2*rad;
	else if(dim==2) vol=PI*rad*rad;
	else if(dim==3) vol=4.0/3.0*PI*rad*rad*rad;
	else vol=2.0/(dim*exp(gammaln(dim/2.0)))*intpower(SQRTPI*rad,dim);
	return vol; }


double Geo_SphOLSph(double *cent1,double *cent2,double r1,double r2,int dim) {
	double ol,dist;
	int d;

	dist=0;
	for(d=0;d<dim;d++) dist+=(cent2[d]-cent1[d])*(cent2[d]-cent1[d]);
	dist=sqrt(dist);
	if(dist>=r1+r2) ol=0;
	else if(dist+r2<=r1) ol=Geo_SphVolume(r2,dim);
	else if(dist+r1<=r2) ol=Geo_SphVolume(r1,dim);
	else {
		if(dim==1)
			ol=r1+r2-dist;
		else if(dim==2)
			ol=r1*r1*acos((dist*dist+r1*r1-r2*r2)/(2*dist*r1))+r2*r2*acos((dist*dist+r2*r2-r1*r1)/(2*dist*r2))-0.5*sqrt((-dist+r1+r2)*(dist+r1-r2)*(dist-r1+r2)*(dist+r1+r2));
		else if(dim==3)
			ol=PI*(r2+r1-dist)*(r2+r1-dist)*(dist*dist+2*dist*r1-3*r1*r1+2*dist*r2+6*r1*r2-3*r2*r2)/(12*dist);
		else ol=-1; }
	return ol; }