几何处理代码集合

三维空间求两向量逆时针角度

double CCWangle(Point3D A,Point3D B,Point3D n){//A->B 沿n方向看的逆时针角度
	double a = ACOS(A ^ B / (A.Len() * B.Len()));
	Point3D C = A * B;
	double p = C ^ n;
	if (p > 0.0) a = 2 * PI - a;
	return a;
}

三维空间求线段与平面(or三角形)交点

求AB线段和三角面片（abc）的交点，并储存在re中

bool line_in_face(Point3D& a, Point3D& b, Point3D& c, Point3D& A, Point3D& B, Point3D& re) {
	//（1）求线段与面交点
	Point3D n = VectorCross(a, b, c);
	Point3D S = A - B;
	if (S ^ n == 0) return 0;//点乘 共面
	n.Normalize();S.Normalize();
	double d1 = (A - a) ^ n;
	double d2 = (B - a) ^ n;
	if (d1 * d2 > 0) return 0;//同侧 
	d1 = fabs(d1); d2 = fabs(d2);
	re = (d1 / (d1 + d2)) * B + (d2 / (d1 + d2)) * A;//re此时是与面的交点
	//（2）判断交点是否在abc内部
	Point3D AR=a-re;
	Point3D BR=b-re;
	Point3D CR=c-re;
	double angle1 = CCWangle(AR,BR,n);
	if(angle1>PI) angle1=-angle1;
	double angle2 = CCWangle(BR,CR,n);
	if(angle2>PI) angle2=-angle2;
	double angle3 = CCWangle(CR,AR,n);
	if(angle3>PI) angle3=-angle3;
	double ans=angle1+angle2+angle3;
	if(angle==0) return 0;
	return 1;
}

这里判断点是否在三角形内部使用的方法是：回转数法
另外还有一种方法是：
$Z_1=P_1{P}_2\times P_{1}Q$
$Z_2=P_2{P}_3\times P_{2}Q$
$Z_3=P_3{P}_1\times P_{2}Q$
若在内部，则$Z_1\cdot Z_2\cdot Z_3>0$

三维空间异面直线公垂线及交点坐标求取（含共面直线交点求取）

异面直线公垂线及交点坐标求取(Q为直线 k 上一点，S为方向向量)
也能处理共面直线交点
参考思路

Point3D Get_common_perpendicular(int i,int j) {//输出公垂线在j处的交点(要求S1和S2不平行)
	Point3D Q1 = ProjV[i].first;
	Point3D S1 = ProjV[i].second;
	Point3D Q2 = ProjV[j].first;
	Point3D S2 = ProjV[j].second;
	if((S1*S2).Len()==0){
		cout<<"平行"<<endl;
		return Point3D(-1,-1,-1);
	}
	double c1 = (Q1 - Q2) ^ S2;
	double c2 = (Q1 - Q2) ^ S1;
	double a1 = S1 ^ S2;
	double b2 = -a1;
	double a2 = S1.Len2();
	double b1 = -S2.Len2();
	//double t1 = (-b2 * c1 + c2 * b1) / (a1 * b2 - a2 * b1);
	double t2 = (-a1 * c2 + a2 * c1) / (a1 * b2 - a2 * b1);
	return Q2 + t2 * S2;
}

三维空间点在平面上的投影点

Point3D Get_proj(Point3D A, Point3D n, Point3D B) {//n为平面单位法向量,平面过点B 
	double l = ((A - B) ^ n);
	Point3D S1 = A - n * l;
	return S1;
}

三维空间平面两圆交点求取

获得平面Plane上两圆交点 圆1:(A,r1) 圆2:(B,r2)
Plane：平面单位法向量n(该平面过AB)
返回交点要求与点P在AB同侧
方法参考思路

