Mesh Deform Modifier

====================

The MeshDeform modifier can deform a mesh with another 'cage' mesh.
It is similar to a lattice modifier, but instead of being restricted
to the regular grid layout of a lattice, the cage mesh can be modeled
to fit the mesh better.

http://www.blender.org/development/current-projects/changes-since-244/modifiers/

Implementation Notes:
- OpenNL has been refactored a bit to allow least squares matrices to
  be built without passing the matrix row by row, but instead with
  random access. MDef doesn't need this actually, but it's using this
  version of OpenNL so I'm just committing it now.
- Mean value weights for polygons have been added to arithb.c, a type
  of barycentric coordinates for polygons with >= 3 vertices. This
  might be useful for other parts of blender too.

2 files changed, 254 insertions, 236 deletions

diff --git a/intern/opennl/extern/ONL_opennl.h b/intern/opennl/extern/ONL_opennl.h
index 7f501629290..be76aa95eac 100644
--- a/intern/opennl/extern/ONL_opennl.h
+++ b/intern/opennl/extern/ONL_opennl.h
@@ -79,20 +79,16 @@ typedef void* NLContext;
 
 #define NL_SYSTEM  0x0
 #define NL_MATRIX  0x1
-#define NL_ROW     0x2
 
 /* Solver Parameters */
 
-#define NL_SOLVER           0x100
-#define NL_NB_VARIABLES     0x101
-#define NL_LEAST_SQUARES    0x102
-#define NL_SYMMETRIC        0x106
-#define NL_ERROR            0x108
-
-/* Row parameters */
-
-#define NL_RIGHT_HAND_SIDE 0x500
-#define NL_ROW_SCALING     0x501
+#define NL_SOLVER              0x100
+#define NL_NB_VARIABLES        0x101
+#define NL_LEAST_SQUARES       0x102
+#define NL_SYMMETRIC           0x106
+#define NL_ERROR               0x108
+#define NL_NB_ROWS             0x110
+#define NL_NB_RIGHT_HAND_SIDES 0x112 /* 4 max */
 
 /* Contexts */
 
@@ -106,9 +102,6 @@ NLContext nlGetCurrent(void);
 void nlSolverParameterf(NLenum pname, NLfloat param);
 void nlSolverParameteri(NLenum pname, NLint param);
 
-void nlRowParameterf(NLenum pname, NLfloat param);
-void nlRowParameteri(NLenum pname, NLint param);
-
 void nlGetBooleanv(NLenum pname, NLboolean* params);
 void nlGetFloatv(NLenum pname, NLfloat* params);
 void nlGetIntergerv(NLenum pname, NLint* params);
@@ -119,8 +112,8 @@ NLboolean nlIsEnabled(NLenum pname);
 
 /* Variables */
 
-void nlSetVariable(NLuint index, NLfloat value);
-NLfloat nlGetVariable(NLuint index);
+void nlSetVariable(NLuint rhsindex, NLuint index, NLfloat value);
+NLfloat nlGetVariable(NLuint rhsindex, NLuint index);
 void nlLockVariable(NLuint index);
 void nlUnlockVariable(NLuint index);
 NLboolean nlVariableIsLocked(NLuint index);
@@ -129,14 +122,16 @@ NLboolean nlVariableIsLocked(NLuint index);
 
 void nlBegin(NLenum primitive);
 void nlEnd(NLenum primitive);
-void nlCoefficient(NLuint index, NLfloat value);
 
-/* Setting random elements matrix/vector - not supported for
-   least squares! */
+/* Setting elements in matrix/vector */
 
 void nlMatrixAdd(NLuint row, NLuint col, NLfloat value);
-void nlRightHandSideAdd(NLuint index, NLfloat value);
-void nlRightHandSideSet(NLuint index, NLfloat value);
+void nlRightHandSideAdd(NLuint rhsindex, NLuint index, NLfloat value);
+void nlRightHandSideSet(NLuint rhsindex, NLuint index, NLfloat value);
+
+/* Multiply */
+
+void nlMatrixMultiply(NLfloat *x, NLfloat *y);
 
 /* Solve */
 
