void BaseSpline::solveInPlace(PackedMatrix& mat, PackedVector& res) {
res = res.makeSparse();
// Current implementation requires a quadratic mat
int nCol = mat.numCols();
PackedVector nonEmpty;
int col, row, rowPos;
for (col = 0; col < nCol; col++) {
//int stop = 1;
//if(col==stop-1){
// cout << "*********************"<<endl;
// cout << col << endl;
// cout << "*********************"<<endl;
// mat.displayMatrix();
// cout << "RES: " <<res.values<<endl;
//}
// find the non-null elements in this column (below row "col")
nonEmpty = PackedVector();
int pivotPos(-1);
for (row = col; row < mat.numRows(); row++) {
int rowEntries = mat[row].numberEntries();
for (rowPos = rowEntries; rowPos--;) {
int entryIndex = mat[row].index(rowPos);
if (entryIndex == col) {
nonEmpty.packedAddElement(row, mat[row][rowPos]);
if (row == col) {
pivotPos = nonEmpty.numberEntries()-1;
}
}
}
}
// if(col==stop-1){
// cout<<"nonEmpty " << nonEmpty.values<< endl;
// cout<<"pivot " << pivotPos<< endl<<endl;
// }
//find most significant row
double maxVal(0.0);
int maxRow(-1), maxRowPos(-1);
for (rowPos = nonEmpty.numberEntries(); rowPos--;) {
double val = nonEmpty[rowPos];
assert(isfinite(val));
if (fabs(val) > fabs(maxVal)) {
maxVal = val;
maxRow = nonEmpty.index(rowPos);
maxRowPos = rowPos;
}
}
//int imaxRow = maxRow;
//int imaxRowPos = maxRowPos;
// if(col==stop-1){
// cout << "imaxRow " << imaxRow << endl;
// cout << "imaxRowPos " << imaxRowPos << endl<<endl;
// }
// Put the most significant row at row "col"
if (maxVal != 0.0) {
if (maxRow != col) {
swap(mat[col], mat[maxRow]);
res.swapElements(col, maxRow);
if (pivotPos >= 0) {
nonEmpty.swapElements(maxRowPos, pivotPos);
}
}
// if(col==stop-1){
// cout << "SWAP\n";
// mat.displayMatrix();
// }
// Divide the row with maxVal
mat[col].packedDiv(maxVal);
double value = res[col] / maxVal;
res.packedReplace(col,value);
// if(col==stop-1){
// cout << "\nDIVIDE ROW\n";
// mat.displayMatrix();
// cout << "res " << res.values<<endl;
// cout << "nonEmpty " << nonEmpty.values<<endl;
// cout << "\n\n";
// }
}
// subtract the row from other rows
for (rowPos = nonEmpty.numberEntries(); rowPos--;) {
row = nonEmpty.index(rowPos);
if (row == col) {
continue;
}
// If the pivotRow was empty (prior to swap) at col=row do not process this row
if (pivotPos < 0 && row == maxRow) {
continue;
}
double val = nonEmpty[rowPos];
PackedVector prodVector = mat[col];
mat[row] = mat[row].packedSubtract(prodVector.packedProd(val));
double value = res[row] - (val * res[col]);
res.packedReplace(row, value);
// if(col==stop-1){
// cout << "SUBTRACT ROW\n";
// mat.displayMatrix();
// cout << "res " << res.values<<endl;
// cout << "nonEmpty " << nonEmpty.values<<endl;
// cout << "\n\n";
// }
}
}
// Go bottom up and clear upper halfmatrix
// mat.displayMatrix();
// res.displayVector();
// nonEmpty.displayVector();
// cout << "\n\n\n\n";
for (col = mat.numCols(); col--;) {
nonEmpty = PackedVector();
for (row = 0; row < col; row++) {
for (rowPos = mat[row].numberEntries(); rowPos--;) {
if (mat[row].index(rowPos) == col) {
nonEmpty.packedAddElement(row, mat[row][rowPos]);
}
}
}
// cout << nonEmpty.values<<endl;
// subtract the row from other rows
for (rowPos = nonEmpty.numberEntries(); rowPos--;) {
row = nonEmpty.index(rowPos);
double val = nonEmpty[rowPos];
mat[row] = mat[row].packedSubtract(mat[col].packedProd(val));
double value = res[row] - (val * res[col]);
res.packedReplace(row, value);
}
// cout << res.values<<endl;
}
return;
}
