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

细分

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

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

平面类

class plane3 {
public:cv::Point3d a, b, c;plane3() {}plane3(cv::Point3d _a, cv::Point3d _b, cv::Point3d _c) {a = _a;b = _b;c = _c;}};

三维直线类

class line3 {
public:cv::Point3d a, b;line3() {}line3(cv::Point3d _a, cv::Point3d _b) {a = _a;b = _b;}
};

度数转弧度

//度数转弧度
double get_radian(double angel);

//度数转弧度
double get_radian(double angel) {return PI * angel / 180.0;
}

叉乘，返回向量

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

cv::Point3d xmult(cv::Point3d a, cv::Point3d b) {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);
}

点乘

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

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

向量

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

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

取模

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

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

判断直线平行

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

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

平面法向量

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

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

线线求交

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

//线线求交
cv::Point3d line_interseciton(line3 u, line3 v) {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);double t = length(v1) / length(v2) * (dmult(v1, v2) > 0 ? -1 : 1);return ret + ((u.b - u.a) * t);
}

线面求交

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

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

判断两直线的位置关系

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

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

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

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

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

求三维下的两线交点

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

//求三维下的两线交点
bool Intersection_lines(line3 L1, line3 L2, cv::Point3d& insc) {int relationship = line_to_line(L1, L2);switch (relationship) {case 1:                     //相交insc = line_interseciton(L1, L2);return true;case 0:                     //平行return false;case -1: {        //异面cv::Point3d v;double closestDist = Common_Vertical_Line(L1, L2, v); //两直线的最近距离plane3 p1(L1.a, L1.b, L1.a + v), p2(L2.a, L2.b, L2.a + v);cv::Point3d line1_ans = line_plane_intersection(L1, p2); //交点cv::Point3d line2_ans = line_plane_intersection(L2, p1); //交点insc.x = (line1_ans.x + line2_ans.x) / 2;insc.y = (line1_ans.y + line2_ans.y) / 2;insc.z = (line1_ans.z + line2_ans.z) / 2;return true;}default:return false;}
}

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

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

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

点线垂足

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

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

点线距离

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

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

检查三点共线

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

//检查三点共线 pt1 pt2是线
bool check_collinear(cv::Point3d pt1, cv::Point3d pt2, cv::Point3d pt3) {line3 line(pt1, pt2);double d = dist_point_to_line(pt3, line);if (sign(d) == 0) {return true;//共线}return false;
}

三点求圆

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

//三点求圆
void centerCircle3d(cv::Point3d pt1, cv::Point3d pt2, cv::Point3d pt3, double& radius,cv::Point3d &center)
{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),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),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),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);double a2 = 2 * (pt2.x - pt1.x),b2 = 2 * (pt2.y - pt1.y),c2 = 2 * (pt2.z - pt1.z),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;double a3 = 2 * (pt3.x - pt1.x),b3 = 2 * (pt3.y - pt1.y),c3 = 2 * (pt3.z - pt1.z),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;double x = -(b1 * c2 * d3 - b1 * c3 * d2 - b2 * c1 * d3 + b2 * c3 * d1 + b3 * c1 * d2 - b3 * c2 * d1)/ (a1 * b2 * c3 - a1 * b3 * c2 - a2 * b1 * c3 + a2 * b3 * c1 + a3 * b1 * c2 - a3 * b2 * c1);double y = (a1 * c2 * d3 - a1 * c3 * d2 - a2 * c1 * d3 + a2 * c3 * d1 + a3 * c1 * d2 - a3 * c2 * d1)/ (a1 * b2 * c3 - a1 * b3 * c2 - a2 * b1 * c3 + a2 * b3 * c1 + a3 * b1 * c2 - a3 * b2 * c1);double z = -(a1 * b2 * d3 - a1 * b3 * d2 - a2 * b1 * d3 + a2 * b3 * d1 + a3 * b1 * d2 - a3 * b2 * d1)/ (a1 * b2 * c3 - a1 * b3 * c2 - a2 * b1 * c3 + a2 * b3 * c1 + a3 * b1 * c2 - a3 * b2 * c1);radius = sqrt((pt1.x - x) * (pt1.x - x) + (pt1.y - y) * (pt1.y - y) + (pt1.z - z) * (pt1.z - z));center = { x,y,z };
}

