ZOJ 3608 Signal Detection

来源:互联网 发布:淘宝网王小帮 编辑:程序博客网 时间:2024/05/24 01:44
#include<iostream>#include<cstdio>#include<cstring>#include<cmath>using namespace std;const double eps=1e-6;typedef struct point3{double x,y,z;point3(double x=0,double y=0,double z=0):x(x),y(y),z(z){}}vector3;struct face{point3 a,b,c,d;vector3 n;face(){}face(point3 a,point3 b,point3 c,point3 d,vector3 n):a(a),b(b),c(c),d(d),n(n){}};vector3 operator + (vector3 a,vector3 b){return vector3(a.x+b.x,a.y+b.y,a.z+b.z);}vector3 operator - (vector3 a,vector3 b){return vector3(a.x-b.x,a.y-b.y,a.z-b.z);}vector3 operator * (vector3 a,double b){return vector3(a.x*b,a.y*b,a.z*b);}vector3 operator / (vector3 a,double b){return vector3(a.x/b,a.y/b,a.z/b);}double dot(vector3 a,vector3 b){return a.x*b.x+a.y*b.y+a.z*b.z;}double len(vector3 a){return sqrt(dot(a,a));}double angle(vector3 a,vector3 b){return acos(dot(a,b)/len(a)/len(b));}vector3 cross(vector3 a,vector3 b){return vector3(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 area(point3 a,point3 b,point3 c){return len(cross(b-a,c-a));}int dcmp(double x){if(fabs(x)<eps) return 0;else return x<0 ? -1 : 1;}bool pointint(point3 p,point3 p0,point3 p1,point3 p2){double area1=area(p,p0,p1);double area2=area(p,p1,p2);double area3=area(p,p2,p0);return dcmp(area1+area2+area3-area(p0,p1,p2))==0;}int tri(point3 p0,point3 p1,point3 p2,point3 a,point3 b,point3 &p){vector3 n=cross(p1-p0,p2-p0);if(dcmp(dot(n,b-a))==0) return 0;else{double t=dot(n,p0-a)/dot(n,b-a);if(dcmp(t)<0) return 0;p=a+(b-a)*t;return pointint(p,p0,p1,p2);}}void getf(point3 a,point3 b,point3 c,point3 d,face &f1,face &f2){vector3 n=cross(b-a,c-a);if(dcmp(dot(n,d-a))>0) n=n*(-1.0);f1=face(a,b,c,c+b-a,n);vector3 v=d-a;f2=face(d,b+v,c+v,c+b-a+v,n*(-1.0));}double dist(point3 p,point3 a,point3 b){vector3 v1=b-a,v2=p-a;return len(cross(v1,v2))/len(v1);}point3 resiz(point3 a,double d){    d/=len(a);    return a*d;}point3 rot(point3 p,point3 s,point3 e,double ang){    point3 fa1=cross(e-s,p-s);    point3 fa2=cross(e-s,fa1);    double le=fabs(area(p,e,s)/len(e-s));    fa2=resiz(fa2,le);    fa1=resiz(fa1,le);    point3 h=p+fa2;    point3 pp=h+fa1;    point3 re=h+(p-h)*cos(ang)+(pp-h)*sin(ang);    return re;}int main(){int t,i;point3 a,b,p[4],m;vector3 n;double r,d1,d2;face f[6];scanf("%d",&t);while(t--){scanf("%lf%lf%lf",&a.x,&a.y,&a.z);scanf("%lf%lf%lf",&b.x,&b.y,&b.z);for(i=0;i<4;i++){scanf("%lf%lf%lf",&p[i].x,&p[i].y,&p[i].z);}scanf("%lf",&r);int floor=0;for(i=0;i<3;i++){getf(p[0],p[i+1],p[(i+1)%3+1],p[(i+2)%3+1],f[i],f[i+3]);}for(i=0;i<3;i++){if(dcmp(dot(b-a,f[i].n))==0){    continue;}else if(dcmp(dot(b-a,f[i].n))<0){if(tri(f[i].a,f[i].b,f[i].c,a,b,m)){d1=dcmp(dist(m,f[i].a,f[i].b));d2=dcmp(dist(m,f[i].a,f[i].c));if(d1 && d2) floor=i+1;else floor--;}else if(tri(f[i].d,f[i].b,f[i].c,a,b,m)){d1=dcmp(dist(m,f[i].d,f[i].b));d2=dcmp(dist(m,f[i].d,f[i].c));if(d1 && d2) floor=i+1;else floor--;}}else{if(tri(f[i+3].a,f[i+3].b,f[i+3].c,a,b,m)){d1=dcmp(dist(m,f[i+3].a,f[i+3].b));d2=dcmp(dist(m,f[i+3].a,f[i+3].c));if(d1 && d2) floor=i+3+1;else floor--;}else if(tri(f[i+3].d,f[i+3].b,f[i+3].c,a,b,m)){d1=dcmp(dist(m,f[i+3].d,f[i+3].b));d2=dcmp(dist(m,f[i+3].d,f[i+3].c));if(d1 && d2) floor=i+3+1;else floor--;}}if(floor>0) break;}if(floor==0 || floor==-1){if(dcmp(b.z-a.z)>=0) printf("Error\n");else{double nn=a.z/(a.z-b.z);printf("%.3lf %.3lf\n",a.x+(b.x-a.x)*nn,a.y+(b.y-a.y)*nn);}continue;}else if(floor>0){n=f[floor-1].n*(-1.0);double ang1=angle(n,b-a);double ang2=asin(sin(ang1)/r);vector3 vn=cross(n,b-a);a=m;b=rot(m+n,m,m+vn,ang2);}int fl=0;for(i=0;i<3;i++){    if(dcmp(dot(b-a,f[i].n))==0) continue;else if(dcmp(dot(b-a,f[i].n))>0){if(tri(f[i].a,f[i].b,f[i].c,a,b,m)){d1=dcmp(dist(m,f[i].a,f[i].b));d2=dcmp(dist(m,f[i].a,f[i].c));if(d1 && d2) fl=i+1;else fl--;}else if(tri(f[i].d,f[i].b,f[i].c,a,b,m)){d1=dcmp(dist(m,f[i].d,f[i].b));d2=dcmp(dist(m,f[i].d,f[i].c));if(d1 && d2) fl=i+1;else fl--;}}else{if(tri(f[i+3].a,f[i+3].b,f[i+3].c,a,b,m)){d1=dcmp(dist(m,f[i+3].a,f[i+3].b));d2=dcmp(dist(m,f[i+3].a,f[i+3].c));if(d1 && d2) fl=i+3+1;else fl--;}else if(tri(f[i+3].d,f[i+3].b,f[i+3].c,a,b,m)){d1=dcmp(dist(m,f[i+3].d,f[i+3].b));d2=dcmp(dist(m,f[i+3].d,f[i+3].c));if(d1 && d2) fl=i+3+1;else fl--;}}if(fl>0) break;}if(fl>0){n=f[fl-1].n;double ang1=angle(n,b-a);double ang2=asin(sin(ang1)*r);vector3 v1=cross(n,b-a);a=m;b=rot(m+n,m,m+v1,ang2);}if(dcmp(b.z-a.z)>=0) printf("Error\n");else{double nn=a.z/(a.z-b.z);printf("%.3lf %.3lf\n",a.x+(b.x-a.x)*nn,a.y+(b.y-a.y)*nn);}}return 0;}

原创粉丝点击