// 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 &center)
{

    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;
}