模板：三维计算几何

三维计算几何

typedef double lf;
const lf EPS=1e-9,INF=1e9,PI=acos(-1);

int sgn(lf d)
{
	return (d>EPS)-(d<-EPS);
}

struct Coord3
{
	lf X,Y,Z;
	Coord3(lf X=0,lf Y=0,lf Z=0):X(X),Y(Y),Z(Z) {}
	friend bool operator!=(const Coord3 &a,const Coord3 &b)
	{
		return sgn(a.X-b.X)||sgn(a.Y-b.Y)||sgn(a.Z-b.Z);
	}
	friend bool operator==(const Coord3 &a,const Coord3 &b)
	{
		return !(a!=b);
	}
	Coord3& operator+=(const Coord3 &b)
	{
		return X+=b.X,Y+=b.Y,Z+=b.Z,*this;
	}
	friend Coord3 operator+(Coord3 a,const Coord3 &b)
	{
		return a+=b;
	}
	Coord3& operator-=(const Coord3 &b)
	{
		return X-=b.X,Y-=b.Y,Z-=b.Z,*this;
	}
	friend Coord3 operator-(Coord3 a,const Coord3 &b)
	{
		return a-=b;
	}
	Coord3& operator*=(lf d)
	{
		return X*=d,Y*=d,Z*=d,*this;
	}
	friend Coord3 operator*(Coord3 a,lf d)
	{
		return a*=d;
	}
	friend Coord3 operator*(lf d,Coord3 a)
	{
		return a*=d;
	}
	Coord3& operator/=(lf d)
	{
		return X/=d,Y/=d,Z/=d,*this;
	}
	friend Coord3 operator/(Coord3 a,lf d)
	{
		return a/=d;
	}
};

struct Line3
{
	Coord3 p,v;
	Line3(Coord3 p=Coord3(),Coord3 v=Coord3()):p(p),v(v) {}
	Coord3 point(lf t)const
	{
		return p+v*t;
	}
};

struct Sphere
{
	Coord3 o;
	lf r;
	Sphere(Coord3 o=Coord3(),lf r=0):o(o),r(r) {}
	Coord3 point(lf lat,lf lng)const//经纬度确定球面上一点
	{
		lat*=PI/180;
		lng*=PI/180;
		return o+r*Coord3(cos(lat)*cos(lng),cos(lat)*sin(lng),sin(lat));
	}
};

lf Dot(const Coord3& A,const Coord3& B)
{
	return A.X*B.X+A.Y*B.Y+A.Z*B.Z;
}

Coord3 Cross(const Coord3& A,const Coord3& B)
{
	return Coord3(A.Y*B.Z-A.Z*B.Y,A.Z*B.X-A.X*B.Z,A.X*B.Y-A.Y*B.X);
}

lf norm(const Coord3& A)
{
	return Dot(A,A);
}

lf abs(const Coord3& A)
{
	return sqrt(norm(A));
}

lf Angle(const Coord3& A,const Coord3& B)
{
	return acos(Dot(A,B)/abs(A)/abs(B));
}

lf Area2(Coord3 A,Coord3 B,Coord3 C)
{
	return abs(Cross(B-A,C-A));
}

lf Volume6(Coord3 A,Coord3 B,Coord3 C,Coord3 D)//四面体体积
{
	return Dot(D-A,Cross(B-A,C-A));
}

Coord3 Centroid(Coord3 A,Coord3 B,Coord3 C,Coord3 D)//四面体的重心
{
	return (A+B+C+D)/4.0;
}

lf DistanceToPlane(Coord3 p,Coord3 p0,const Coord3& n)//点p到平面p0-n的有向距离
{
	return Dot(p-p0,n)/abs(n);
}

Coord3 getPlaneProjection(Coord3 p,Coord3 p0,const Coord3& n)//点p在平面p0-n上的投影。n必须为单位向量
{
	return p-n*Dot(p-p0,n);
}

Coord3 LinePlaneIntersection(Coord3 p1,Coord3 p2,Coord3 p0,Coord3 n)//直线p1-p2 与平面p0-n的交点，假设交点唯一存在
{
	Coord3 v=p2-p1;
	lf t=Dot(n,p0-p1)/Dot(n,p2-p1);//分母为0，直线与平面平行或在平面上
	return p1+v*t;//如果是线段 判断t是否在0~1之间
}

lf DistanceToLine(Coord3 P,Coord3 A,Coord3 B)//点P到直线AB的距离
{
	Coord3 v1=B-A,v2=P-A;
	return abs(Cross(v1,v2))/abs(v1);
}

lf DistanceToSeg(Coord3 P,Coord3 A,Coord3 B)//点到线段的距离
{
	if(A==B)return abs(P-A);
	Coord3 v1=B-A,v2=P-A,v3=P-B;
	if(sgn(Dot(v1,v2))<0)return abs(v2);
	if(sgn(Dot(v1,v3))>0)return abs(v3);
	return fabs(DistanceToLine(P,A,B));
}

bool LineDistance3D(Coord3 p1,Coord3 u,Coord3 p2,Coord3 v,lf& s)//求异面直线 p1+s*u与p2+t*v的公垂线对应的s，如果平行|重合，返回0
{
	lf b=Dot(u,u)*Dot(v,v)-Dot(u,v)*Dot(u,v);
	if(!sgn(b))return 0;
	lf a=Dot(u,v)*Dot(v,p1-p2)-Dot(v,v)*Dot(u,p1-p2);
	return s=a/b,1;
}