Point3D get_circle_intersection(Point3D &A,Point3D &B,double r1,double r2,Point3D &n,Point3D &P) {//获得两圆交点 圆1:(A,r1) 圆2:(B,r2) 平面单位法向量n(该平面过AB) 返回点与点P在AB同侧
	Point3D e = B - A; e.Normalize();//x轴
	Point3D k = e * n; k.Normalize();//y轴
	double d = (B-A).Len();
	if (dcmp(d) == 0) {
		cout << "error: get_circle_intersection:A B 重合！！" << endl;
		return Point3D(0, 0, 0);
	}
	double a = (d * d + r1 * r1 - r2 * r2) / (2 * d);
	if (dcmp(a * a - r1 * r1) == 0) {//相切
		cout << "error: get_circle_intersection:A B 相切！！" << endl;
		//p = A + a * e;
		return A + a * e;
	}
	if (a * a - r1 * r1 > 0.0) {//相离
		cout << "error: get_circle_intersection:A B 相离！！" << endl;
		cout << "a :" << a << " r1 :" << r1 << endl;
		return A + a * e;
	}
	//相交处理：p = A + a * e ± h * k;
	double h = sqrt(r1 * r1 - a * a);
	Point3D t = A + a * e + h * k;//交点
	//判断t是否和P在AB同一侧
	Point3D C = (B - A) * (P - A);
	Point3D D = (B - A) * (t - A);
	double q = C ^ D;
	if (q> 0.0) {
		//cout << "Get insect" << endl;
		return t;
	}
	t = A + a * e - h * k;
	//判断t是否和P在AB同一侧
	C = (B - A) * (P - A);
	D = (B - A) * (t - A);
	q = C ^ D;
	if (q > 0.0) {
		//cout << "Get insect" << endl;
		return t;
	}
	cout << "error: get_circle_intersection:两侧的点都不满足和P在一侧！！（P在AB线上）" << endl;
	return Point3D(0, 0, 0);
}

Quadric Error Mactrics

参考：

void testQEM() {

	vector<Eigen::Vector3d>     InputPos;//读入点
	vector<Eigen::Vector3d>     InputNormal;
	InputPos.push_back(Eigen::Vector3d(1.0, 1.0, 0.0));
	InputPos.push_back(Eigen::Vector3d(1.0, 0.0, 1.0));
	InputPos.push_back(Eigen::Vector3d(0.0, 1.0, 1.0));
	InputPos.push_back(Eigen::Vector3d(1.0, 1.0, 1.0));

	InputNormal.push_back(Eigen::Vector3d(0.0, 0.0, 1.0));
	InputNormal.push_back(Eigen::Vector3d(0.0, 1.0, 0.0));
	InputNormal.push_back(Eigen::Vector3d(1.0, 0.0, 0.0));
	InputNormal.push_back(Eigen::Vector3d(0.0, 0.0, 1.0));


	set<int> s;
	s.insert(0);
	s.insert(1);
	s.insert(2);
	s.insert(3);

	Eigen::Matrix4d Q, A;//默认列优先
	Eigen::Vector4d n, x;
	Eigen::Vector4d b(0, 0, 0, 1);
	Q = Eigen::Matrix4d::Zero();
	for (auto ite = s.begin(); ite != s.end(); ite++) {
		if (*ite == -1) continue;
		double fd = InputNormal[*ite].dot(InputPos[*ite]);
		n = Eigen::Vector4d(InputNormal[*ite].x(), InputNormal[*ite].y(), InputNormal[*ite].z(), -fd);
		Q += n * n.transpose();
	}
	A(0, 0) = Q(0, 0);
	A(1, 0) = Q(0, 1);
	A(2, 0) = Q(0, 2);
	A(3, 0) = 0.0;
	A(0, 1) = Q(0, 1);
	A(1, 1) = Q(1, 1);
	A(2, 1) = Q(1, 2);
	A(3, 1) = 0.0;
	A(0, 2) = Q(0, 2);
	A(1, 2) = Q(1, 2);
	A(2, 2) = Q(2, 2);
	A(3, 2) = 0.0;
	A(0, 3) = Q(0, 3);
	A(1, 3) = Q(1, 3);
	A(2, 3) = Q(2, 3);
	A(3, 3) = 1.0;

	Q = A.transpose() * A;
	x = Q.inverse() * A.transpose() * b;
	x /= x[3];
	cout << x << endl;
}