private void EllipseRegression(out float Xc, out float Yc, out float a, out float b, out float theta)
{
int n = nPoints; //Minimum 5
float sum_x = 0;
float sum_y = 0;
float sum_xy = 0;
float sum_x2y = 0;
float sum_x3y = 0;
float sum_xy2 = 0;
float sum_xy3 = 0;
float sum_x2y2 = 0;
float sum_x2 = 0;
float sum_x3 = 0;
float sum_x4 = 0;
float sum_y2 = 0;
float sum_y3 = 0;
float sum_y4 = 0;
for (int i = 0; i < n; i++)
{
sum_x = sum_x + _CurvePoints[i].X;
sum_y = sum_y + _CurvePoints[i].Y;
sum_xy = sum_xy + _CurvePoints[i].X * _CurvePoints[i].Y;
sum_x2y = sum_x2y + (float)Math.Pow(_CurvePoints[i].X, 2) * _CurvePoints[i].Y;
sum_x3y = sum_x3y + (float)Math.Pow(_CurvePoints[i].X, 3) * _CurvePoints[i].Y;
sum_xy2 = sum_xy2 + _CurvePoints[i].X * (float)Math.Pow(_CurvePoints[i].Y, 2);
sum_xy3 = sum_xy3 + _CurvePoints[i].X * (float)Math.Pow(_CurvePoints[i].Y, 3);
sum_x2y2 = sum_x2y2 + (float)Math.Pow(_CurvePoints[i].X, 2) * (float)Math.Pow(_CurvePoints[i].Y, 2);
sum_x2 = sum_x2 + (float)Math.Pow(_CurvePoints[i].X, 2);
sum_x3 = sum_x3 + (float)Math.Pow(_CurvePoints[i].X, 3);
sum_x4 = sum_x4 + (float)Math.Pow(_CurvePoints[i].X, 4);
sum_y2 = sum_y2 + (float)Math.Pow(_CurvePoints[i].Y, 2);
sum_y3 = sum_y3 + (float)Math.Pow(_CurvePoints[i].Y, 3);
sum_y4 = sum_y4 + (float)Math.Pow(_CurvePoints[i].Y, 4);
}
double[][] m = MatrixCalc.MatrixCreate(5, 5);
m[0][0] = sum_y4; m[0][1] = sum_x2y2; m[0][2] = sum_xy3; m[0][3] = sum_y3; m[0][4] = sum_xy2;
m[1][0] = sum_x2y2; m[1][1] = sum_x4; m[1][2] = sum_x3y; m[1][3] = sum_x2y; m[1][4] = sum_x3;
m[2][0] = sum_xy3; m[2][1] = sum_x3y; m[2][2] = sum_x2y2; m[2][3] = sum_xy2; m[2][4] = sum_x2y;
m[3][0] = sum_y3; m[3][1] = sum_x2y; m[3][2] = sum_xy2; m[3][3] = sum_y2; m[3][4] = sum_xy;
m[4][0] = sum_xy2; m[4][1] = sum_x3; m[4][2] = sum_x2y; m[4][3] = sum_xy; m[4][4] = sum_x2;
double[][] vars = MatrixCalc.MatrixCreate(5, 1);
vars[0][0] = -sum_y2; //Cy2
vars[1][0] = -sum_x2; //Ax2
vars[2][0] = -sum_xy; //Bxy
vars[3][0] = -sum_y; //Ey
vars[4][0] = -sum_x; //Dx
double[][] inv = MatrixCalc.MatrixInverse(m);
double[][] m1 = MatrixCalc.MatrixProduct(inv, vars);
//Ay2 + Bx2 + Cxy + Dy + Ex + 1 = 0
float C = (float)m1[0][0];
float A = (float)m1[1][0];
float B = (float)m1[2][0];
float E = (float)m1[3][0];
float D = (float)m1[4][0];
float F = 1;
//Ax2 + Bxy + Cy2 + Dx + Ey + F = 0
Xc = ((2 * C * D) - (B * E)) / ((float)Math.Pow(B, 2) - 4 * A * C);
Yc = ((2 * A * E) - (B * D)) / ((float)Math.Pow(B, 2) - 4 * A * C);
a = -(float)Math.Sqrt(2 * (A * (float)Math.Pow(E, 2) + C * (float)Math.Pow(D, 2) - B * D * E + ((float)Math.Pow(B, 2) - 4 * A * C) * F) * (A + C + (float)Math.Sqrt((float)Math.Pow((A - C), 2) + (float)Math.Pow(B, 2)))) / ((float)Math.Pow(B, 2) - 4 * A * C);
b = -(float)Math.Sqrt(2 * (A * (float)Math.Pow(E, 2) + C * (float)Math.Pow(D, 2) - B * D * E + ((float)Math.Pow(B, 2) - 4 * A * C) * F) * (A + C - (float)Math.Sqrt((float)Math.Pow((A - C), 2) + (float)Math.Pow(B, 2)))) / ((float)Math.Pow(B, 2) - 4 * A * C);
if (a < b)
{
float c = a;
a = b;
b = c;
}
if (B == 0)
{
if (A < C)
{
theta = 0;
}
else
{
theta = (float)Math.PI / 2;
}
}
else
{
theta = (float)(Math.Atan((C - A - (float)Math.Sqrt((float)Math.Pow((A - C), 2) + (float)Math.Pow(B, 2)))) / B);
}
theta = theta * 180 / (float)Math.PI;
}