三维几何
struct Coord3
{
double X,Y,Z;
Coord3(double X=0,double Y=0,double Z=0):X(X),Y(Y),Z(Z) {}
};
int sgn(double d)
{
return (d>EPS)-(d<-EPS);
}
bool operator!=(const Coord3 &a,const Coord3 &b)
{
return sgn(a.X-b.X)||sgn(a.Y-b.Y)||sgn(a.Z-b.Z);
}
bool operator==(const Coord3 &a,const Coord3 &b)
{
return !(a!=b);
}
Coord3& operator+=(Coord3 &a,const Coord3 &b)
{
return a.X+=b.X,a.Y+=b.Y,a.Z+=b.Z,a;
}
Coord3 operator+(Coord3 a,const Coord3 &b)
{
return a+=b;
}
Coord3& operator-=(Coord3 &a,const Coord3 &b)
{
return a.X-=b.X,a.Y-=b.Y,a.Z-=b.Z,a;
}
Coord3 operator-(Coord3 a,const Coord3 &b)
{
return a-=b;
}
Coord3& operator*=(Coord3 &a,double d)
{
return a.X*=d,a.Y*=d,a.Z*=d,a;
}
Coord3 operator*(Coord3 a,double d)
{
return a*=d;
}
Coord3 operator*(double d,Coord3 a)
{
return a*=d;
}
Coord3& operator/=(Coord3 &a,double d)
{
return a.X/=d,a.Y/=d,a.Z/=d,a;
}
Coord3 operator/(Coord3 a,double d)
{
return a/=d;
}
double 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);
}
double norm(const Coord3& A)
{
return Dot(A,A);
}
double abs(const Coord3& A)
{
return sqrt(norm(A));
}
double Angle(const Coord3& A,const Coord3& B)
{
return acos(Dot(A,B)/abs(A)/abs(B));
}
double Area2(Coord3 A,Coord3 B,Coord3 C)
{
return abs(Cross(B-A,C-A));
}
double 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;
}
double DistanceToPlane(Coord3 p,Coord3 p0,const Coord3& n)
{
return Dot(p-p0,n)/abs(n);
}
Coord3 getPlaneProjection(Coord3 p, Coord3 p0, const Coord3& n)
{
return p-n*Dot(p-p0,n);
}
Coord3 LinePlaneIntersection(Coord3 p1, Coord3 p2, Coord3 p0, Coord3 n)
{
Coord3 v = p2-p1;
double t = Dot(n, p0-p1) / Dot(n, p2-p1);
return p1 + v*t;
}
double DistanceToLine(Coord3 P,Coord3 A,Coord3 B)
{
Coord3 v1=B-A,v2=P-A;
return abs(Cross(v1,v2))/abs(v1);
}
double 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, double& s)
{
double b = Dot(u, u) * Dot(v, v) - Dot(u, v) * Dot(u, v);
if(!sgn(b)) return 0;
double a = Dot(u, v) * Dot(v, p1-p2) - Dot(v, v) * Dot(u, p1-p2);
s = a/b;
return 1;
}
bool SameSide(Coord3 p1, Coord3 p2, Coord3 a, Coord3 b)
{
return sgn(Dot(Cross(b-a, p1-a), Cross(b-a, p2-a)))>=0;
}
bool PointInTri(Coord3 PP, Coord3 *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,Coord3 A,Coord3 B,Coord3& PP)
{
Coord3 n = Cross(P[1]-P[0], P[2]-P[0]);
if(sgn(Dot(n, B-A)) == 0) return false;
double t = Dot(n, P[0]-A) / Dot(n, B-A);
if(sgn(t) < 0 || sgn(t-1) > 0) return false;
PP = A + (B-A)*t;
return PointInTri(PP, P);
}
bool TriTriIntersection(Coord3* T1, Coord3* T2)
{
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;
}
double SegSegDistance(Coord3 a1, Coord3 b1, Coord3 a2, Coord3 b2,Coord3 &ans1,Coord3 &ans2)
{
Coord3 v1 = (a1-b1), v2 = (a2-b2);
Coord3 N = Cross(v1, v2);
Coord3 ab = (a1-a2);
double ans = Dot(N, ab) / abs(N);
Coord3 p1 = a1, p2 = a2;
Coord3 d1 = b1-a1, d2 = b2-a2;
double t1, t2;
t1 = Dot((Cross(p2-p1, d2)), Cross(d1, d2));
t2 = Dot((Cross(p2-p1, d1)), Cross(d1, d2));
double dd = abs((Cross(d1, d2)));
t1 /= dd*dd;
t2 /= dd*dd;
ans1 = (a1 + (b1-a1)*t1);
ans2 = (a2 + (b2-a2)*t2);
return fabs(ans);
}
bool InsideWithMinDistance(Coord3 PP, Coord3 *P, double mindist)
{
return PointInTri(PP,P)&&
DistanceToLine(PP,P[0],P[1])>=mindist||
DistanceToLine(PP,P[1],P[2])>=mindist||
DistanceToLine(PP,P[2],P[0])>=mindist;
}
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
{
return Dot(P[i]-P[v[0]], Normal(P)) > 0;
}
};
double randeps()
{
return (rand()/(double)RAND_MAX-0.5)*EPS;
}
void add_noise(Coord3 &p)
{
p+=Coord3(randeps(),randeps(),randeps());
}
vector<Coord3> P;
vector<Face> faces;
void read(vector<Coord3> P2)
{
P=P2;
for(int i = 0; i < P2.size(); i++)
add_noise(P2[i]);
faces = CH3D(P2);
}
vector<Face> CH3D(const vector<Coord3>& P)
{
int n = P.size();
vector<vector<int> > vis(n);
for(int i = 0; i < n; i++) vis[i].resize(n);
vector<Face> cur;
cur.push_back(Face(0, 1, 2));
cur.push_back(Face(2, 1, 0));
for(int i = 3; i < n; i++)
{
vector<Face> neXt;
for(int j = 0; j < cur.size(); j++)
{
Face& f = cur[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 < cur.size(); j++)
for(int k = 0; k < 3; k++)
{
int a = cur[j].v[k], b = cur[j].v[(k+1)%3];
if(vis[a][b] != vis[b][a] && vis[a][b])
neXt.push_back(Face(a, b, i));
}
cur = neXt;
}
return cur;
}
Coord3 centroid()
{
Coord3 C = P[0],tot(0,0,0);
double 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]];
double v = -Volume6(p1, p2, p3, C);
totv += v;
tot = tot + Centroid(p1, p2, p3, C)*v;
}
return tot / totv;
}
double mindist(Coord3 C)
{
double ans = 1e30;
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;
}
};