根据4个三维空间点坐标求球心以及半径

根据4个三维空间点坐标求球心以及半径

  • 1、算法思路
  • 2、C++实现

1、算法思路

参考:https://wenku.baidu.com/view/82abe82ebd64783e09122b14.html

2、C++实现

/*
 * 功能:通过4个三维坐标点求过这四个点的球心以及半径
 * 参数:
 * - p1:其中一个三维坐标点
 * - p2:其中一个三维坐标点
 * - p3:其中一个三维坐标点
 * - p4:其中一个三维坐标点
 * - centre:球心对应的三维坐标点
 * - radius: 半径
 * 返回值:0表示正常,-1表示异常
 */
int calculateCentreAndRadius(const vector<double> &p1,const vector<double> &p2,
        const vector<double> &p3, const vector<double> &p4,
        vector<double> &centre, double &radius){
    double a = p1[0] - p2[0], b = p1[1] - p2[1], c = p1[2] - p2[2];
    double a1 = p3[0] - p4[0], b1 = p3[1] - p4[1], c1 = p3[2] - p3[2];
    double a2 = p2[0] - p3[0], b2 = p2[1] - p3[1], c2 = p2[2] - p3[2];
    double A = p1[0] * p1[0] - p2[0] * p2[0];
    double B = p1[1] * p1[1] - p2[1] * p2[1];
    double C = p1[2] * p1[2] - p2[2] * p2[2];
    double A1 = p3[0] * p3[0] - p4[0] * p4[0];
    double B1 = p3[1] * p3[1] - p4[1] * p4[1];
    double C1 = p3[2] * p3[2] - p4[2] * p4[2];
    double A2 = p2[0] * p2[0] - p3[0] * p3[0];
    double B2 = p2[1] * p2[1] - p3[1] * p3[1];
    double C2 = p2[2] * p2[2] - p3[2] * p3[2];
    double P = (A + B + C) /2;
    double Q = (A1 + B1 + C1) / 2;
    double R = (A2 + B2 + C2) / 2;

    // D是系数行列式,利用克拉默法则
    double D = a*b1*c2 + a2*b*c1 + c*a1*b2 - (a2*b1*c + a1*b*c2 + a*b2*c1);
    double Dx = P*b1*c2 + b*c1*R + c*Q*b2 - (c*b1*R + P*c1*b2 + Q*b*c2);
    double Dy = a*Q*c2 + P*c1*a2 + c*a1*R - (c*Q*a2 + a*c1*R + c2*P*a1);
    double Dz = a*b1*R + b*Q*a2 + P*a1*b2 - (a2*b1*P + a*Q*b2 + R*b*a1);

    if(D == 0){
        cerr << "四点共面" << endl;
        return -1;
    }else{
        centre.push_back(Dx/D);
        centre.push_back(Dy/D);
        centre.push_back(Dz/D);
        radius = sqrt((p1[0]-centre[0])*(p1[0]-centre[0]) +
                              (p1[1]-centre[1])*(p1[1]-centre[1]) +
                              (p1[2]-centre[2])*(p1[2]-centre[2]));
        return 0;
    }
}

你可能感兴趣的:(根据4个三维空间点坐标求球心以及半径)