void nlMatrixAdd(NLuint row, NLuint col, NLfloat value)
{
__NLContext *context = __nlCurrentContext;
__nlCheckState(__NL_STATE_MATRIX);
if(context->solve_again)
return;
if (!context->least_squares && context->variable[row].locked);
else if (context->variable[col].locked) {
if(!context->least_squares)
row = context->variable[row].index;
__nlRowColumnAppend(context->variable[col].a, row, value);
}
else {
__NLSparseMatrix* M = &context->M;
if(!context->least_squares)
row = context->variable[row].index;
col = context->variable[col].index;
__nl_range_assert(row, 0, context->m - 1);
__nl_range_assert(col, 0, context->n - 1);
__nlSparseMatrixAdd(M, row, col, value);
}
}
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 __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;
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;
}
}
__nlCurrentContext->current_row++;
__nlCurrentContext->right_hand_side = 0.0;
}
void nlMatrixAdd(NLuint row, NLuint col, NLfloat value)
{
__NLSparseMatrix* M = &__nlCurrentContext->M;
__nlCheckState(__NL_STATE_MATRIX);
__nl_range_assert((float)row, 0, (float)(__nlCurrentContext->n - 1));
__nl_range_assert((float)col, 0, (float)(__nlCurrentContext->nb_variables - 1));
__nl_assert(!__nlCurrentContext->least_squares);
__nlSparseMatrixAdd(M, row, col, value);
}
