SGU 110. Dungeon

Link To The Problem

知识点： 三维 直线和球的交、向量旋转

Code:

// SGU 110 Dungeon
// 三维直线和圆的交、向量旋转

#include<cstdio>
#include<cstring>
#include<iostream>
#include<algorithm>
#include<vector>
#include<cmath>

using namespace std;

#define nMax 10010
#define sf scanf
#define pf printf
#define rep(i,n) for(int (i)=0;(i)<(n);(i)++)

#define pb push_back
#define mp make_pair

double const eps = 1e-9;
double const pi = acos(-1.0);

inline int dcmp(double x) {
	if(x>=-eps && x<=eps) return 0;
	return x > 0 ? 1 : -1;
}

// Data Struct Part:


/***********基础*************/
struct Point3 {
  double x, y, z;
  Point3(double x=0, double y=0, double z=0):x(x),y(y),z(z) { }
  void read() { sf("%lf%lf%lf",&x,&y,&z); }
  double len() { return sqrt(x*x+y*y+z*z); }
  void out() { pf("(%.4lf, %.4lf, %.4lf)\n",x,y,z); }
};

typedef Point3 Vector3;

Vector3 operator + (const Vector3& A, const Vector3& B) { return Vector3(A.x+B.x, A.y+B.y, A.z+B.z); }
Vector3 operator - (const Point3& A, const Point3& B) { return Vector3(A.x-B.x, A.y-B.y, A.z-B.z); }
Vector3 operator * (const Vector3& A, double p) { return Vector3(A.x*p, A.y*p, A.z*p); }
Vector3 operator * (const Vector3& A, const 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); }
Vector3 operator / (const Vector3& A, double p) { return Vector3(A.x/p, A.y/p, A.z/p); }
double  operator ^ (const Vector3& A, const Vector3& B) { return A.x*B.x + A.y*B.y + A.z*B.z; }
bool operator == (const Point3& A,const Point3& B)  { return dcmp(A.x-B.x)==0 && dcmp(A.y-B.y)==0 && dcmp(A.z-B.z)==0 ; }

struct Line3{
	Point3 a,b;
	Line3(){};
	Line3(Point3 a,Point3 b):a(a),b(b){};
};
struct Sphere{
	Point3 O;
	double R;
	void read() {
		O.read();
		sf("%lf",&R);
	}
};

// Basic Function Part:

//点到直线的距离
double PToL(Point3 p,Line3 l){
	Vector3 v1 = l.b-l.a, v2 = p - l.a;
	return (v1*v2).len() / v1.len();
}
//射线ab与圆的第一个交点
#define bug puts("Here");
int LineInjectSphere(Point3 fr,Point3 to,Sphere s,double& ret){
	double d = PToL(s.O,Line3(fr,to));
	//cout << d << ' ' << s.R << endl;
	if(dcmp(d-s.R)>0) return 0;
	double ss = s.R*s.R-d*d;
	if(ss<0) ss=0;
	ss = sqrt(ss);
	Vector3 v1=to-fr,v2=s.O-fr;
	d = (v1^v2)/v1.len();
	d -= ss;
	if(dcmp(d)<=0) return 0;
	ret = d;
	return 1;
}


Point3 Rotate(Point3 O,Point3 a,Point3 b,double angle){
	a = a-O;
	b = b-O;
	Point3 e1,e2,e3;
	e3 = b/b.len();
	double len = a^e3;
	Point3 p=e3*len;
	e1=a-p;
	if(dcmp(e1.len())>0) e1=e1/e1.len();
	e2 = e1*e3;
	double x1=a^e1,y1=a^e2;
	double x = -x1;
	double y = -y1;
	return e1*x+e2*y+p+O;
}

// Sovle Part

int n;
Sphere s[nMax];
Point3 fr,to;

void work() {
	//to = to-fr;
	for(int i=0;i<11;i++) {
	//	fr.out(),(to-fr).out();
		int ok=0;
		double dist=1e9,tmp;
		int cur ;
		rep(j,n) {
			if(LineInjectSphere(fr,to,s[j],tmp)){
				if(dcmp(tmp-dist)<0) {
					dist = tmp;
					cur = j;
					ok = 1;
				}
			}
		}
		if(!ok) {
			pf("\n");
			return ;
		}
		if(i) pf(" ");
		if(i==10) {
			pf("etc.\n");
			return ;
		}
		pf("%d",cur+1);
		Vector3 v = (to-fr);
		v = v/v.len();
		Point3 nfr = fr + v*dist;
		Vector3 n = nfr;
		to = Rotate(s[cur].O,fr,nfr,pi);
		fr = nfr;
	}
}



int main() {
#ifndef ONLINE_JUDGE
	freopen("in.txt","r",stdin);
	freopen("out.txt","w",stdout);
#endif

	while(~sf("%d",&n)){
		rep(i,n) s[i].read();
		fr.read(),to.read();
		work();
	}
	return 0;
}