【3D】几何算法合集2（自用资源）方便cv

细分

1. #include<cmath>
2. #include<algorithm>
3. #include <opencv2/core/core.hpp>
4. #include <opencv2/imgproc/imgproc.hpp>
5. #include <opencv2/highgui/highgui.hpp>
7. #define sign(x) ((x)>eps?1:((x)<-eps?(-1):(0)))
8. #define PI 3.1415926

复制代码

1. #include "Algorithm.h"
3. const double eps = 1e-8, inf = 1e50;

复制代码

平面类

1. class plane3 {
2. public:
3.     cv::Point3d a, b, c;
4.     plane3() {}
5.     plane3(cv::Point3d _a, cv::Point3d _b, cv::Point3d _c) {
6.         a = _a;
7.         b = _b;
8.         c = _c;
9.     }
11. };

复制代码

三维直线类

1. class line3 {
2. public:
3.     cv::Point3d a, b;
4.     line3() {}
5.     line3(cv::Point3d _a, cv::Point3d _b) {
6.         a = _a;
7.         b = _b;
8.     }
9. };

复制代码

度数转弧度

1. //度数转弧度
2. double get_radian(double angel);

复制代码

1. //度数转弧度
2. double get_radian(double angel) {
3.     return PI * angel / 180.0;
4. }

复制代码

叉乘，返回向量

1. //叉乘，返回向量
2. cv::Point3d xmult(cv::Point3d a, cv::Point3d b);

复制代码

