int
matrix_is_sigular( Matrix * m )
{
if( matrix_determinant(m) == 0 )
return 1;
else
return 0;
}
/**
* Figure out what region the origin is in relative to a collision simplex.
* Returns a vertex index for the region opposite that vertex, 4 if we're
* inside the simplex
**/
static size_t
object_simplex_detect_region(float simplex[4][3], size_t simplex_pos)
{
MATRIX_DECL(base_det_matrix,
simplex[0][0], simplex[0][1], simplex[0][2], 1,
simplex[1][0], simplex[1][1], simplex[1][2], 1,
simplex[2][0], simplex[2][1], simplex[2][2], 1,
simplex[3][0], simplex[3][1], simplex[3][2], 1);
float boundary_det_matrices[4][16] = {
{ 0, simplex[1][0], simplex[2][0], simplex[3][0],
0, simplex[1][1], simplex[2][1], simplex[3][1],
0, simplex[1][2], simplex[2][2], simplex[3][2],
1, 1, 1, 1,
}, { simplex[0][0], 0, simplex[2][0], simplex[3][0],
simplex[0][1], 0, simplex[2][1], simplex[3][1],
simplex[0][2], 0, simplex[2][2], simplex[3][2],
1, 1, 1, 1,
}, { simplex[0][0], simplex[1][0], 0, simplex[3][0],
simplex[0][1], simplex[1][1], 0, simplex[3][1],
simplex[0][2], simplex[1][2], 0, simplex[3][2],
1, 1, 1, 1,
}, { simplex[0][0], simplex[1][0], simplex[2][0], 0,
simplex[0][1], simplex[1][1], simplex[2][1], 0,
simplex[0][2], simplex[1][2], simplex[2][2], 0,
1, 1, 1, 1,
},
};
float base_det;
size_t i;
base_det = matrix_determinant(base_det_matrix);
for (i = 0; i < 4; i++) {
if (i == simplex_pos)
continue;
if (base_det *
matrix_determinant(boundary_det_matrices[i]) > 0)
break;
}
return i;
}
Matrix matrix_inverse(Matrix M)
{
if(!is_square_matrix(M))
return NULL;
int n = M->r;
Matrix N = matrix_new_empty(n, n);
double det = matrix_determinant(M);
if(double_equals(det, 0))
return NULL;
for(int i = 0; i < n; i++)
for(int j = 0; j < n; j++) {
Matrix C = matrix_cofactor(M, i, j);
if((i + j) % 2 == 0)
N->A[i][j] = matrix_determinant(C) / det;
else
N->A[i][j] = -1 * matrix_determinant(C) / det;
matrix_free(C);
}
Matrix I = matrix_transpose(N);
matrix_free(N);
assert(is_square_matrix(I));
return I;
}
/*******************************************************************************
* matrix_t invert_matrix(matrix_t A)
*
* Invert Matrix function based on LUP decomposition and then forward and
* backward substitution.
*******************************************************************************/
matrix_t invert_matrix(matrix_t A){
int i,j,k,m;
matrix_t L,U,P,D,temp;
matrix_t out = create_empty_matrix();
if(!A.initialized){
printf("ERROR: matrix not initialized yet\n");
return out;
}
if(A.cols != A.rows){
printf("ERROR: matrix is not square\n");
return out;
}
if(matrix_determinant(A) == 0){
printf("ERROR: matrix is singular, not invertible\n");
return out;
}
m = A.cols;
LUP_decomposition(A,&L,&U,&P);
D = create_identity_matrix(m);
temp = create_square_matrix(m);
for(j=0;j<m;j++){
for(i=0;i<m;i++){
for(k=0;k<i;k++){
D.data[i][j] -= L.data[i][k] * D.data[k][j];
}
}
for(i=m-1;i>=0;i--){ // backwards.. last to first
temp.data[i][j] = D.data[i][j];
for(k=i+1;k<m;k++){
temp.data[i][j] -= U.data[i][k] * temp.data[k][j];
}
temp.data[i][j] = temp.data[i][j] / U.data[i][i];
}
}
// multiply by permutation matrix
out = multiply_matrices(temp, P);
// free allocation
destroy_matrix(&temp);
destroy_matrix(&L);
destroy_matrix(&U);
destroy_matrix(&P);
destroy_matrix(&D);
return out;
}
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;
}
