player/Project_App_Chat/App_Src/calc/ellipsoid.c

#include "ellipsoid.h"
#include "stdio.h"
#include "string.h"
#include "math.h"




#define printf(a,fmt...) 





/*=================================<3D><><EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD>˽<EFBFBD>к<EFBFBD><D0BA><EFBFBD>=====================================*/

//ȡ<><C8A1><EFBFBD><EFBFBD>ֵ
static float Abs(float a)
{
	return a < 0 ? -a : a;
}

//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA>λ<EFBFBD><CEBB>
static void Row2_swop_Row1(float matrix[MATRIX_SIZE][MATRIX_SIZE+1],int row1, int row2)
{
	float tmp = 0;
	for (int column = 0; column < MATRIX_SIZE + 1; column++)
	{
		tmp = matrix[row1][column];
		matrix[row1][column] = matrix[row2][column];
		matrix[row2][column] = tmp;
	}
}

//<2F>ð<EFBFBD>row<6F>е<EFBFBD>Ԫ<EFBFBD>س<EFBFBD><D8B3><EFBFBD>һ<EFBFBD><D2BB>ϵ<EFBFBD><CFB5>k
static void k_muiltply_Row(float matrix[MATRIX_SIZE][MATRIX_SIZE+1],float k, int row)
{
	for (int column = 0; column < MATRIX_SIZE + 1; column++)
		matrix[row][column] *= k;
}

//<2F><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>row1<77>мӵ<D0BC>row2<77><32><EFBFBD><EFBFBD>ȥ
static void Row2_add_kRow1(float matrix[MATRIX_SIZE][MATRIX_SIZE+1],float k, int row1, int row2)
{
	for (int column = 0; column < MATRIX_SIZE + 1; column++)
		matrix[row2][column] += k*matrix[row1][column];
}

//<2F><><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD>k<EFBFBD><6B><EFBFBD><EFBFBD>Ԫ֮ǰ<D6AE><C7B0><EFBFBD><EFBFBD>k<EFBFBD>е<EFBFBD>MATRIX_SIZE<5A><45><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>н<EFBFBD><D0BD><EFBFBD>ð<EFBFBD><C3B0><EFBFBD>ų<EFBFBD><C5B3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>k<EFBFBD>е<EFBFBD>Ԫ<EFBFBD>صĴ<D8B5>С
static void MoveBiggestElement_to_Top(float matrix[MATRIX_SIZE][MATRIX_SIZE+1],int k)
{
	int row = 0, column = 0;

	for (row = k + 1; row < MATRIX_SIZE; row++)
	{
		if (Abs(matrix[k][k]) < Abs(matrix[row][k]))
		{
			Row2_swop_Row1(matrix,k, row);
		}
	}
}

//<2F><>˹<EFBFBD><CBB9>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>н<EFBFBD><D0BD><EFBFBD><EFBFBD>;<EFBFBD><CDBE><EFBFBD>,<2C><><EFBFBD><EFBFBD>0<EFBFBD>ɹ<EFBFBD>
static int Matrix_GaussElimination(float matrix[MATRIX_SIZE][MATRIX_SIZE+1])
{
	float k = 0;
	for (int cnt = 0; cnt < MATRIX_SIZE; cnt++)//<2F><><EFBFBD>е<EFBFBD>k<EFBFBD>ε<EFBFBD><CEB5><EFBFBD><EFBFBD>㣬<EFBFBD><E3A3AC>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>k<EFBFBD><6B><EFBFBD><EFBFBD><EFBFBD>µ<EFBFBD><C2B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>k<EFBFBD>е<EFBFBD>Ԫ<EFBFBD>ض<EFBFBD><D8B6><EFBFBD><EFBFBD><EFBFBD>0
	{
		//<2F><>k<EFBFBD><6B><EFBFBD><EFBFBD><EFBFBD><EFBFBD>k<EFBFBD>е<EFBFBD>Ԫ<EFBFBD>ش<EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		MoveBiggestElement_to_Top(matrix,cnt);
		if (matrix[cnt][cnt] == 0)
			return(1);//<2F><><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>ʾ<EFBFBD><CABE><EFBFBD><EFBFBD>
		//<2F><>k<EFBFBD><6B><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA>ȫ<EFBFBD><C8AB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD><EFBFBD>б仯
		for (int row = cnt + 1; row < MATRIX_SIZE; row++)
		{
			k = -matrix[row][cnt] / matrix[cnt][cnt];
			Row2_add_kRow1(matrix,k, cnt, row);
		}
//		DispMatrix();
	}
	return 0;
}

