1 files changed, 20 insertions, 6 deletions

diff --git a/libcrystfel/src/utils.c b/libcrystfel/src/utils.c
index 7bfac487..a35ba272 100644
--- a/libcrystfel/src/utils.c
+++ b/libcrystfel/src/utils.c
@@ -135,39 +135,53 @@ gsl_matrix *matrix_invert(gsl_matrix *m)
 	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);
+		ERROR("Couldn't invert matrix:\n");
 		gsl_permutation_free(perm);
 		return NULL;
 	}
 
 	gsl_permutation_free(perm);
-	gsl_matrix_free(m);
 
 	return inv;
 }
 
 
+gsl_vector *solve_inv(gsl_vector *v, gsl_matrix *M)
+{
+	gsl_matrix *Minv;
+	gsl_vector *s;
+	int n;
+
+	n = v->size;
+	if ( v->size != M->size1 ) return NULL;
+	if ( v->size != M->size2 ) return NULL;
+
+	Minv =  matrix_invert(M);
+	if ( Minv == NULL ) return NULL;
+
+	s = gsl_vector_calloc(n);
+	gsl_blas_dgemv(CblasNoTrans, 1.0, M, v, 0.0, s);
+	return s;
+}
+
+
 static int check_eigen(gsl_vector *e_val, int verbose)
 {
 	int i;