ZOJ 3608 Signal Detection

#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;
}