空间向量夹角

空间夹角公式见下图：

 代码：

#include <iostream>
#include <cmath>
using namespace std;


const double eps = 1e-11;

struct Node {
	double x, y, z;
	Node() { x = y = z = 0; }
	Node(double a, double b, double c) { x = a; y = b; z = c; }
	Node operator- ( Node nd ) {
		return Node(x-nd.x, y-nd.y, z-nd.z);
	}
};



Node goodEye, goodFeet, badEye, badFeet, dir;
double h1, r1, h2, r2;


void input() {
	scanf("%lf %lf %lf %lf %lf", &h1, &r1, &badFeet.x, &badFeet.y, &badFeet.z);
	scanf("%lf %lf", &h2, &r2 );
	scanf( "%lf %lf %lf", &goodFeet.x, &goodFeet.y, &goodFeet.z);
	scanf( "%lf %lf %lf", &dir.x, &dir.y, &dir.z );
}


double dis( Node a, Node b ) {      //求出空间两点距离
	a.x -= b.x;
	a.y -= b.y;
	a.z -= b.z;
	return sqrt(a.x*a.x + a.y*a.y + a.z*a.z);
}


double getDree( Node a, Node b ) { //求向量a，b之间的夹角
	double c, d;
	c = a.x*b.x + a.y*b.y + a.z*b.z;
	d = sqrt(a.x*a.x + a.y*a.y + a.z*a.z) * sqrt(b.x*b.x + b.y*b.y + b.z*b.z);
	return acos(c/d);
}



bool solve() {
	double limit, now, len;
	Node a;
	goodEye.x = goodFeet.x;
	goodEye.y = goodFeet.y;
	goodEye.z = goodFeet.z + h2 * 0.9 - r2;

	badEye.x = badFeet.x;
	badEye.y = badFeet.y;
	badEye.z = badFeet.z + h1 - r1;

	len = dis( badEye, goodEye );
	limit = asin( r1/len );     //求出边界
	a = badEye - goodEye;
	now = getDree(a, dir);
	if( now - limit > eps ) return 0;
	return 1;
}


int main() {
//	freopen( "c:/aaa.txt", "r", stdin );
	int T;
	scanf( "%d", &T);
	while( T-- ) {
		input();
		solve() ? puts( "YES" ) : puts( "NO" );
	}
	return 0;
}