bool SameSide(Coord3 p1,Coord3 p2,Coord3 a,Coord3 b)//p1和p2是否在线段a-b的同侧
{
	return sgn(Dot(Cross(b-a,p1-a),Cross(b-a,p2-a)))>=0;
}

bool PointInTri(Coord3 PP,Coord3 P[3])//点P在三角形P0,P1,p中
{
	return SameSide(PP,P[0],P[1],P[2])&&
	       SameSide(PP,P[1],P[0],P[2])&&
	       SameSide(PP,P[2],P[0],P[1]);
}

bool TriSegIntersection(Coord3 P[3],Coord3 A,Coord3 B,Coord3& PP)//三角形P0P1p是否和线段AB相交，如有则为PP
{
	Coord3 n=Cross(P[1]-P[0],P[2]-P[0]);
	if(sgn(Dot(n,B-A))==0)return false;//线段A-B和平面P0P1p平行或共面
	lf t=Dot(n,P[0]-A)/Dot(n,B-A);//平面A和直线P1-p有惟一交点
	if(sgn(t)<0||sgn(t-1)>0)return false;//不在线段AB上
	return PointInTri(PP=A+(B-A)*t,P);
}

bool TriTriIntersection(Coord3 T1[3],Coord3 T2[3])//空间两三角形是否相交
{
	Coord3 P;
	for(int i=0; i<3; ++i)
		if(TriSegIntersection(T1,T2[i],T2[(i+1)%3],P)||
		        TriSegIntersection(T2,T1[i],T1[(i+1)%3],P))
			return 1;
	return 0;
}

lf SegSegDistance(Coord3 a1,Coord3 b1,Coord3 a2,Coord3 b2,Coord3 &ans1,Coord3 &ans2)//空间两直线上最近点对 返回最近距离 点对保存在ans1 ans2中
{
	Coord3 v1=(a1-b1),v2=(a2-b2);
	Coord3 N=Cross(v1,v2);
	Coord3 ab=(a1-a2);
	lf ans=Dot(N,ab)/abs(N);
	Coord3 d1=b1-a1,d2=b2-a2,cd=Cross(d1,d2);
	lf nd=norm(cd),t1=Dot(Cross(a2-a1,d2),cd)/nd,t2=Dot(Cross(a2-a1,d1),cd)/nd;
	return ans1=a1+(b1-a1)*t1,ans2=a2+(b2-a2)*t2,fabs(ans);
}

bool InsideWithMinDistance(Coord3 PP,Coord3 *P,lf dist)//判断PP是否在三角形P中，并且到三条边的距离都至少为dist。保证P,A,B,C共面
{
	return PointInTri(PP,P)&&
	       DistanceToLine(PP,P[0],P[1])>=dist||
	       DistanceToLine(PP,P[1],P[2])>=dist||
	       DistanceToLine(PP,P[2],P[0])>=dist;
}

struct ConvexPolyhedron//空间多边形和凸包问题
{
	struct Face
	{
		int v[3];
		Face(int a,int b,int c)
		{
			v[0]=a,v[1]=b,v[2]=c;
		}
		Coord3 Normal(const vector<Coord3>& P)const
		{
			return Cross(P[v[1]]-P[v[0]],P[v[2]]-P[v[0]]);
		}
		bool CanSee(const vector<Coord3>& P,int i)const//f是否能看见P[i]
		{
			return Dot(P[i]-P[v[0]],Normal(P))> 0;
		}
	};
	vector<Face> faces;
	vector<Coord3> p;
	ConvexPolyhedron(vector<Coord3> P):p(P)
	{
		for(int i=0; i<p.size(); ++i)P[i]+=Coord3(randEPS(),randEPS(),randEPS());
		vector<vector<int> > vis(P.size(),vector<int>(P.size()));
		faces.push_back(Face(0,1,2));//由于已经进行扰动，前三个点不共线
		faces.push_back(Face(2,1,0));
		for(int i=3; i<P.size(); ++i)
		{
			vector<Face> next;
			for(int j=0; j<faces.size(); ++j)//计算每条边的“左面”的可见性
			{
				Face& f=faces[j];
				int res=f.CanSee(P,i);
				if(!res)next.push_back(f);
				for(int k=0; k<3; ++k)vis[f.v[k]][f.v[(k+1)%3]]=res;
			}
			for(int j=0; j<faces.size(); ++j)
				for(int k=0; k<3; ++k)
				{
					int a=faces[j].v[k],b=faces[j].v[(k+1)%3];
					if(vis[a][b]!=vis[b][a]&&vis[a][b])//(a,b)是分界线，左边对P[i]可见
						next.push_back(Face(a,b,i));
				}
			swap(faces,next);
		}
	}
	lf randEPS()
	{
		return (rand()/lf(RAND_MAX)-0.5)*EPS;
	}
	Coord3 centroid()//三维凸包重心
	{
		Coord3 C=p[0],tot(0,0,0);
		lf totv=0;
		for(int i=0; i<faces.size(); ++i)
		{
			Coord3 p1=p[faces[i].v[0]],p2=p[faces[i].v[1]],p3=p[faces[i].v[2]];
			lf v=-Volume6(p1,p2,p3,C);
			totv+=v;
			tot+=v*Centroid(p1,p2,p3,C);
		}
		return tot/totv;
	}
	lf dist(Coord3 C)//凸包内一点到表面最近距离
	{
		lf ans=INF;
		for(int i=0; i<faces.size(); ++i)
		{
			Coord3 p1=p[faces[i].v[0]],p2=p[faces[i].v[1]],p3=p[faces[i].v[2]];
			ans=min(ans,fabs(-Volume6(p1,p2,p3,C)/Area2(p1,p2,p3)));
		}
		return ans;
	}
};