void destroy_hashmap(hashmap_p m){
int x;
item_t* itm;
for(x=0;x<m->num_buckets;++x){
itm = m->buckets[x];
while(itm!=NULL){
free(itm->key);
m->destructor(itm->val);
free(itm);
itm = itm->next;
}
}
destroy_vector(m->keys);
free(m->buckets);
free(m);
}
void decode_vector(char *s, int length, int r, int m) {
vector *e;
vector *d;
e = to_int_vector(s, length);
d = decode(e,r,m);
printf("encoded:\n");
print_vector(e, stdout);
if (d == NULL) {
printf("Error: 0s and 1s tied in majority logic.\n");
} else {
printf("decoded:\n");
print_vector(d, stdout);
}
destroy_vector(e);
printf("\n\n");
}
intptr_t _mock(const char *function, const char *parameters, ...) {
unwanted_check(function);
RecordedExpectation *expectation = find_expectation(function);
if (expectation != NULL) {
Vector *names = create_vector_of_names(parameters);
int i;
va_list actual;
va_start(actual, parameters);
for (i = 0; i < vector_size(names); i++) {
apply_any_constraints(expectation, vector_get(names, i), va_arg(actual, intptr_t));
}
va_end(actual);
destroy_vector(names);
}
return stubbed_result(function);
}
/*******************************************************************************
* vector_t lin_system_solve_qr(matrix_t A, vector_t b)
*
* Gives a least-squares solution to the system AX=b for non-square A using QR.
*
* Ax=b
* QRx=b
* Rx=Q'b (because Q'Q=I)
* then solve for x with gaussian elimination
*******************************************************************************/
vector_t lin_system_solve_qr(matrix_t A, vector_t b){
vector_t xout = create_empty_vector();
vector_t temp;
matrix_t Q,R;
int i,k;
if(!A.initialized || !b.initialized){
printf("ERROR: matrix or vector not initialized yet\n");
return xout;
}
// do QR decomposition
if(QR_decomposition(A,&Q,&R)<0){
printf("failed to perform QR decomposition on A\n");
return xout;
}
// transpose Q matrix
if(transpose_matrix(&Q)<0){
printf("ERROR: failed to transpose Q\n");
return xout;
}
// multiply through
temp = matrix_times_col_vec(Q,b);
destroy_matrix(&Q);
// solve for x knowing R is upper triangular
int nDim = R.cols;
xout = create_vector(nDim);
for(k=(nDim-1); k>=0; k--){
xout.data[k] = temp.data[k];
for(i=(k+1); i<nDim; i++){
xout.data[k] -= (R.data[k][i]*xout.data[i]);
}
xout.data[k] = xout.data[k] / R.data[k][k];
}
destroy_matrix(&R);
destroy_vector(&temp);
return xout;
}
/*******************************************************************************
* 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;
}
/*******************************************************************************
* vector_t lin_system_solve(matrix_t A, vector_t b)
*
* Returns the vector x that solves Ax=b
* Thank you to Henry Guennadi Levkin for open sourcing this routine.
*******************************************************************************/
vector_t lin_system_solve(matrix_t A, vector_t b){
float fMaxElem, fAcc;
int nDim,i,j,k,m;
vector_t xout = create_empty_vector();
if(!A.initialized || !b.initialized){
printf("ERROR: matrix or vector not initialized yet\n");
return xout;
}
if(A.cols != b.len){
printf("ERROR: matrix dimensions do not match\n");
return xout;
}
nDim = A.cols;
xout = create_vector(nDim);
matrix_t Atemp = duplicate_matrix(A); // duplicate the given matrix
vector_t btemp = duplicate_vector(b); // duplicate the given vector
for(k=0; k<(nDim-1); k++){ // base row of matrix
// search of line with max element
fMaxElem = fabs( Atemp.data[k][k]);
m = k;
for(i=k+1; i<nDim; i++){
if(fMaxElem < fabs(Atemp.data[i][k])){
fMaxElem = Atemp.data[i][k];
m = i;
}
}
// permutation of base line (index k) and max element line(index m)
if(m != k){
for(i=k; i<nDim; i++){
fAcc = Atemp.data[k][i];
Atemp.data[k][i] = Atemp.data[m][i];
Atemp.data[m][i] = fAcc;
}
fAcc = btemp.data[k];
btemp.data[k] = btemp.data[m];
btemp.data[m] = fAcc;
}
if(Atemp.data[k][k] == 0.0) return xout; // needs improvement !!!
// triangulation of matrix with coefficients
for(j=(k+1); j<nDim; j++){ // current row of matrix
fAcc = - Atemp.data[j][k] / Atemp.data[k][k];
for(i=k; i<nDim; i++){
Atemp.data[j][i] = Atemp.data[j][i] + fAcc*Atemp.data[k][i] ;
}
// free member recalculation
btemp.data[j] = btemp.data[j] + fAcc*btemp.data[k];
}
}
for(k=(nDim-1); k>=0; k--){
xout.data[k] = btemp.data[k];
for(i=(k+1); i<nDim; i++){
xout.data[k] -= (Atemp.data[k][i]*xout.data[i]);
}
xout.data[k] = xout.data[k] / Atemp.data[k][k];
}
destroy_matrix(&Atemp);
destroy_vector(&btemp);
return xout;
}
/*******************************************************************************
* int QR_decomposition(matrix_t A, matrix_t* Q, matrix_t* R)
*
*
*******************************************************************************/
int QR_decomposition(matrix_t A, matrix_t* Q, matrix_t* R){
int i, j, k, s;
int m = A.rows;
int n = A.cols;
vector_t xtemp;
matrix_t Qt, Rt, Qi, F, temp;
if(!A.initialized){
printf("ERROR: matrix not initialized yet\n");
return -1;
}
destroy_matrix(Q);
destroy_matrix(R);
Qt = create_matrix(m,m);
for(i=0;i<m;i++){ // initialize Qt as I
Qt.data[i][i] = 1;
}
Rt = duplicate_matrix(A); // duplicate A to Rt
for(i=0;i<n;i++){ // iterate through columns of A
xtemp = create_vector(m-i); // allocate length, decreases with i
for(j=i;j<m;j++){ // take col of -R from diag down
xtemp.data[j-i] = -Rt.data[j][i];
}
if(Rt.data[i][i] > 0) s = -1; // check the sign
else s = 1;
xtemp.data[0] += s*vector_norm(xtemp); // add norm to 1st element
Qi = create_square_matrix(m); // initialize Qi
F = create_square_matrix(m-i); // initialize shrinking householder_matrix
F = householder_matrix(xtemp); // fill in Househodor
for(j=0;j<i;j++){
Qi.data[j][j] = 1; // fill in partial I matrix
}
for(j=i;j<m;j++){ // fill in remainder (householder_matrix)
for(k=i;k<m;k++){
Qi.data[j][k] = F.data[j-i][k-i];
}
}
// multiply new Qi to old Qtemp
temp = duplicate_matrix(Qt);
destroy_matrix(&Qt);
Qt = multiply_matrices(Qi,temp);
destroy_matrix(&temp);
// same with Rtemp
temp = duplicate_matrix(Rt);
destroy_matrix(&Rt);
Rt = multiply_matrices(Qi,temp);
destroy_matrix(&temp);
// free other allocation used in this step
destroy_matrix(&Qi);
destroy_matrix(&F);
destroy_vector(&xtemp);
}
transpose_matrix(&Qt);
*Q = Qt;
*R = Rt;
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;
}
static void destroy_expectation(void *abstract) {
RecordedExpectation *expectation = (RecordedExpectation *)abstract;
destroy_vector(expectation->constraints);
free(expectation);
}