diff --git a/intern/opennl/intern/opennl.c b/intern/opennl/intern/opennl.c
index 836c9d01e60..2d30da075d3 100644
--- a/intern/opennl/intern/opennl.c
+++ b/intern/opennl/intern/opennl.c
@@ -207,10 +207,6 @@ static void __nlRowColumnAppend(__NLRowColumn* c, NLint index, NLfloat value) {
 	c->size++;
 }
 
-static void __nlRowColumnZero(__NLRowColumn* c) {
-	c->size = 0;
-}
-
 static void __nlRowColumnClear(__NLRowColumn* c) {
 	c->size	 = 0;
 	c->capacity = 0;
@@ -432,13 +428,62 @@ static void __nlSparseMatrixMult(__NLSparseMatrix* A, NLfloat* x, NLfloat* y) {
 	}
 }
 
+/* ****************** Routines for least squares ******************* */
+
+static void __nlSparseMatrix_square(
+	__NLSparseMatrix* AtA, __NLSparseMatrix *A
+) {
+	NLuint m = A->m;
+	NLuint n = A->n;
+	NLuint i, j0, j1;
+	__NLRowColumn *Ri = NULL;
+	__NLCoeff *c0 = NULL, *c1 = NULL;
+	float value;
+
+	__nlSparseMatrixConstruct(AtA, n, n, A->storage);
+
+	for(i=0; i<m; i++) {
+		Ri = &(A->row[i]);
+
+		for(j0=0; j0<Ri->size; j0++) {
+			c0 = &(Ri->coeff[j0]);
+			for(j1=0; j1<Ri->size; j1++) {
+				c1 = &(Ri->coeff[j1]);
+
+				value = c0->value*c1->value;
+				__nlSparseMatrixAdd(AtA, c0->index, c1->index, value);
+			}
+		}
+	}
+}
+
+static void __nlSparseMatrix_transpose_mult_rows(
+	__NLSparseMatrix* A, NLfloat* x, NLfloat* y
+) {
+	NLuint m = A->m;
+	NLuint n = A->n;
+	NLuint i,ij;
+	__NLRowColumn* Ri = NULL;
+	__NLCoeff* c = NULL;
+
+	__NL_CLEAR_ARRAY(NLfloat, y, n);
+
+	for(i=0; i<m; i++) {
+		Ri = &(A->row[i]);
+		for(ij=0; ij<Ri->size; ij++) {
+			c = &(Ri->coeff[ij]);
+			y[c->index] += c->value * x[i];
+		}
+	}
+}
+
 /************************************************************************************/
 /* NLContext data structure */
 
 typedef void(*__NLMatrixFunc)(float* x, float* y);
 
 typedef struct {
-	NLfloat  value;
+	NLfloat  value[4];
 	NLboolean locked;
 	NLuint	index;
 	__NLRowColumn *a;
@@ -447,32 +492,32 @@ typedef struct {
 #define __NL_STATE_INITIAL				0
 #define __NL_STATE_SYSTEM				1
 #define __NL_STATE_MATRIX				2
-#define __NL_STATE_ROW					3
-#define __NL_STATE_MATRIX_CONSTRUCTED	4
-#define __NL_STATE_SYSTEM_CONSTRUCTED	5
-#define __NL_STATE_SYSTEM_SOLVED		7
+#define __NL_STATE_MATRIX_CONSTRUCTED	3
+#define __NL_STATE_SYSTEM_CONSTRUCTED	4
+#define __NL_STATE_SYSTEM_SOLVED		5
 
 typedef struct {
-	NLenum		   state;
-	NLuint		   n;
+	NLenum			state;
+	NLuint			n;
+	NLuint			m;
 	__NLVariable*	variable;
 	NLfloat*		b;
+	NLfloat*		Mtb;
 	__NLSparseMatrix M;
-	__NLRowColumn	af;
-	__NLRowColumn	al;
+	__NLSparseMatrix MtM;
 	NLfloat*		x;
-	NLfloat		 right_hand_side;
-	NLuint		   nb_variables;
-	NLuint		   current_row;
+	NLuint			nb_variables;
+	NLuint			nb_rows;
 	NLboolean		least_squares;
 	NLboolean		symmetric;
+	NLuint			nb_rhs;
 	NLboolean		solve_again;
 	NLboolean		alloc_M;
-	NLboolean		alloc_af;
-	NLboolean		alloc_al;
+	NLboolean		alloc_MtM;
 	NLboolean		alloc_variable;
 	NLboolean		alloc_x;
 	NLboolean		alloc_b;
+	NLboolean		alloc_Mtb;
 	NLfloat			error;
 	__NLMatrixFunc	matrix_vector_prod;
 
@@ -494,8 +539,8 @@ static void __nlMatrixVectorProd_default(NLfloat* x, NLfloat* y) {
 NLContext nlNewContext(void) {
 	__NLContext* result	  = __NL_NEW(__NLContext);
 	result->state			= __NL_STATE_INITIAL;
-	result->right_hand_side  = 0.0;
 	result->matrix_vector_prod = __nlMatrixVectorProd_default;
+	result->nb_rhs = 1;
 	nlMakeCurrent(result);
 	return result;
 }
@@ -512,11 +557,8 @@ void nlDeleteContext(NLContext context_in) {
 	if(context->alloc_M) {
 		__nlSparseMatrixDestroy(&context->M);
 	}
-	if(context->alloc_af) {
-		__nlRowColumnDestroy(&context->af);
-	}
-	if(context->alloc_al) {
-		__nlRowColumnDestroy(&context->al);
+	if(context->alloc_MtM) {
+		__nlSparseMatrixDestroy(&context->MtM);
 	}
 	if(context->alloc_variable) {
 		for(i=0; i<context->nb_variables; i++) {
@@ -529,6 +571,9 @@ void nlDeleteContext(NLContext context_in) {
 	if(context->alloc_b) {
 		__NL_DELETE_ARRAY(context->b);
 	}
+	if(context->alloc_Mtb) {
+		__NL_DELETE_ARRAY(context->Mtb);
+	}
 	if(context->alloc_x) {
 		__NL_DELETE_ARRAY(context->x);
 	}
@@ -569,12 +614,19 @@ void nlSolverParameterf(NLenum pname, NLfloat param) {
 		__nl_assert(param > 0);
 		__nlCurrentContext->nb_variables = (NLuint)param;
 	} break;
+	case NL_NB_ROWS: {
+		__nl_assert(param > 0);
+		__nlCurrentContext->nb_rows = (NLuint)param;
+	} break;
 	case NL_LEAST_SQUARES: {
 		__nlCurrentContext->least_squares = (NLboolean)param;
 	} break;
 	case NL_SYMMETRIC: {
 		__nlCurrentContext->symmetric = (NLboolean)param;		
-	}
+	} break;
+	case NL_NB_RIGHT_HAND_SIDES: {
+		__nlCurrentContext->nb_rhs = (NLuint)param;
+	} break;
 	default: {
 		__nl_assert_not_reached;
 	} break;
@@ -588,32 +640,21 @@ void nlSolverParameteri(NLenum pname, NLint param) {
 		__nl_assert(param > 0);
 		__nlCurrentContext->nb_variables = (NLuint)param;
 	} break;
+	case NL_NB_ROWS: {
+		__nl_assert(param > 0);
+		__nlCurrentContext->nb_rows = (NLuint)param;
+	} break;
 	case NL_LEAST_SQUARES: {
 		__nlCurrentContext->least_squares = (NLboolean)param;
 	} break;
 	case NL_SYMMETRIC: {
 		__nlCurrentContext->symmetric = (NLboolean)param;		
-	}
-	default: {
-		__nl_assert_not_reached;
 	} break;
-	}
-}
-
-void nlRowParameterf(NLenum pname, NLfloat param) {
-	__nlCheckState(__NL_STATE_MATRIX);
-	switch(pname) {
-	case NL_RIGHT_HAND_SIDE: {
-		__nlCurrentContext->right_hand_side = param;
+	case NL_NB_RIGHT_HAND_SIDES: {
+		__nlCurrentContext->nb_rhs = (NLuint)param;
 	} break;
-	}
-}
-
-void nlRowParameteri(NLenum pname, NLint param) {
-	__nlCheckState(__NL_STATE_MATRIX);
-	switch(pname) {
-	case NL_RIGHT_HAND_SIDE: {
-		__nlCurrentContext->right_hand_side = (NLfloat)param;
+	default: {
+		__nl_assert_not_reached;
 	} break;
 	}
 }
@@ -637,6 +678,9 @@ void nlGetFloatv(NLenum pname, NLfloat* params) {
 	case NL_NB_VARIABLES: {
 		*params = (NLfloat)(__nlCurrentContext->nb_variables);
 	} break;
+	case NL_NB_ROWS: {
+		*params = (NLfloat)(__nlCurrentContext->nb_rows);
+	} break;
 	case NL_LEAST_SQUARES: {
 		*params = (NLfloat)(__nlCurrentContext->least_squares);
 	} break;
@@ -657,6 +701,9 @@ void nlGetIntergerv(NLenum pname, NLint* params) {
 	case NL_NB_VARIABLES: {
 		*params = (NLint)(__nlCurrentContext->nb_variables);
 	} break;
+	case NL_NB_ROWS: {
+		*params = (NLint)(__nlCurrentContext->nb_rows);
+	} break;
 	case NL_LEAST_SQUARES: {
 		*params = (NLint)(__nlCurrentContext->least_squares);
 	} break;
@@ -700,16 +747,16 @@ NLboolean nlIsEnabled(NLenum pname) {
 /************************************************************************************/
 /* Get/Set Lock/Unlock variables */
 
-void nlSetVariable(NLuint index, NLfloat value) {
+void nlSetVariable(NLuint rhsindex, NLuint index, NLfloat value) {
 	__nlCheckState(__NL_STATE_SYSTEM);
 	__nl_parano_range_assert(index, 0, __nlCurrentContext->nb_variables - 1);
-	__nlCurrentContext->variable[index].value = value;	
+	__nlCurrentContext->variable[index].value[rhsindex] = value;	
 }
 
-NLfloat nlGetVariable(NLuint index) {
+NLfloat nlGetVariable(NLuint rhsindex, NLuint index) {
 	__nl_assert(__nlCurrentContext->state != __NL_STATE_INITIAL);
 	__nl_parano_range_assert(index, 0, __nlCurrentContext->nb_variables - 1);
-	return __nlCurrentContext->variable[index].value;
+	return __nlCurrentContext->variable[index].value[rhsindex];
 }
 
 void nlLockVariable(NLuint index) {
@@ -734,31 +781,41 @@ NLboolean nlVariableIsLocked(NLuint index) {
 /* System construction */
 
 static void __nlVariablesToVector() {
-	NLuint i;
+	__NLContext *context = __nlCurrentContext;
+	NLuint i, j, nb_rhs;
 
-	__nl_assert(__nlCurrentContext->alloc_x);
-	__nl_assert(__nlCurrentContext->alloc_variable);
+	__nl_assert(context->alloc_x);
+	__nl_assert(context->alloc_variable);
 
-	for(i=0; i<__nlCurrentContext->nb_variables; i++) {
-		__NLVariable* v = &(__nlCurrentContext->variable[i]);
+	nb_rhs= context->nb_rhs;
+
+	for(i=0; i<context->nb_variables; i++) {
+		__NLVariable* v = &(context->variable[i]);
 		if(!v->locked) {
-			__nl_assert(v->index < __nlCurrentContext->n);
-			__nlCurrentContext->x[v->index] = v->value;
+			__nl_assert(v->index < context->n);
+
+			for(j=0; j<nb_rhs; j++)
+				context->x[context->n*j + v->index] = v->value[j];
 		}
 	}
 }
 
 static void __nlVectorToVariables() {
-	NLuint i;
+	__NLContext *context = __nlCurrentContext;
+	NLuint i, j, nb_rhs;
 
-	__nl_assert(__nlCurrentContext->alloc_x);
-	__nl_assert(__nlCurrentContext->alloc_variable);
+	__nl_assert(context->alloc_x);
+	__nl_assert(context->alloc_variable);
 
-	for(i=0; i<__nlCurrentContext->nb_variables; i++) {
-		__NLVariable* v = &(__nlCurrentContext->variable[i]);
+	nb_rhs= context->nb_rhs;
+
+	for(i=0; i<context->nb_variables; i++) {
+		__NLVariable* v = &(context->variable[i]);
 		if(!v->locked) {
-			__nl_assert(v->index < __nlCurrentContext->n);
-			v->value = __nlCurrentContext->x[v->index];
+			__nl_assert(v->index < context->n);
+
+			for(j=0; j<nb_rhs; j++)
+				v->value[j] = context->x[context->n*j + v->index];
 		}
 	}
 }
@@ -783,8 +840,8 @@ static void __nlEndSystem() {
 }
 
 static void __nlBeginMatrix() {
-	NLuint i, j;
-	NLuint n = 0;
+	NLuint i;
+	NLuint m = 0, n = 0;
 	NLenum storage = __NL_ROWS;
 	__NLContext *context = __nlCurrentContext;
 
@@ -801,57 +858,37 @@ static void __nlBeginMatrix() {
 				context->variable[i].index = n++;
 		}
 
-		context->n = n;
-
-		/* a least squares problem results in a symmetric matrix */
-		if(context->least_squares)
-			context->symmetric = NL_TRUE;
+		m = (context->nb_rows == 0)? n: context->nb_rows;
 
-		if(context->symmetric)
-			storage = (storage | __NL_SYMMETRIC);
-
-		/* SuperLU storage does not support symmetric storage */
-		storage = (storage & ~__NL_SYMMETRIC);
+		context->m = m;
+		context->n = n;
 
-		__nlSparseMatrixConstruct(&context->M, n, n, storage);
+		__nlSparseMatrixConstruct(&context->M, m, n, storage);
 		context->alloc_M = NL_TRUE;
 
-		context->b = __NL_NEW_ARRAY(NLfloat, n);
+		context->b = __NL_NEW_ARRAY(NLfloat, m*context->nb_rhs);
 		context->alloc_b = NL_TRUE;
 
-		context->x = __NL_NEW_ARRAY(NLfloat, n);
+		context->x = __NL_NEW_ARRAY(NLfloat, n*context->nb_rhs);
 		context->alloc_x = NL_TRUE;
 	}
 	else {
 		/* need to recompute b only, A is not constructed anymore */
-		__NL_CLEAR_ARRAY(NLfloat, context->b, context->n);
+		__NL_CLEAR_ARRAY(NLfloat, context->b, context->m*context->nb_rhs);
 	}
 
 	__nlVariablesToVector();
-
-	__nlRowColumnConstruct(&context->af);
-	context->alloc_af = NL_TRUE;
-	__nlRowColumnConstruct(&context->al);
-	context->alloc_al = NL_TRUE;
-
-	context->current_row = 0;
 }
 
-static void __nlEndMatrix() {
+static void __nlEndMatrixRHS(NLuint rhs) {
 	__NLContext *context = __nlCurrentContext;
 	__NLVariable *variable;
 	__NLRowColumn *a;
-	NLfloat *b;
+	NLfloat *b, *Mtb;
 	NLuint i, j;
 
-	__nlTransition(__NL_STATE_MATRIX, __NL_STATE_MATRIX_CONSTRUCTED);	
-	
-	__nlRowColumnDestroy(&context->af);
-	context->alloc_af = NL_FALSE;
-	__nlRowColumnDestroy(&context->al);
-	context->alloc_al = NL_FALSE;
-	
-	b = context->b;
+	b = context->b + context->m*rhs;
+	Mtb = context->Mtb + context->n*rhs;
 
 	for(i=0; i<__nlCurrentContext->nb_variables; i++) {
 		variable = &(context->variable[i]);
@@ -860,73 +897,34 @@ static void __nlEndMatrix() {
 			a = variable->a;
 
 			for(j=0; j<a->size; j++) {
-				b[a->coeff[j].index] -= a->coeff[j].value*variable->value;
+				b[a->coeff[j].index] -= a->coeff[j].value*variable->value[rhs];
 			}
 		}
 	}
 
-#if 0
-	if(!context->least_squares) {
-		__nl_assert(
-			context->current_row == 
-			context->n
-		);
-	}
-#endif
-}
-
-static void __nlBeginRow() {
-	__nlTransition(__NL_STATE_MATRIX, __NL_STATE_ROW);
-	__nlRowColumnZero(&__nlCurrentContext->af);
-	__nlRowColumnZero(&__nlCurrentContext->al);
+	if(context->least_squares)
+		__nlSparseMatrix_transpose_mult_rows(&context->M, b, Mtb);
 }
 
-static void __nlEndRow() {
-	__NLRowColumn*	af = &__nlCurrentContext->af;
-	__NLRowColumn*	al = &__nlCurrentContext->al;
-	__NLSparseMatrix* M  = &__nlCurrentContext->M;
-	NLfloat* b		= __nlCurrentContext->b;
-	NLuint nf		  = af->size;
-	NLuint nl		  = al->size;
-	NLuint current_row = __nlCurrentContext->current_row;
+static void __nlEndMatrix() {
+	__NLContext *context = __nlCurrentContext;
 	NLuint i;
-	NLuint j;
-	NLfloat S;
-	__nlTransition(__NL_STATE_ROW, __NL_STATE_MATRIX);
-
-	if(__nlCurrentContext->least_squares) {
-		if (!__nlCurrentContext->solve_again) {
-			for(i=0; i<nf; i++) {
-				for(j=0; j<nf; j++) {
-					__nlSparseMatrixAdd(
-						M, af->coeff[i].index, af->coeff[j].index,
-						af->coeff[i].value * af->coeff[j].value
-					);
-				}
-			}
-		}
-
-		S = -__nlCurrentContext->right_hand_side;
-		for(j=0; j<nl; j++)
-			S += al->coeff[j].value;
 
-		for(i=0; i<nf; i++)
-			b[ af->coeff[i].index ] -= af->coeff[i].value * S;
-	} else {
-		if (!__nlCurrentContext->solve_again) {
-			for(i=0; i<nf; i++) {
-				__nlSparseMatrixAdd(
-					M, current_row, af->coeff[i].index, af->coeff[i].value
-				);
-			}
-		}
-		b[current_row] = -__nlCurrentContext->right_hand_side;
-		for(i=0; i<nl; i++) {
-			b[current_row] -= al->coeff[i].value;
+	__nlTransition(__NL_STATE_MATRIX, __NL_STATE_MATRIX_CONSTRUCTED);	
+	
+	if(context->least_squares) {
+		if(!__nlCurrentContext->solve_again) {
+			__nlSparseMatrix_square(&context->MtM, &context->M);
+			context->alloc_MtM = NL_TRUE;
+
+			context->Mtb =
+				__NL_NEW_ARRAY(NLfloat, context->n*context->nb_rhs);
+			context->alloc_Mtb = NL_TRUE;
 		}
 	}
-	__nlCurrentContext->current_row++;
-	__nlCurrentContext->right_hand_side = 0.0;	
+
+	for(i=0; i<context->nb_rhs; i++)
+		__nlEndMatrixRHS(i);
 }
 
 void nlMatrixAdd(NLuint row, NLuint col, NLfloat value)
@@ -934,68 +932,70 @@ void nlMatrixAdd(NLuint row, NLuint col, NLfloat value)
 	__NLContext *context = __nlCurrentContext;
 
 	__nlCheckState(__NL_STATE_MATRIX);
-	__nl_assert(!context->least_squares);
 
-	if (context->variable[row].locked);
+	if(context->solve_again)
+		return;
+
+	if (!context->least_squares && context->variable[row].locked);
 	else if (context->variable[col].locked) {
-		row = context->variable[row].index;
+		if(!context->least_squares)
+			row = context->variable[row].index;
 		__nlRowColumnAppend(context->variable[col].a, row, value);
 	}
 	else {
 		__NLSparseMatrix* M  = &context->M;
-
-		row = context->variable[row].index;
+		
+		if(!context->least_squares)
+			row = context->variable[row].index;
 		col = context->variable[col].index;
 		
-		__nl_range_assert(row, 0, context->n - 1);
+		__nl_range_assert(row, 0, context->m - 1);
 		__nl_range_assert(col, 0, context->n - 1);
 
 		__nlSparseMatrixAdd(M, row, col, value);
 	}
 }
 
-void nlRightHandSideAdd(NLuint index, NLfloat value)
+void nlRightHandSideAdd(NLuint rhsindex, NLuint index, NLfloat value)
 {
 	__NLContext *context = __nlCurrentContext;
 	NLfloat* b = context->b;
 
 	__nlCheckState(__NL_STATE_MATRIX);
-	__nl_assert(!context->least_squares);
 
-	if(!context->variable[index].locked) {
-		index = context->variable[index].index;
-		__nl_range_assert(index, 0, context->n - 1);
+	if(context->least_squares) {
+		__nl_range_assert(index, 0, context->m - 1);
+		b[rhsindex*context->m + index] += value;
+	}
+	else {
+		if(!context->variable[index].locked) {
+			index = context->variable[index].index;
+			__nl_range_assert(index, 0, context->m - 1);
 
-		b[index] += value;
+			b[rhsindex*context->m + index] += value;
+		}
 	}
 }
 
-void nlRightHandSideSet(NLuint index, NLfloat value)
+void nlRightHandSideSet(NLuint rhsindex, NLuint index, NLfloat value)
 {
 	__NLContext *context = __nlCurrentContext;
 	NLfloat* b = context->b;
 
 	__nlCheckState(__NL_STATE_MATRIX);
-	__nl_assert(!context->least_squares);
 
-	if(!context->variable[index].locked) {
-		index = context->variable[index].index;
-		__nl_range_assert(index, 0, context->n - 1);
-
-		b[index] = value;
+	if(context->least_squares) {
+		__nl_range_assert(index, 0, context->m - 1);
+		b[rhsindex*context->m + index] = value;
 	}
-}
+	else {
+		if(!context->variable[index].locked) {
+			index = context->variable[index].index;
+			__nl_range_assert(index, 0, context->m - 1);
 
-void nlCoefficient(NLuint index, NLfloat value) {
-	__NLVariable* v;
-	unsigned int zero= 0;
-	__nlCheckState(__NL_STATE_ROW);
-	__nl_range_assert(index, zero, __nlCurrentContext->nb_variables - 1);
-	v = &(__nlCurrentContext->variable[index]);
-	if(v->locked)
-		__nlRowColumnAppend(&(__nlCurrentContext->al), 0, value*v->value);
-	else
-		__nlRowColumnAppend(&(__nlCurrentContext->af), v->index, value);
+			b[rhsindex*context->m + index] = value;
+		}
+	}
 }
 
 void nlBegin(NLenum prim) {
@@ -1006,9 +1006,6 @@ void nlBegin(NLenum prim) {
 	case NL_MATRIX: {
 		__nlBeginMatrix();
 	} break;
-	case NL_ROW: {
-		__nlBeginRow();
-	} break;
 	default: {
 		__nl_assert_not_reached;
 	}
@@ -1023,9 +1020,6 @@ void nlEnd(NLenum prim) {
 	case NL_MATRIX: {
 		__nlEndMatrix();
 	} break;
-	case NL_ROW: {
-		__nlEndRow();
-	} break;
 	default: {
 		__nl_assert_not_reached;
 	}
@@ -1040,7 +1034,7 @@ void nlEnd(NLenum prim) {
 static NLboolean __nlFactorize_SUPERLU(__NLContext *context, NLint *permutation) {
 
 	/* OpenNL Context */
-	__NLSparseMatrix* M = &(context->M);
+	__NLSparseMatrix* M = (context->least_squares)? &context->MtM: &context->M;
 	NLuint n = context->n;
 	NLuint nnz = __nlSparseMatrixNNZ(M); /* number of non-zero coeffs */
 
@@ -1057,7 +1051,6 @@ static NLboolean __nlFactorize_SUPERLU(__NLContext *context, NLint *permutation)
 	superlu_options_t options;
 
 	/* Temporary variables */
-	__NLRowColumn* Ri = NULL;
 	NLuint i, jj, count;
 	
 	__nl_assert(!(M->storage & __NL_SYMMETRIC));
@@ -1136,31 +1129,33 @@ static NLboolean __nlFactorize_SUPERLU(__NLContext *context, NLint *permutation)
 static NLboolean __nlInvert_SUPERLU(__NLContext *context) {
 
 	/* OpenNL Context */
-	NLfloat* b = context->b;
+	NLfloat* b = (context->least_squares)? context->Mtb: context->b;
 	NLfloat* x = context->x;
-	NLuint n = context->n;
+	NLuint n = context->n, j;
 
 	/* SuperLU variables */
 	SuperMatrix B;
 	NLint info;
 
-	/* Create superlu array for B */
-	sCreate_Dense_Matrix(
-		&B, n, 1, b, n, 
-		SLU_DN, /* Fortran-type column-wise storage */
-		SLU_S,  /* floats						  */
-		SLU_GE  /* general						  */
-	);
+	for(j=0; j<context->nb_rhs; j++, b+=n, x+=n) {
+		/* Create superlu array for B */
+		sCreate_Dense_Matrix(
+			&B, n, 1, b, n, 
+			SLU_DN, /* Fortran-type column-wise storage */
+			SLU_S,  /* floats						  */
+			SLU_GE  /* general						  */
+		);
 
-	/* Forward/Back substitution to compute x */
-	sgstrs(TRANS, &(context->slu.L), &(context->slu.U),
-		context->slu.perm_c, context->slu.perm_r, &B,
-		&(context->slu.stat), &info);
+		/* Forward/Back substitution to compute x */
+		sgstrs(TRANS, &(context->slu.L), &(context->slu.U),
+			context->slu.perm_c, context->slu.perm_r, &B,
+			&(context->slu.stat), &info);
 
-	if(info == 0)
-		memcpy(x, ((DNformat*)B.Store)->nzval, sizeof(*x)*n);
+		if(info == 0)
+			memcpy(x, ((DNformat*)B.Store)->nzval, sizeof(*x)*n);
 
-	Destroy_SuperMatrix_Store(&B);
+		Destroy_SuperMatrix_Store(&B);
+	}
 
 	return (info == 0);
 }
@@ -1179,15 +1174,18 @@ static void __nlFree_SUPERLU(__NLContext *context) {
 }
 
 void nlPrintMatrix(void) {
-	__NLSparseMatrix* M  = &(__nlCurrentContext->M);
-	float *b = __nlCurrentContext->b;
+	__NLContext *context = __nlCurrentContext;
+	__NLSparseMatrix* M  = &(context->M);
+	__NLSparseMatrix* MtM  = &(context->MtM);
+	float *b = context->b;
 	NLuint i, jj, k;
-	NLuint n = __nlCurrentContext->n;
+	NLuint m = context->m;
+	NLuint n = context->n;
 	__NLRowColumn* Ri = NULL;
-	float *value = malloc(sizeof(*value)*n);
+	float *value = malloc(sizeof(*value)*(n+m));
 
 	printf("A:\n");
-	for(i=0; i<n; i++) {
+	for(i=0; i<m; i++) {
 		Ri = &(M->row[i]);
 
 		memset(value, 0.0, sizeof(*value)*n);
@@ -1199,10 +1197,35 @@ void nlPrintMatrix(void) {
 		printf("\n");
 	}
 
-	printf("b:\n");
-	for(i=0; i<n; i++)
-		printf("%f ", b[i]);
-	printf("\n");
+	for(k=0; k<context->nb_rhs; k++) {
+		printf("b (%d):\n", k);
+		for(i=0; i<n; i++)
+			printf("%f ", b[context->n*k + i]);
+		printf("\n");
+	}
+
+	if(context->alloc_MtM) {
+		printf("AtA:\n");
+		for(i=0; i<n; i++) {
+			Ri = &(MtM->row[i]);
+
+			memset(value, 0.0, sizeof(*value)*m);
+			for(jj=0; jj<Ri->size; jj++)
+				value[Ri->coeff[jj].index] = Ri->coeff[jj].value;
+
+			for (k = 0; k<n; k++)
+				printf("%.3f ", value[k]);
+			printf("\n");
+		}
+
+		for(k=0; k<context->nb_rhs; k++) {
+			printf("Mtb (%d):\n", k);
+			for(i=0; i<n; i++)
+				printf("%f ", context->Mtb[context->n*k + i]);
+			printf("\n");
+		}
+		printf("\n");
+	}
 
 	free(value);
 }