1. cv::Point3d xmult(cv::Point3d a, cv::Point3d b) {
2.     return cv::Point3d(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
3. }

复制代码

点乘

1. //点乘
2. double dmult(cv::Point3d a, cv::Point3d b);

复制代码

1. //点乘
2. double dmult(cv::Point3d a, cv::Point3d b) {
3.     return a.x * b.x + a.y * b.y + a.z * b.z;
4. }

复制代码

向量

1. //向量
2. cv::Point3d Vec(cv::Point3d a, cv::Point3d b);

复制代码

1. //向量
2. cv::Point3d Vec(cv::Point3d a, cv::Point3d b) {
3.     return cv::Point3d(a.x - b.x, a.y - b.y, a.z - b.z);
4. }

复制代码

取模

1. //取模
2. double length(cv::Point3d v);

复制代码

1. //取模
2. double length(cv::Point3d v) {
3.     return sqrt(v.x * v.x + v.y * v.y + v.z * v.z);
4. }

复制代码

判断直线平行

1. //判断直线平行
2. bool parallel(line3 u, line3 v);

复制代码

1. //判断直线平行
2. bool parallel(line3 u, line3 v) {
3.     return sign(length(xmult(u.b - u.a, v.b - v.a))) == 0;
4. }

复制代码

平面法向量

1. //平面法向量
2. cv::Point3d pvec(plane3 s);

复制代码

1. //平面法向量
2. cv::Point3d pvec(plane3 s) {
3.     return xmult(s.b - s.a, s.c - s.a);
4. }

复制代码

线线求交

1. //线线求交
2. cv::Point3d line_interseciton(line3 u, line3 v);

复制代码

1. //线线求交
2. cv::Point3d line_interseciton(line3 u, line3 v) {
3.     cv::Point3d ret = u.a, v1 = xmult(u.a - v.a, v.b - v.a), v2 = xmult(u.b - u.a, v.b - v.a);
4.     double t = length(v1) / length(v2) * (dmult(v1, v2) > 0 ? -1 : 1);
5.     return ret + ((u.b - u.a) * t);
6. }

复制代码

线面求交

1. //线面求交
2. cv::Point3d line_plane_intersection(line3 u, plane3 s);

复制代码

1. //线面求交
2. cv::Point3d line_plane_intersection(line3 u, plane3 s) {
3.     cv::Point3d ret = pvec(s), der = u.b - u.a;
4.     double t = dmult(ret, s.a - u.a) / dmult(ret, u.b - u.a);
5.     return u.a + der * t;
6. }

复制代码

判断两直线的位置关系

1. //判断两直线的位置关系
2. int line_to_line(line3 u, line3 v);

复制代码

1. //判断两直线的位置关系
2. int line_to_line(line3 u, line3 v) {
3.     plane3 s1(u.a, u.b, v.a), s2(u.a, u.b, v.b);
4.     if (sign(length(xmult(pvec(s1), pvec(s2)))))
5.         return -1;//异面
6.     else if (parallel(u, v))
7.         return 0;//平行
8.     else
9.         return 1;//相交
10. }

复制代码

直线到直线的距离 w是垂直向量，根据w算出标量

1. //直线到直线的距离 w是垂直向量，根据w算出标量
2. double Common_Vertical_Line(line3 u, line3 v, cv::Point3d& w);

复制代码

1. //直线到直线的距离 w是垂直向量，根据w算出标量
2. double Common_Vertical_Line(line3 u, line3 v, cv::Point3d& w) {
3.     w = xmult(u.a - u.b, v.a - v.b);
4.     return fabs(dmult(u.a - v.a, w) / length(w));
5. }

复制代码

求三维下的两线交点

1. //求三维下的两线交点
2. bool Intersection_lines(line3 L1, line3 L2, cv::Point3d& insc);

复制代码

1. //求三维下的两线交点
2. bool Intersection_lines(line3 L1, line3 L2, cv::Point3d& insc) {
3.     int relationship = line_to_line(L1, L2);
5.     switch (relationship) {
6.     case 1:                     //相交
7.         insc = line_interseciton(L1, L2);
8.         return true;
9.     case 0:                     //平行
10.         return false;
11.     case -1: {        //异面
12.         cv::Point3d v;
13.         double closestDist = Common_Vertical_Line(L1, L2, v); //两直线的最近距离
14.         plane3 p1(L1.a, L1.b, L1.a + v), p2(L2.a, L2.b, L2.a + v);
15.         cv::Point3d line1_ans = line_plane_intersection(L1, p2); //交点
16.         cv::Point3d line2_ans = line_plane_intersection(L2, p1); //交点
17.         insc.x = (line1_ans.x + line2_ans.x) / 2;
18.         insc.y = (line1_ans.y + line2_ans.y) / 2;
19.         insc.z = (line1_ans.z + line2_ans.z) / 2;
21.         return true;
22.     }
23.     default:
24.         return false;
25.     }
26. }

复制代码

输入四个点，求交点法线的起点

1. //输入四个点，求交点法线的起点
2. bool get_joint(line3 L1, line3 L2, cv::Point3d& beg);

复制代码

1. //输入四个点，求交点法线的起点
2. bool get_joint(line3 L1, line3 L2,  cv::Point3d& beg) {
4.     if (Intersection_lines(L1, L2, beg) == false) {
5.         return false;
6.     }
7.     return true;
8. }

复制代码

点线垂足

1. //点线垂足
2. cv::Point3d vertical_foot(cv::Point3d p, line3 u);

复制代码

1. //点线垂足
2. cv::Point3d vertical_foot(cv::Point3d p, line3 u) {
3.     double t = dmult(p - u.a, u.b - u.a) / dist2(u.a, u.b);
4.     cv::Point3d ret = u.a;
5.     return ret + ((u.b - u.a) * t);
6. }

复制代码

点线距离

1. //点线距离
2. double dist_point_to_line(cv::Point3d p, line3 u);

复制代码

1. //点线距离
2. double dist_point_to_line(cv::Point3d p, line3 u) {
3.     return length(xmult(p - u.a, u.b - u.a)) / dist(u.a, u.b);
4. }

复制代码

查抄三点共线

1. //检查三点共线
2. bool check_collinear(cv::Point3d pt1, cv::Point3d pt2, cv::Point3d pt3);

复制代码

1. //检查三点共线 pt1 pt2是线
2. bool check_collinear(cv::Point3d pt1, cv::Point3d pt2, cv::Point3d pt3) {
3.     line3 line(pt1, pt2);
4.     double d = dist_point_to_line(pt3, line);
6.     if (sign(d) == 0) {
7.         return true;//共线
8.     }
9.     return false;
10. }

复制代码

三点求圆

1. //三点求圆
2. void centerCircle3d(cv::Point3d pt1, cv::Point3d pt2, cv::Point3d pt3, double& radius, cv::Point3d& center);

复制代码

1. //三点求圆
2. void centerCircle3d(cv::Point3d pt1, cv::Point3d pt2, cv::Point3d pt3, double& radius,cv::Point3d &center)
3. {
4.     double a1 = (pt1.y * pt2.z - pt2.y * pt1.z - pt1.y * pt3.z + pt3.y * pt1.z + pt2.y * pt3.z - pt3.y * pt2.z),
5.         b1 = -(pt1.x * pt2.z - pt2.x * pt1.z - pt1.x * pt3.z + pt3.x * pt1.z + pt2.x * pt3.z - pt3.x * pt2.z),
6.         c1 = (pt1.x * pt2.y - pt2.x * pt1.y - pt1.x * pt3.y + pt3.x * pt1.y + pt2.x * pt3.y - pt3.x * pt2.y),
7.         d1 = -(pt1.x * pt2.y * pt3.z - pt1.x * pt3.y * pt2.z - pt2.x * pt1.y * pt3.z + pt2.x * pt3.y * pt1.z + pt3.x * pt1.y * pt2.z - pt3.x * pt2.y * pt1.z);
9.     double a2 = 2 * (pt2.x - pt1.x),
10.         b2 = 2 * (pt2.y - pt1.y),
11.         c2 = 2 * (pt2.z - pt1.z),
12.         d2 = pt1.x * pt1.x + pt1.y * pt1.y + pt1.z * pt1.z - pt2.x * pt2.x - pt2.y * pt2.y - pt2.z * pt2.z;
14.     double a3 = 2 * (pt3.x - pt1.x),
15.         b3 = 2 * (pt3.y - pt1.y),
16.         c3 = 2 * (pt3.z - pt1.z),
17.         d3 = pt1.x * pt1.x + pt1.y * pt1.y + pt1.z * pt1.z - pt3.x * pt3.x - pt3.y * pt3.y - pt3.z * pt3.z;
19.     double x = -(b1 * c2 * d3 - b1 * c3 * d2 - b2 * c1 * d3 + b2 * c3 * d1 + b3 * c1 * d2 - b3 * c2 * d1)
20.         / (a1 * b2 * c3 - a1 * b3 * c2 - a2 * b1 * c3 + a2 * b3 * c1 + a3 * b1 * c2 - a3 * b2 * c1);
21.     double y = (a1 * c2 * d3 - a1 * c3 * d2 - a2 * c1 * d3 + a2 * c3 * d1 + a3 * c1 * d2 - a3 * c2 * d1)
22.         / (a1 * b2 * c3 - a1 * b3 * c2 - a2 * b1 * c3 + a2 * b3 * c1 + a3 * b1 * c2 - a3 * b2 * c1);
23.     double z = -(a1 * b2 * d3 - a1 * b3 * d2 - a2 * b1 * d3 + a2 * b3 * d1 + a3 * b1 * d2 - a3 * b2 * d1)
24.         / (a1 * b2 * c3 - a1 * b3 * c2 - a2 * b1 * c3 + a2 * b3 * c1 + a3 * b1 * c2 - a3 * b2 * c1);
25.     radius = sqrt((pt1.x - x) * (pt1.x - x) + (pt1.y - y) * (pt1.y - y) + (pt1.z - z) * (pt1.z - z));
26.     center = { x,y,z };
27. }

复制代码

拟合圆

1. //拟合圆
2. bool Fitting_radius(cv::Point3d pt1, cv::Point3d pt2, cv::Point3d pt3, double& r, cv::Point3d& center);

复制代码

1. bool Fitting_radius(cv::Point3d pt1, cv::Point3d pt2, cv::Point3d pt3,double &r,cv::Point3d &center) {
2.    
3.     if (check_collinear(pt1, pt2, pt3)) {
4.         return false;
5.     }
6.     centerCircle3d(pt1, pt2, pt3, r,center);
7.     return true;
8. }

复制代码

点线求垂足，pt1和pt2是线，会查共线

1. //点线求垂足，pt1和pt2是线，会查共线
2. bool get_intersection_foot(cv::Point3d pt1, cv::Point3d pt2, cv::Point3d pt3, cv::Point3d& intersection);

复制代码

1. bool get_intersection_foot(cv::Point3d pt1, cv::Point3d pt2, cv::Point3d pt3, cv::Point3d& intersection)
2. {
3.     if (check_collinear(pt1, pt2, pt3)) {
4.         return false;
5.     }
6.     line3 line(pt1, pt2);
7.     intersection = vertical_foot(pt3, line);
8.     return true;
9. }

复制代码

三点求内切圆，pt2是交点,算夹角<180侧的内切圆

1. //三点求内切圆，pt2是交点,算夹角<180侧的内切圆
2. double get_inc_circle_R(const cv::Point3d& p1, const cv::Point3d& p2, const cv::Point3d& p3);

复制代码

1. double get_inc_circle_R(const cv::Point3d& p1, const cv::Point3d& p2, const cv::Point3d& p3)
2. {
3.     //三角形三边边长
4.     double p2p1, p3p2, p1p3;
5.     p2p1 = norm(p2 - p1);
6.     p3p2 = norm(p3 - p2);
7.     p1p3 = norm(p1 - p3);
9.     //三角形半周长
10.     double semiperimeter;
11.     semiperimeter = (p2p1 + p3p2 + p1p3) / 2.0;
13.     //三角形面积
14.     double area;
15.     area = sqrt(semiperimeter * (semiperimeter - p2p1) * (semiperimeter - p3p2) * (semiperimeter - p1p3));
17.     //计算三角形内接圆半径
18.     double radius;
19.     radius = 2 * area / (p2p1 + p3p2 + p1p3);
21.     return radius;
22. }

复制代码

未注释

1

1. double dist(cv::Point3d a, cv::Point3d b);
2. double dist2(cv::Point3d a, cv::Point3d b);

复制代码

1. double dist(cv::Point3d a, cv::Point3d b) {
2.     return length(a - b);
3. }
4. double dist2(cv::Point3d a, cv::Point3d b) {
5.     return dmult(a - b, a - b);
6. }

复制代码

2

1. cv::Point3d get_midPoint(const cv::Point3d pt1, const cv::Point3d pt2);

复制代码

1. cv::Point3d get_midPoint(const cv::Point3d pt1, const cv::Point3d pt2)
2. {
3.     cv::Point3d mid;
4.     cv::Point3d vec = pt2 - pt1;
5.     mid = pt1 + 0.5 * (vec);
6.     return mid;
7. }

复制代码

 3

1. double convert_angle_360(double src);

复制代码

1. double convert_angle_360(double src)
2. {
3.     double dst = src;
4.     while(dst >= 360) {
5.         dst -= 360;
6.     }
7.     while (dst < 0) {
8.         dst += 360;
9.     }
10.     return dst;
11. }

复制代码

汇总

头文件

1. #pragma once#include <iostream>#include<cstdlib>#include<cmath>
2. #include<algorithm>
3. #include <opencv2/core/core.hpp>
4. #include <opencv2/imgproc/imgproc.hpp>
5. #include <opencv2/highgui/highgui.hpp>
7. #define sign(x) ((x)>eps?1:((x)<-eps?(-1):(0)))
8. #define PI 3.1415926class plane3 {
9. public:
10.     cv::Point3d a, b, c;
11.     plane3() {}
12.     plane3(cv::Point3d _a, cv::Point3d _b, cv::Point3d _c) {
13.         a = _a;
14.         b = _b;
15.         c = _c;
16.     }
18. };class line3 {
19. public:
20.     cv::Point3d a, b;
21.     line3() {}
22.     line3(cv::Point3d _a, cv::Point3d _b) {
23.         a = _a;
24.         b = _b;
25.     }
26. };//度数转弧度
27. double get_radian(double angel);//叉乘，返回向量
28. cv::Point3d xmult(cv::Point3d a, cv::Point3d b);//点乘
29. double dmult(cv::Point3d a, cv::Point3d b);//向量
30. cv::Point3d Vec(cv::Point3d a, cv::Point3d b);//取模
31. double length(cv::Point3d v);//判断直线平行
32. bool parallel(line3 u, line3 v);//平面法向量
33. cv::Point3d pvec(plane3 s);//线线求交
34. cv::Point3d line_interseciton(line3 u, line3 v);//线面求交
35. cv::Point3d line_plane_intersection(line3 u, plane3 s);//判断两直线的位置关系
36. int line_to_line(line3 u, line3 v);//直线到直线的距离 w是垂直向量，根据w算出标量
37. double Common_Vertical_Line(line3 u, line3 v, cv::Point3d& w);//求三维下的两线交点
38. bool Intersection_lines(line3 L1, line3 L2, cv::Point3d& insc);//输入四个点，求交点法线的起点
39. bool get_joint(line3 L1, line3 L2, cv::Point3d& beg);double dist2(cv::Point3d a, cv::Point3d b);//点线垂足
40. cv::Point3d vertical_foot(cv::Point3d p, line3 u);double dist(cv::Point3d a, cv::Point3d b);//点线距离
41. double dist_point_to_line(cv::Point3d p, line3 u);//检查三点共线
42. bool check_collinear(cv::Point3d pt1, cv::Point3d pt2, cv::Point3d pt3);//三点求圆
43. void centerCircle3d(cv::Point3d pt1, cv::Point3d pt2, cv::Point3d pt3, double& radius, cv::Point3d& center);//拟合圆
44. bool Fitting_radius(cv::Point3d pt1, cv::Point3d pt2, cv::Point3d pt3, double& r, cv::Point3d& center);//点线求垂足，pt1和pt2是线，会查共线
45. bool get_intersection_foot(cv::Point3d pt1, cv::Point3d pt2, cv::Point3d pt3, cv::Point3d& intersection);//三点求内切圆，pt2是交点,算夹角<180侧的内切圆
46. double get_inc_circle_R(const cv::Point3d& p1, const cv::Point3d& p2, const cv::Point3d& p3);cv::Point3d get_midPoint(const cv::Point3d pt1, const cv::Point3d pt2);double convert_angle_360(double src);

