【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;//平行
else
return 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 ¢er)
{
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 ¢er) {
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;//平行
else
return 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 ¢er)
{
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 ¢er) {
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;
}
免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！更多信息从访问主页：qidao123.com:ToB企服之家，中国第一个企服评测及商务社交产业平台。
页:
[1]