文章目录
三维空间求两向量逆时针角度
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
×
P
1
Q
Z_1=P_1P_2 \times P_1Q
Z1=P1P2×P1Q
Z
2
=
P
2
P
3
×
P
2
Q
Z_2=P_2P_3 \times P_2Q
Z2=P2P3×P2Q
Z
3
=
P
3
P
1
×
P
2
Q
Z_3=P_3P_1 \times P_2Q
Z3=P3P1×P2Q
若在内部,则
Z
1
⋅
Z
2
⋅
Z
3
>
0
Z_1 \cdot Z_2 \cdot Z_3>0
Z1⋅Z2⋅Z3>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;
}