复制代码

cpp文件

1. #include "Algorithm.h"
3. const double eps = 1e-8, inf = 1e50;//度数转弧度
4. double get_radian(double angel) {
5.     return PI * angel / 180.0;
6. }//叉乘，返回向量cv::Point3d xmult(cv::Point3d a, cv::Point3d b) {
7.     return cv::Point3d(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
8. }//点乘
9. double dmult(cv::Point3d a, cv::Point3d b) {
10.     return a.x * b.x + a.y * b.y + a.z * b.z;
11. }//向量
12. cv::Point3d Vec(cv::Point3d a, cv::Point3d b) {
13.     return cv::Point3d(a.x - b.x, a.y - b.y, a.z - b.z);
14. }//取模
15. double length(cv::Point3d v) {
16.     return sqrt(v.x * v.x + v.y * v.y + v.z * v.z);
17. }//判断直线平行
18. bool parallel(line3 u, line3 v) {
19.     return sign(length(xmult(u.b - u.a, v.b - v.a))) == 0;
20. }//平面法向量
21. cv::Point3d pvec(plane3 s) {
22.     return xmult(s.b - s.a, s.c - s.a);
23. }//线线求交
24. cv::Point3d line_interseciton(line3 u, line3 v) {
25.     cv::Point3d ret = u.a, v1 = xmult(u.a - v.a, v.b - v.a), v2 = xmult(u.b - u.a, v.b - v.a);
26.     double t = length(v1) / length(v2) * (dmult(v1, v2) > 0 ? -1 : 1);
27.     return ret + ((u.b - u.a) * t);
28. }//线面求交
29. cv::Point3d line_plane_intersection(line3 u, plane3 s) {
30.     cv::Point3d ret = pvec(s), der = u.b - u.a;
31.     double t = dmult(ret, s.a - u.a) / dmult(ret, u.b - u.a);
32.     return u.a + der * t;
33. }//判断两直线的位置关系
34. int line_to_line(line3 u, line3 v) {
35.     plane3 s1(u.a, u.b, v.a), s2(u.a, u.b, v.b);
36.     if (sign(length(xmult(pvec(s1), pvec(s2)))))
37.         return -1;//异面
38.     else if (parallel(u, v))
39.         return 0;//平行
40.     else
41.         return 1;//相交
42. }//直线到直线的距离 w是垂直向量，根据w算出标量
43. double Common_Vertical_Line(line3 u, line3 v, cv::Point3d& w) {
44.     w = xmult(u.a - u.b, v.a - v.b);
45.     return fabs(dmult(u.a - v.a, w) / length(w));
46. }//求三维下的两线交点
47. bool Intersection_lines(line3 L1, line3 L2, cv::Point3d& insc) {
48.     int relationship = line_to_line(L1, L2);
50.     switch (relationship) {
51.     case 1:                     //相交
52.         insc = line_interseciton(L1, L2);
53.         return true;
54.     case 0:                     //平行
55.         return false;
56.     case -1: {        //异面
57.         cv::Point3d v;
58.         double closestDist = Common_Vertical_Line(L1, L2, v); //两直线的最近距离
59.         plane3 p1(L1.a, L1.b, L1.a + v), p2(L2.a, L2.b, L2.a + v);
60.         cv::Point3d line1_ans = line_plane_intersection(L1, p2); //交点
61.         cv::Point3d line2_ans = line_plane_intersection(L2, p1); //交点
62.         insc.x = (line1_ans.x + line2_ans.x) / 2;
63.         insc.y = (line1_ans.y + line2_ans.y) / 2;
64.         insc.z = (line1_ans.z + line2_ans.z) / 2;
66.         return true;
67.     }
68.     default:
69.         return false;
70.     }
71. }//输入四个点，求交点法线的起点
72. bool get_joint(line3 L1, line3 L2,  cv::Point3d& beg) {
74.     if (Intersection_lines(L1, L2, beg) == false) {
75.         return false;
76.     }
77.     return true;
78. }double dist2(cv::Point3d a, cv::Point3d b) {    return dmult(a - b, a - b);}//点线垂足
79. cv::Point3d vertical_foot(cv::Point3d p, line3 u) {
80.     double t = dmult(p - u.a, u.b - u.a) / dist2(u.a, u.b);
81.     cv::Point3d ret = u.a;
82.     return ret + ((u.b - u.a) * t);
83. }double dist(cv::Point3d a, cv::Point3d b) {    return length(a - b);}//点线距离
84. double dist_point_to_line(cv::Point3d p, line3 u) {
85.     return length(xmult(p - u.a, u.b - u.a)) / dist(u.a, u.b);
86. }//检查三点共线 pt1 pt2是线
87. bool check_collinear(cv::Point3d pt1, cv::Point3d pt2, cv::Point3d pt3) {
88.     line3 line(pt1, pt2);
89.     double d = dist_point_to_line(pt3, line);
91.     if (sign(d) == 0) {
92.         return true;//共线
93.     }
94.     return false;
95. }//三点求圆
96. void centerCircle3d(cv::Point3d pt1, cv::Point3d pt2, cv::Point3d pt3, double& radius,cv::Point3d &center)
97. {
98.     double a1 = (pt1.y * pt2.z - pt2.y * pt1.z - pt1.y * pt3.z + pt3.y * pt1.z + pt2.y * pt3.z - pt3.y * pt2.z),
99.         b1 = -(pt1.x * pt2.z - pt2.x * pt1.z - pt1.x * pt3.z + pt3.x * pt1.z + pt2.x * pt3.z - pt3.x * pt2.z),
100.         c1 = (pt1.x * pt2.y - pt2.x * pt1.y - pt1.x * pt3.y + pt3.x * pt1.y + pt2.x * pt3.y - pt3.x * pt2.y),
101.         d1 = -(pt1.x * pt2.y * pt3.z - pt1.x * pt3.y * pt2.z - pt2.x * pt1.y * pt3.z + pt2.x * pt3.y * pt1.z + pt3.x * pt1.y * pt2.z - pt3.x * pt2.y * pt1.z);
103.     double a2 = 2 * (pt2.x - pt1.x),
104.         b2 = 2 * (pt2.y - pt1.y),
105.         c2 = 2 * (pt2.z - pt1.z),
106.         d2 = pt1.x * pt1.x + pt1.y * pt1.y + pt1.z * pt1.z - pt2.x * pt2.x - pt2.y * pt2.y - pt2.z * pt2.z;
108.     double a3 = 2 * (pt3.x - pt1.x),
109.         b3 = 2 * (pt3.y - pt1.y),
110.         c3 = 2 * (pt3.z - pt1.z),
111.         d3 = pt1.x * pt1.x + pt1.y * pt1.y + pt1.z * pt1.z - pt3.x * pt3.x - pt3.y * pt3.y - pt3.z * pt3.z;
113.     double x = -(b1 * c2 * d3 - b1 * c3 * d2 - b2 * c1 * d3 + b2 * c3 * d1 + b3 * c1 * d2 - b3 * c2 * d1)
114.         / (a1 * b2 * c3 - a1 * b3 * c2 - a2 * b1 * c3 + a2 * b3 * c1 + a3 * b1 * c2 - a3 * b2 * c1);
115.     double y = (a1 * c2 * d3 - a1 * c3 * d2 - a2 * c1 * d3 + a2 * c3 * d1 + a3 * c1 * d2 - a3 * c2 * d1)
116.         / (a1 * b2 * c3 - a1 * b3 * c2 - a2 * b1 * c3 + a2 * b3 * c1 + a3 * b1 * c2 - a3 * b2 * c1);
117.     double z = -(a1 * b2 * d3 - a1 * b3 * d2 - a2 * b1 * d3 + a2 * b3 * d1 + a3 * b1 * d2 - a3 * b2 * d1)
118.         / (a1 * b2 * c3 - a1 * b3 * c2 - a2 * b1 * c3 + a2 * b3 * c1 + a3 * b1 * c2 - a3 * b2 * c1);
119.     radius = sqrt((pt1.x - x) * (pt1.x - x) + (pt1.y - y) * (pt1.y - y) + (pt1.z - z) * (pt1.z - z));
120.     center = { x,y,z };
121. }bool Fitting_radius(cv::Point3d pt1, cv::Point3d pt2, cv::Point3d pt3,double &r,cv::Point3d &center) {
122.    
123.     if (check_collinear(pt1, pt2, pt3)) {
124.         return false;
125.     }
126.     centerCircle3d(pt1, pt2, pt3, r,center);
127.     return true;
128. }bool get_intersection_foot(cv::Point3d pt1, cv::Point3d pt2, cv::Point3d pt3, cv::Point3d& intersection)
129. {
130.     if (check_collinear(pt1, pt2, pt3)) {
131.         return false;
132.     }
133.     line3 line(pt1, pt2);
134.     intersection = vertical_foot(pt3, line);
135.     return true;
136. }double get_inc_circle_R(const cv::Point3d& p1, const cv::Point3d& p2, const cv::Point3d& p3)
137. {
138.     //三角形三边边长
139.     double p2p1, p3p2, p1p3;
140.     p2p1 = norm(p2 - p1);
141.     p3p2 = norm(p3 - p2);
142.     p1p3 = norm(p1 - p3);
144.     //三角形半周长
145.     double semiperimeter;
146.     semiperimeter = (p2p1 + p3p2 + p1p3) / 2.0;
148.     //三角形面积
149.     double area;
150.     area = sqrt(semiperimeter * (semiperimeter - p2p1) * (semiperimeter - p3p2) * (semiperimeter - p1p3));
152.     //计算三角形内接圆半径
153.     double radius;
154.     radius = 2 * area / (p2p1 + p3p2 + p1p3);
156.     return radius;
157. }cv::Point3d get_midPoint(const cv::Point3d pt1, const cv::Point3d pt2)
158. {
159.     cv::Point3d mid;
160.     cv::Point3d vec = pt2 - pt1;
161.     mid = pt1 + 0.5 * (vec);
162.     return mid;
163. }double convert_angle_360(double src)
164. {
165.     double dst = src;
166.     while(dst >= 360) {
167.         dst -= 360;
168.     }
169.     while (dst < 0) {
170.         dst += 360;
171.     }
172.     return dst;
173. }

复制代码

免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！更多信息从访问主页：qidao123.com:ToB企服之家，中国第一个企服评测及商务社交产业平台。