/*******************************************************************************
* int fit_ellipsoid(matrix_t points, vector_t* center, vector_t* lengths)
*
* Fits an ellipsoid to a set of points in 3D space. The principle axes of the
* fitted ellipsoid align with the global coordinate system. Therefore there are
* 6 degrees of freedom defining the ellipsoid: the x,y,z coordinates of the
* centroid and the lengths from the centroid to the surfance in each of the 3
* directions.
*
* matrix_t points is a tall matrix with 3 columns and at least 6 rows. Each row
* must contain the xy&z components of each individual point to be fit. If only
* 6 rows are provided, the resulting ellipsoid will be an exact fit. Otherwise
* the result is a least-squares fit to the overdefined dataset.
*
* vector_t* center is a pointer to a user-created vector which will contain the
* x,y,z position of the centroid of the fit ellipsoid.
*
* vector_t* lengths is a pointer to a user-created vector which will be
* populated with the 3 distances from the surface to the centroid in each of the
* 3 directions.
*******************************************************************************/
int fit_ellipsoid(matrix_t points, vector_t* center, vector_t* lengths){
int i,p;
matrix_t A;
vector_t b;
if(!points.initialized){
printf("ERROR: matrix_t points not initialized\n");
return -1;
}
if(points.cols!=3){
printf("ERROR: matrix_t points must have 3 columns\n");
return -1;
}
p = points.rows;
if(p<6){
printf("ERROR: matrix_t points must have at least 6 rows\n");
return -1;
}
b = create_vector_of_ones(p);
A = create_matrix(p,6);
for(i=0;i<p;i++){
A.data[i][0] = points.data[i][0] * points.data[i][0];
A.data[i][1] = points.data[i][0];
A.data[i][2] = points.data[i][1] * points.data[i][1];
A.data[i][3] = points.data[i][1];
A.data[i][4] = points.data[i][2] * points.data[i][2];
A.data[i][5] = points.data[i][2];
}
vector_t f = lin_system_solve_qr(A,b);
destroy_matrix(&A);
destroy_vector(&b);
// compute center
*center = create_vector(3);
center->data[0] = -f.data[1]/(2*f.data[0]);
center->data[1] = -f.data[3]/(2*f.data[2]);
center->data[2] = -f.data[5]/(2*f.data[4]);
// Solve for lengths
A = create_square_matrix(3);
b = create_vector(3);
// fill in A
A.data[0][0] = (f.data[0] * center->data[0] * center->data[0]) + 1.0;
A.data[0][1] = (f.data[0] * center->data[1] * center->data[1]);
A.data[0][2] = (f.data[0] * center->data[2] * center->data[2]);
A.data[1][0] = (f.data[2] * center->data[0] * center->data[0]);
A.data[1][1] = (f.data[2] * center->data[1] * center->data[1]) + 1.0;
A.data[1][2] = (f.data[2] * center->data[2] * center->data[2]);
A.data[2][0] = (f.data[4] * center->data[0] * center->data[0]);
A.data[2][1] = (f.data[4] * center->data[1] * center->data[1]);
A.data[2][2] = (f.data[4] * center->data[2] * center->data[2]) + 1.0;
// fill in b
b.data[0] = f.data[0];
b.data[1] = f.data[2];
b.data[2] = f.data[4];
// solve for lengths
vector_t scales = lin_system_solve(A, b);
*lengths = create_vector(3);
lengths->data[0] = 1.0/sqrt(scales.data[0]);
lengths->data[1] = 1.0/sqrt(scales.data[1]);
lengths->data[2] = 1.0/sqrt(scales.data[2]);
// cleanup
destroy_vector(&scales);
destroy_matrix(&A);
destroy_vector(&b);
return 0;
}
int main(){
printf("Let's test some linear algebra functions....\n\n");
// create a random nxn matrix for later use
matrix_t A = create_random_matrix(DIM,DIM);
printf("New Random Matrix A:\n");
print_matrix(A);
// also create random vector
vector_t b = create_random_vector(DIM);
printf("\nNew Random Vector b:\n");
print_vector(b);
// do an LUP decomposition on A
matrix_t L,U,P;
LUP_decomposition(A,&L,&U,&P);
printf("\nL:\n");
print_matrix(L);
printf("U:\n");
print_matrix(U);
printf("P:\n");
print_matrix(P);
// do a QR decomposition on A
matrix_t Q,R;
QR_decomposition(A,&Q,&R);
printf("\nQR Decomposition of A\n");
printf("Q:\n");
print_matrix(Q);
printf("R:\n");
print_matrix(R);
// get determinant of A
float det = matrix_determinant(A);
printf("\nDeterminant of A : %8.4f\n", det);
// get an inverse for A
matrix_t Ainv = invert_matrix(A);
if(A.initialized != 1) return -1;
printf("\nAinverse\n");
print_matrix(Ainv);
// multiply A times A inverse
matrix_t AA = multiply_matrices(A,Ainv);
if(AA.initialized!=1) return -1;
printf("\nA * Ainverse:\n");
print_matrix(AA);
// solve a square linear system
vector_t x = lin_system_solve(A, b);
printf("\nGaussian Elimination solution x to the equation Ax=b:\n");
print_vector(x);
// now do again but with qr decomposition method
vector_t xqr = lin_system_solve_qr(A, b);
printf("\nQR solution x to the equation Ax=b:\n");
print_vector(xqr);
// If b are the coefficients of a polynomial, get the coefficients of the
// new polynomial b^2
vector_t bb = poly_power(b,2);
printf("\nCoefficients of polynomial b times itself\n");
print_vector(bb);
// clean up all the allocated memory. This isn't strictly necessary since
// we are already at the end of the program, but good practice to do.
destroy_matrix(&A);
destroy_matrix(&AA);
destroy_vector(&b);
destroy_vector(&bb);
destroy_vector(&x);
destroy_vector(&xqr);
destroy_matrix(&Q);
destroy_matrix(&R);
printf("DONE\n");
return 0;
}
