#include<iostream>
#include <vector>
using namespace std;

struct SPoint
{
    float x, y, z;
    SPoint(float u = 0.0f, float v = 0.0f, float w = 0.0f)
    {
        x = u, y = v, z = w;
    }
};

void getpoint(vector<SPoint>& point3d)
{
    float a = 1;
    float b = 1;
    float c = 1;
    float d = 1;
    float x;
    float y;
    float z;

    float random;
    for (int i = 0; i < 100; ++i)
        for (int j = 0; j < 10; ++j)
        {
            random = (float)(rand() % 10);
            random = random / 1000;// 随机产生噪声，加到z坐标上。
            x = (float)i;
            y = (float)j;
            z = (-d - a * x - b * y) / c + random;

            point3d.push_back(SPoint(x, y, z));
        }
}

void get_plane(vector<SPoint> point, double& A, double& B, double& C)
{
    double x = 0, y = 0, z = 0, xy = 0, xz = 0, yz = 0, x2 = 0, y2 = 0, z2 = 0;
    for (int i = 0; i < point.size(); ++i)
    {
        x += (double)point[i].x;
        y += (double)point[i].y;
        z += (double)point[i].z;
        xy += (double)point[i].x * (double)point[i].y;
        xz += (double)point[i].x * (double)point[i].z;
        yz += (double)point[i].y * (double)point[i].z;
        x2 += (double)point[i].x * (double)point[i].x;
        y2 += (double)point[i].y * (double)point[i].y;
        z2 += (double)point[i].z * (double)point[i].z;
    }
    double m1, m2, m3, m4, m5, m6, m7, m8, m9, m10, m11, m12;
    m1 = x2;
    m2 = xy;
    m3 = xz;
    m4 = x;
    m5 = xy;
    m6 = y2;
    m7 = yz;
    m8 = y;
    m9 = xz;
    m10 = yz;
    m11 = z2;
    m12 = z;
    double n1, n2, n3, n4, n5, n6;
    n1 = m5 * m3 - m1 * m7;
    n2 = m6 * m3 - m2 * m7;
    n3 = m8 * m3 - m4 * m7;
    n4 = m9 * m3 - m1 * m11;
    n5 = m10 * m3 - m2 * m11;
    n6 = m12 * m3 - m4 * m11;
    A = (n6 * n2 - n3 * n5) / (n1 * n5 - n4 * n2);
    B = (-n3 - A * n1) / n2;
    C = (-m4 - A * m1 - B * m2) / m3;
    cout << "A = " << A << " B = " << B << " C = " << C << endl;
}

int main()
{
    vector<SPoint> point3d;
    getpoint(point3d);//获取3D空间点，有噪声。
    double aa, bb, cc;
    get_plane(point3d, aa, bb, cc);//拟合平面，平面方程设为Ax+By+Cz+1=0。
    system("pause");
    return 0;
}