1 files changed, 59 insertions, 0 deletions
diff --git a/libcrystfel/src/utils.c b/libcrystfel/src/utils.c
index ae423eed..262a32cc 100644
--- a/libcrystfel/src/utils.c
+++ b/libcrystfel/src/utils.c
@@ -109,6 +109,65 @@ void show_vector(gsl_vector *v)
 }
 
 
+gsl_matrix *matrix_mult(gsl_matrix *A, gsl_matrix *B)
+{
+	gsl_matrix *r = gsl_matrix_calloc(A->size1, A->size2);
+	gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, A, B, 0.0, r);
+	return r;
+}
+
+
+gsl_matrix *matrix_mult3(gsl_matrix *A, gsl_matrix *B, gsl_matrix *C)
+{
+	gsl_matrix *tmp = matrix_mult(B, C);
+	gsl_matrix *r = matrix_mult(A, tmp);
+	gsl_matrix_free(tmp);
+	return r;
+}
+
+
+gsl_matrix *matrix_invert(gsl_matrix *m)
+{
+	gsl_permutation *perm;
+	gsl_matrix *inv;
+	int s;
+
+	perm = gsl_permutation_alloc(m->size1);
+	if ( perm == NULL ) {
+		ERROR("Couldn't allocate permutation\n");
+		gsl_matrix_free(m);
+		return NULL;
+	}
+
+	inv = gsl_matrix_alloc(m->size1, m->size2);
+	if ( inv == NULL ) {
+		ERROR("Couldn't allocate inverse\n");
+		gsl_matrix_free(m);
+		gsl_permutation_free(perm);
+		return NULL;
+	}
+
+	if ( gsl_linalg_LU_decomp(m, perm, &s) ) {
+		ERROR("Couldn't decompose matrix\n");
+		gsl_matrix_free(m);
+		gsl_permutation_free(perm);
+		return NULL;
+	}
+
+	if ( gsl_linalg_LU_invert(m, perm, inv)  ) {
+		ERROR("Couldn't invert cell matrix:\n");
+		gsl_matrix_free(m);
+		gsl_permutation_free(perm);
+		return NULL;
+	}
+
+	gsl_permutation_free(perm);
+	gsl_matrix_free(m);
+
+	return inv;
+}
+
+
 static int check_eigen(gsl_vector *e_val, int verbose)
 {
 	int i;