// compute_sphere_center.cpp : 定义控制台应用程序的入口点。
//
#include<iostream>
#include<vector>
using namespace std;
struct point
{
double x = 0;
double y = 0;
double z = 0;
};
struct plane
{
double a = 0;
double b = 0;
double c = 0;
double d = 0;
};
//三个点计算平面参数
void get_panel(point p1, point p2, point p3, plane &plane_data)
{
plane_data.a = (p2.y - p1.y)*(p3.z - p1.z) - (p2.z - p1.z)*(p3.y - p1.y);
plane_data.b = (p2.z - p1.z)*(p3.x - p1.x) - (p2.x - p1.x)*(p3.z - p1.z);
plane_data.c = (p2.x - p1.x)*(p3.y - p1.y) - (p2.y - p1.y)*(p3.x - p1.x);
plane_data.d = 0 - (plane_data.a * p1.x + plane_data.b*p1.y + plane_data.c*p1.z);
}
//已知两点计算其中垂面的参数
void get_vertical_plane(point p1, point p2, plane &plane_data)
{
plane_data.a = p2.x - p1.x;
plane_data.b = p2.y - p1.y;
plane_data.c = p2.z - p1.z;
plane_data.d = 0 - 0.5*(p2.x*p2.x - p1.x*p1.x + p2.y*p2.y - p1.y*p1.y + p2.z*p2.z - p1.z*p1.z);
}
//计算三个平面的交点,即圆心
void compute_center(plane p1,plane p2,plane p3, point ¢er)
{
double a11, a12, a13, a21, a22, a23, a31, a32, a33, b1, b2, b3, delt, d1, d2, d3;
a11 = p1.a; a12 = p1.b; a13 = p1.c;
a21 = p2.a; a22 = p2.b; a23 = p2.c;
a31 = p3.a; a32 = p3.b; a33 = p3.c;
b1 = -p1.d; b2 = -p2.d; b3 = -p3.d;
delt = a11*a22*a33 + a12*a23*a31 + a13*a21*a32 - a11*a23*a32 - a12*a21*a33 - a13*a22*a31;
if (delt == 0)
{
cout << "delt == 0 此方程无解或者有多解" << endl;
}
d1 = b1*a22*a33 + a12*a23*b3 + a13*b2*a32 - b1*a23*a32 - a12*b2*a33 - a13*a22*b3;
d2 = a11*b2*a33 + b1*a23*a31 + a13*a21*b3 - a11*a23*b3 - b1*a21*a33 - a13*b2*a31;
d3 = a11*a22*b3 + a12*b2*a31 + b1*a21*a32 - a11*b2*a32 - a12*a21*b3 - b1*a22*a31;
center.x = d1 / delt;
center.y = d2 / delt;
center.z = d3 / delt;
}
//计算圆心到圆周上点的距离
void compute_dist(point p1, point p2)
{
double dist = (p1.x - p2.x)*(p1.x - p2.x) + (p1.y - p2.y)*(p1.y - p2.y) + (p1.z - p2.z)*(p1.z - p2.z);
cout << "the distance of two point is: " << dist << endl;
}
int solveCenterPointOfCircle(std::vector<point> pt)
{
double a1, b1, c1, d1;
double a2, b2, c2, d2;
double a3, b3, c3, d3;
double x1 = pt[0].x, y1 = pt[0].y, z1 = pt[0].z;
double x2 = pt[1].x, y2 = pt[1].y, z2 = pt[1].z;
double x3 = pt[2].x, y3 = pt[2].y, z3 = pt[2].z;
a1 = (y1*z2 - y2*z1 - y1*z3 + y3*z1 + y2*z3 - y3*z2);
b1 = -(x1*z2 - x2*z1 - x1*z3 + x3*z1 + x2*z3 - x3*z2);
c1 = (x1*y2 - x2*y1 - x1*y3 + x3*y1 + x2*y3 - x3*y2);
d1 = -(x1*y2*z3 - x1*y3*z2 - x2*y1*z3 + x2*y3*z1 + x3*y1*z2 - x3*y2*z1);
a2 = 2 * (x2 - x1);
b2 = 2 * (y2 - y1);
c2 = 2 * (z2 - z1);
d2 = x1 * x1 + y1 * y1 + z1 * z1 - x2 * x2 - y2 * y2 - z2 * z2;
a3 = 2 * (x3 - x1);
b3 = 2 * (y3 - y1);
c3 = 2 * (z3 - z1);
d3 = x1 * x1 + y1 * y1 + z1 * z1 - x3 * x3 - y3 * y3 - z3 * z3;
double x, y, z;
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);
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);
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);
double r1 = 0.0, r2 = 0.0,r3 = 0.0;
r1 = sqrt((x1 - x)*(x1 - x) + (y1 - y)*(y1 - y) + (z1 - z)*(z1 - z));
r2 = sqrt((x2 - x)*(x2 - x) + (y2 - y)*(y2 - y) + (z2 - z)*(z2 - z));
r3 = sqrt((x3 - x)*(x3 - x) + (y3 - y)*(y3 - y) + (z3 - z)*(z3 - z));
cout << "x= " << x << " " << "y= " << y << " " << "z= " << z << endl;
cout << "r1= " << r1 << " " << "r2= " << r2 << " " << "r3= " << r3 << endl;
return 0;
}
int main()
{
point p1, p2, p3;
p1.x = 41.264801;
p1.y = -5.300050;
p1.z =4.526780;
p2.x = 42.314800;
p2.y = -5.359960;
p2.z =2.104020;
p3.x = 38.758999;
p3.y = -5.132990;
p3.z =2.282340;
vector<point> data;
data.push_back(p1);
data.push_back(p2);
data.push_back(p3);
solveCenterPointOfCircle(data);
//测试用的数据
point p4;
p4.x = 39.378502;
p4.y = -5.169710;
p4.z =1.310680;
point p5;
p5.x = 38.704399;
p5.y = -5.131250;
p5.z = 2.866250;
plane plane1, plane2, plane3;
get_panel(p1, p2, p3, plane1);
get_vertical_plane(p1, p2, plane2);
get_vertical_plane(p1, p3, plane3);
point center;
compute_center(plane1, plane2, plane3, center);
cout <<"center.x: "<< center.x << " " << "center.y: "<<center.y << " " << "center.z: "<<center.z << endl;
compute_dist(p1, center);
compute_dist(p2, center);
compute_dist(p3, center);
//测试
compute_dist(p4, center);
compute_dist(p5, center);
system("pause");
return 0;
}
已知圆弧上任意三点,计算圆心与半径--C++代码实现
最新推荐文章于 2024-06-05 14:36:41 发布