拟合圆

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

bool Fitting_radius(cv::Point3d pt1, cv::Point3d pt2, cv::Point3d pt3,double &r,cv::Point3d &center) {if (check_collinear(pt1, pt2, pt3)) {return false;}centerCircle3d(pt1, pt2, pt3, r,center);return true;
}

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

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

bool get_intersection_foot(cv::Point3d pt1, cv::Point3d pt2, cv::Point3d pt3, cv::Point3d& intersection)
{if (check_collinear(pt1, pt2, pt3)) {return false;}line3 line(pt1, pt2);intersection = vertical_foot(pt3, line);return true;
}

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

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

double get_inc_circle_R(const cv::Point3d& p1, const cv::Point3d& p2, const cv::Point3d& p3)
{//三角形三边边长double p2p1, p3p2, p1p3;p2p1 = norm(p2 - p1);p3p2 = norm(p3 - p2);p1p3 = norm(p1 - p3);//三角形半周长double semiperimeter;semiperimeter = (p2p1 + p3p2 + p1p3) / 2.0;//三角形面积double area;area = sqrt(semiperimeter * (semiperimeter - p2p1) * (semiperimeter - p3p2) * (semiperimeter - p1p3));//计算三角形内接圆半径double radius;radius = 2 * area / (p2p1 + p3p2 + p1p3);return radius;
}

未注释

1

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

double dist(cv::Point3d a, cv::Point3d b) {return length(a - b);
}
double dist2(cv::Point3d a, cv::Point3d b) {return dmult(a - b, a - b);
}

2

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

cv::Point3d get_midPoint(const cv::Point3d pt1, const cv::Point3d pt2)
{cv::Point3d mid;cv::Point3d vec = pt2 - pt1;mid = pt1 + 0.5 * (vec);return mid;
}

 3

double convert_angle_360(double src);

double convert_angle_360(double src)
{double dst = src;while(dst >= 360) {dst -= 360;}while (dst < 0) {dst += 360;}return dst;
}

汇总

头文件

#pragma once
#include <iostream>
#include<cstdlib>
#include<cmath>
#include<algorithm>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>#define sign(x) ((x)>eps?1:((x)<-eps?(-1):(0)))
#define PI 3.1415926class plane3 {
public:cv::Point3d a, b, c;plane3() {}plane3(cv::Point3d _a, cv::Point3d _b, cv::Point3d _c) {a = _a;b = _b;c = _c;}};class line3 {
public:cv::Point3d a, b;line3() {}line3(cv::Point3d _a, cv::Point3d _b) {a = _a;b = _b;}
};
//度数转弧度
double get_radian(double angel);
//叉乘，返回向量
cv::Point3d xmult(cv::Point3d a, cv::Point3d b);//点乘
double dmult(cv::Point3d a, cv::Point3d b);//向量
cv::Point3d Vec(cv::Point3d a, cv::Point3d b);//取模
double length(cv::Point3d v);//判断直线平行
bool parallel(line3 u, line3 v);//平面法向量
cv::Point3d pvec(plane3 s);//线线求交
cv::Point3d line_interseciton(line3 u, line3 v);//线面求交
cv::Point3d line_plane_intersection(line3 u, plane3 s);//判断两直线的位置关系
int line_to_line(line3 u, line3 v);//直线到直线的距离 w是垂直向量，根据w算出标量
double Common_Vertical_Line(line3 u, line3 v, cv::Point3d& w);//求三维下的两线交点
bool Intersection_lines(line3 L1, line3 L2, cv::Point3d& insc);//输入四个点，求交点法线的起点
bool get_joint(line3 L1, line3 L2, cv::Point3d& beg);double dist2(cv::Point3d a, cv::Point3d b);//点线垂足
cv::Point3d vertical_foot(cv::Point3d p, line3 u);double dist(cv::Point3d a, cv::Point3d b);//点线距离
double dist_point_to_line(cv::Point3d p, line3 u);//检查三点共线
bool check_collinear(cv::Point3d pt1, cv::Point3d pt2, cv::Point3d pt3);//三点求圆
void centerCircle3d(cv::Point3d pt1, cv::Point3d pt2, cv::Point3d pt3, double& radius, cv::Point3d& center);//拟合圆
bool Fitting_radius(cv::Point3d pt1, cv::Point3d pt2, cv::Point3d pt3, double& r, cv::Point3d& center);//点线求垂足，pt1和pt2是线，会查共线
bool get_intersection_foot(cv::Point3d pt1, cv::Point3d pt2, cv::Point3d pt3, cv::Point3d& intersection);//三点求内切圆，pt2是交点,算夹角<180侧的内切圆
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文件

