// Solve the stokes equation
// F -> U X P
void ProblemStructure::solveStokes() {
Map<VectorXd> stokesSolnVector (geometry.getStokesData(), M * (N - 1) + (M - 1) * N + M * N);
#ifndef USE_DENSE
static SparseMatrix<double> stokesMatrix (3 * M * N - M - N, 3 * M * N - M - N);
static SparseMatrix<double> forcingMatrix (3 * M * N - M - N, 2 * M * N - M - N);
static SparseMatrix<double> boundaryMatrix (3 * M * N - M - N, 2 * M + 2 * N);
static SparseLU<SparseMatrix<double>, COLAMDOrdering<int> > solver;
#else
/* Don't use this unless you hate your computer. */
static MatrixXd stokesMatrix (3 * M * N - M - N, 3 * M * N - M - N);
static MatrixXd forcingMatrix (3 * M * N - M - N, 2 * M * N - M - N);
static MatrixXd boundaryMatrix (3 * M * N - M - N, 2 * M + 2 * N);
static PartialPivLU<MatrixXd> solver;
#endif
static bool initialized;
double * viscosityData = geometry.getViscosityData();
if (!(initialized) || !(viscosityModel=="constant")) {
#ifndef USE_DENSE
SparseForms::makeStokesMatrix (stokesMatrix, M, N, h, viscosityData);
stokesMatrix.makeCompressed();
SparseForms::makeForcingMatrix (forcingMatrix, M, N);
forcingMatrix.makeCompressed();
SparseForms::makeBoundaryMatrix (boundaryMatrix, M, N, h, viscosityData);
boundaryMatrix.makeCompressed();
solver.analyzePattern (stokesMatrix);
solver.factorize (stokesMatrix);
#else
DenseForms::makeStokesMatrix (stokesMatrix, M, N, h, viscosityData);
DenseForms::makeForcingMatrix (forcingMatrix, M, N);
DenseForms::makeBoundaryMatrix (boundaryMatrix, M, N, h, viscosityData);
solver.compute (stokesMatrix);
#endif
initialized = true;
}
stokesSolnVector = solver.solve (forcingMatrix * Map<VectorXd>(geometry.getForcingData(), 2 * M * N - M - N) +
boundaryMatrix * Map<VectorXd>(geometry.getVelocityBoundaryData(), 2 * M + 2 * N));
Map<VectorXd> pressureVector (geometry.getPressureData(), M * N);
double pressureMean = pressureVector.sum() / (M * N);
pressureVector -= VectorXd::Constant (M * N, pressureMean);
#ifdef DEBUG
cout << "<Calculated Stokes Equation Solutions>" << endl;
cout << "<U Velocity Data>" << endl;
cout << DataWindow<double> (geometry.getUVelocityData(), N - 1, M).displayMatrix() << endl;
cout << "<V Velocity Data>" << endl;
cout << DataWindow<double> (geometry.getVVelocityData(), N, M - 1).displayMatrix() << endl;
cout << "<Pressure Data>" << endl;
cout << DataWindow<double> (geometry.getPressureData(), N, M).displayMatrix() << endl << endl;
#endif
}
void solve(int n, int m, int* rowptr, int* colidx,
int nn, int mm, Float* Avals, Float* bvals, Float* xvals) {
#ifdef EIGEN
auto A = csr2eigen<Float,ColMajor>(n, m, rowptr, colidx, nn, mm, Avals);
auto x = new Map<Matrix<Float,Dynamic,1>>(xvals, m);
auto b = new Map<Matrix<Float,Dynamic,1>>(bvals, n);
SparseLU<SparseMatrix<Float, ColMajor>> solver;
solver.compute(A);
*x = solver.solve(*b);
#else
SOLVER_ERROR;
#endif
}
void NRICP::solveLinearSystem()
{
//TO DO: This is the slow bit of the algorithm...understand matrices better to optimize!
//Important stuff down here
m_A->makeCompressed();
m_B->makeCompressed();
SparseLU <SparseMatrix<GLfloat, ColMajor>, COLAMDOrdering<int> > solver;
solver.compute((*m_A).transpose() * (*m_A));
SparseMatrix<GLfloat, ColMajor> I(4 * m_templateVertCount, 4 * m_templateVertCount);
I.setIdentity();
SparseMatrix<GLfloat, ColMajor> A_inv = solver.solve(I);
SparseMatrix<GLfloat, ColMajor> result = A_inv * (*m_A).transpose() * (*m_B);
result.uncompress();
(*m_X) = result;
}
bool WHeadPositionCorrection::computeInverseOperation( MatrixT* const g, const MatrixT& lf ) const
{
WLTimeProfiler profiler( CLASS, __func__, true );
const float snr = 25;
const MatrixT noiseCov = MatrixT::Identity( lf.rows(), lf.rows() );
// Leafield transpose matrix
const MatrixT LT = lf.transpose();
// WinvLT = W^-1 * LT
SpMatrixT w = SpMatrixT( lf.cols(), lf.cols() );
w.setIdentity();
w.makeCompressed();
SparseLU< SpMatrixT > spSolver;
spSolver.compute( w );
if( spSolver.info() != Eigen::Success )
{
wlog::error( CLASS ) << "spSolver.compute( weighting ) not succeeded: " << spSolver.info();
return false;
}
const MatrixT WinvLT = spSolver.solve( LT ); // needs dense matrix, returns dense matrix
if( spSolver.info() != Eigen::Success )
{
wlog::error( CLASS ) << "spSolver.solve( LT ) not succeeded: " << spSolver.info();
return false;
}
wlog::debug( CLASS ) << "WinvLT " << WinvLT.rows() << " x " << WinvLT.cols();
// LWL = L * W^-1 * LT
const MatrixT LWL = lf * WinvLT;
wlog::debug( CLASS ) << "LWL " << LWL.rows() << " x " << LWL.cols();
// alpha = sqrt(trace(LWL)/(snr * num_sensors));
double alpha = sqrt( LWL.trace() / ( snr * lf.rows() ) );
// G = W^-1 * LT * inv( (L W^-1 * LT) + alpha^2 * Cn )
const MatrixT toInv = LWL + pow( alpha, 2 ) * noiseCov;
const MatrixT inv = toInv.inverse();
*g = WinvLT * inv;
WAssertDebug( g->rows() == lf.cols() && g->cols() == lf.rows(), "Dimension of G and L does not match." );
return true;
}
