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

/* * 功能：通过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; } }