#include "Algorithm.h"const double eps = 1e-8, inf = 1e50;//度数转弧度
double get_radian(double angel) {return PI * angel / 180.0;
}
//叉乘，返回向量
cv::Point3d xmult(cv::Point3d a, cv::Point3d b) {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);
}
//点乘
double dmult(cv::Point3d a, cv::Point3d b) {return a.x * b.x + a.y * b.y + a.z * b.z;
}
//向量
cv::Point3d Vec(cv::Point3d a, cv::Point3d b) {return cv::Point3d(a.x - b.x, a.y - b.y, a.z - b.z);
}
//取模
double length(cv::Point3d v) {return sqrt(v.x * v.x + v.y * v.y + v.z * v.z);
}//判断直线平行
bool parallel(line3 u, line3 v) {return sign(length(xmult(u.b - u.a, v.b - v.a))) == 0;
}//平面法向量
cv::Point3d pvec(plane3 s) {return xmult(s.b - s.a, s.c - s.a);
}//线线求交
cv::Point3d line_interseciton(line3 u, line3 v) {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);double t = length(v1) / length(v2) * (dmult(v1, v2) > 0 ? -1 : 1);return ret + ((u.b - u.a) * t);
}//线面求交
cv::Point3d line_plane_intersection(line3 u, plane3 s) {cv::Point3d ret = pvec(s), der = u.b - u.a;double t = dmult(ret, s.a - u.a) / dmult(ret, u.b - u.a);return u.a + der * t;
}//判断两直线的位置关系
int line_to_line(line3 u, line3 v) {plane3 s1(u.a, u.b, v.a), s2(u.a, u.b, v.b);if (sign(length(xmult(pvec(s1), pvec(s2)))))return -1;//异面else if (parallel(u, v))return 0;//平行elsereturn 1;//相交
}//直线到直线的距离 w是垂直向量，根据w算出标量
double Common_Vertical_Line(line3 u, line3 v, cv::Point3d& w) {w = xmult(u.a - u.b, v.a - v.b);return fabs(dmult(u.a - v.a, w) / length(w));
}//求三维下的两线交点
bool Intersection_lines(line3 L1, line3 L2, cv::Point3d& insc) {int relationship = line_to_line(L1, L2);switch (relationship) {case 1:                     //相交insc = line_interseciton(L1, L2);return true;case 0:                     //平行return false;case -1: {        //异面cv::Point3d v;double closestDist = Common_Vertical_Line(L1, L2, v); //两直线的最近距离plane3 p1(L1.a, L1.b, L1.a + v), p2(L2.a, L2.b, L2.a + v);cv::Point3d line1_ans = line_plane_intersection(L1, p2); //交点cv::Point3d line2_ans = line_plane_intersection(L2, p1); //交点insc.x = (line1_ans.x + line2_ans.x) / 2;insc.y = (line1_ans.y + line2_ans.y) / 2;insc.z = (line1_ans.z + line2_ans.z) / 2;return true;}default:return false;}
}//输入四个点，求交点法线的起点
bool get_joint(line3 L1, line3 L2,  cv::Point3d& beg) {if (Intersection_lines(L1, L2, beg) == false) {return false;}return true;
}double dist2(cv::Point3d a, cv::Point3d b) {return dmult(a - b, a - b);
}//点线垂足
cv::Point3d vertical_foot(cv::Point3d p, line3 u) {double t = dmult(p - u.a, u.b - u.a) / dist2(u.a, u.b);cv::Point3d ret = u.a;return ret + ((u.b - u.a) * t);
}double dist(cv::Point3d a, cv::Point3d b) {return length(a - b);
}//点线距离
double dist_point_to_line(cv::Point3d p, line3 u) {return length(xmult(p - u.a, u.b - u.a)) / dist(u.a, u.b);
}//检查三点共线 pt1 pt2是线
bool check_collinear(cv::Point3d pt1, cv::Point3d pt2, cv::Point3d pt3) {line3 line(pt1, pt2);double d = dist_point_to_line(pt3, line);if (sign(d) == 0) {return true;//共线}return false;
}//三点求圆
void centerCircle3d(cv::Point3d pt1, cv::Point3d pt2, cv::Point3d pt3, double& radius,cv::Point3d &center)
{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),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),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),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);double a2 = 2 * (pt2.x - pt1.x),b2 = 2 * (pt2.y - pt1.y),c2 = 2 * (pt2.z - pt1.z),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;double a3 = 2 * (pt3.x - pt1.x),b3 = 2 * (pt3.y - pt1.y),c3 = 2 * (pt3.z - pt1.z),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;double x = -(b1 * c2 * d3 - b1 * c3 * d2 - b2 * c1 * d3 + b2 * c3 * d1 + b3 * c1 * d2 - b3 * c2 * d1)/ (a1 * b2 * c3 - a1 * b3 * c2 - a2 * b1 * c3 + a2 * b3 * c1 + a3 * b1 * c2 - a3 * b2 * c1);double y = (a1 * c2 * d3 - a1 * c3 * d2 - a2 * c1 * d3 + a2 * c3 * d1 + a3 * c1 * d2 - a3 * c2 * d1)/ (a1 * b2 * c3 - a1 * b3 * c2 - a2 * b1 * c3 + a2 * b3 * c1 + a3 * b1 * c2 - a3 * b2 * c1);double z = -(a1 * b2 * d3 - a1 * b3 * d2 - a2 * b1 * d3 + a2 * b3 * d1 + a3 * b1 * d2 - a3 * b2 * d1)/ (a1 * b2 * c3 - a1 * b3 * c2 - a2 * b1 * c3 + a2 * b3 * c1 + a3 * b1 * c2 - a3 * b2 * c1);radius = sqrt((pt1.x - x) * (pt1.x - x) + (pt1.y - y) * (pt1.y - y) + (pt1.z - z) * (pt1.z - z));center = { x,y,z };
}bool Fitting_radius(cv::Point3d pt1, cv::Point3d pt2, cv::Point3d pt3,double &r,cv::Point3d &center) {if (check_collinear(pt1, pt2, pt3)) {return false;}centerCircle3d(pt1, pt2, pt3, r,center);return true;
}bool get_intersection_foot(cv::Point3d pt1, cv::Point3d pt2, cv::Point3d pt3, cv::Point3d& intersection)
{if (check_collinear(pt1, pt2, pt3)) {return false;}line3 line(pt1, pt2);intersection = vertical_foot(pt3, line);return true;
}double get_inc_circle_R(const cv::Point3d& p1, const cv::Point3d& p2, const cv::Point3d& p3)
{//三角形三边边长double p2p1, p3p2, p1p3;p2p1 = norm(p2 - p1);p3p2 = norm(p3 - p2);p1p3 = norm(p1 - p3);//三角形半周长double semiperimeter;semiperimeter = (p2p1 + p3p2 + p1p3) / 2.0;//三角形面积double area;area = sqrt(semiperimeter * (semiperimeter - p2p1) * (semiperimeter - p3p2) * (semiperimeter - p1p3));//计算三角形内接圆半径double radius;radius = 2 * area / (p2p1 + p3p2 + p1p3);return radius;
}cv::Point3d get_midPoint(const cv::Point3d pt1, const cv::Point3d pt2)
{cv::Point3d mid;cv::Point3d vec = pt2 - pt1;mid = pt1 + 0.5 * (vec);return mid;
}double convert_angle_360(double src)
{double dst = src;while(dst >= 360) {dst -= 360;}while (dst < 0) {dst += 360;}return dst;
}