//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>;<EFBFBD><CDBE>󣬼<EFBFBD><F3A3ACBC>ѶԽ<D1B6><D4BD>ߵ<EFBFBD>Ԫ<EFBFBD><D4AA>ȫ<EFBFBD><C8AB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
static void Matrix_RowSimplify(float matrix[MATRIX_SIZE][MATRIX_SIZE+1])
{
	float k = 0;
	for (int row = 0; row < MATRIX_SIZE; row++)
	{
		k = 1 / matrix[row][row];
		k_muiltply_Row(matrix,k, row);
	}
//	DispMatrix();
}

//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Է<EFBFBD><D4B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ľ<EFBFBD>
static void Matrix_Solve(float matrix[MATRIX_SIZE][MATRIX_SIZE+1],float* solve)
{
	for (short row = MATRIX_SIZE - 1; row >= 0; row--)
	{
		solve[row] = matrix[row][MATRIX_SIZE];
		for (int column = MATRIX_SIZE - 1; column >= row + 1; column--)
			solve[row] -= matrix[row][column] * solve[column];
	}
	printf("  a = %f| b = %f| c = %f| d = %f| e = %f| f = %f ", solve[0], solve[1], solve[2], solve[3], solve[4], solve[5]);
	printf("\r\n");
	printf("\r\n");
}

/*=================================<3D><><EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD>˽<EFBFBD>к<EFBFBD><D0BA><EFBFBD>End=====================================*/







//<2F><><EFBFBD><EFBFBD>
void ellipsoid_reset(ellipsoid_struct *ell)
{
	if(ell==0) return;
	memset(ell,0,sizeof(ellipsoid_struct));
}




//<2F><><EFBFBD>ӵ㲢<D3B5><E3B2A2><EFBFBD><EFBFBD>
void ellipsoid_add_point(ellipsoid_struct *ell,float x,float y,float z)
{
	if(ell==0) return;
	
	
	ell->num++;
	double V[MATRIX_SIZE + 1];
	V[0] = y*y;
	V[1] = z*z;
	V[2] = x;
	V[3] = y;
	V[4] = z;
	V[5] = 1.0;
	V[6] = -x*x;
	
	//<2F><><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD>󣬲<EFBFBD><F3A3ACB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ۼ<EFBFBD>
	for (int row = 0; row < MATRIX_SIZE; row++)
	{
		for (int column = 0; column < MATRIX_SIZE + 1; column++)
		{
			ell->sum[row][column] += V[row] * V[column];
		}
	}
	//b<><62><EFBFBD><EFBFBD><EFBFBD><EFBFBD>m_matrix[row][6]
	
}


//<2F><>ƽ<EFBFBD><C6BD>ֵ
void ellipsoid_average(ellipsoid_struct *ell)
{
	if(ell==0) return;

	for (int row = 0; row < MATRIX_SIZE; row++)
	for (int column = 0; column < MATRIX_SIZE + 1; column++)
		ell->avr[row][column]=ell->sum[row][column] / ell->num;
	
	//b<><62><EFBFBD><EFBFBD><EFBFBD><EFBFBD>m_matrix[row][6]
}


//<2F><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>
void ellipsoid_fitting(ellipsoid_struct *ell)
{
	if(ell==0) return;
	
	float solve[6]={0};

	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD>֮<EFBFBD><D6AE><EFBFBD><EFBFBD>ȡƽ<C8A1><C6BD>ֵ
	ellipsoid_average(ell);
	
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	if(Matrix_GaussElimination(ell->avr)==0)
	{
		Matrix_RowSimplify(ell->avr);
		Matrix_Solve(ell->avr,solve);
		
		float a = 0, b = 0, c = 0, d = 0, e = 0, f = 0;
		a = solve[0];
		b = solve[1];
		c = solve[2];
		d = solve[3];
		e = solve[4];
		f = solve[5];

		ell->x = -c / 2;
		ell->y = -d / (2 * a);
		ell->z = -e / (2 * b);
		ell->a = sqrt(ell->x*ell->x + a*ell->y*ell->y + b*ell->z*ell->z - f);
		ell->b = ell->a / sqrtf(a);
		ell->c = ell->a / sqrtf(b);
		printf("  ((x - x0) / A) ^ 2 + ((y - y0) / B) ^ 2 + ((z - z0) / C) ^ 2 = 1 Ellipsoid result as below<6F><77>\r\n");
		printf("\r\n");
		printf("  X0 = %f| Y0 = %f| Z0 = %f| A = %f| B = %f| C = %f \r\n", ell->x, ell->y,ell->z, ell->a, ell->b, ell->c);
		
	}
	
}