/* x=A\b where A can be rectangular; b overwritten with solution */
int cs_qrsol (int order, const cs *A, double *b)
{
double *x ;
css *S ;
csn *N ;
cs *AT = NULL ;
int k, m, n, ok ;
if (!CS_CSC (A) || !b) return (0) ; /* check inputs */
n = A->n ;
m = A->m ;
if (m >= n)
{
S = cs_sqr (order, A, 1) ; /* ordering and symbolic analysis */
N = cs_qr (A, S) ; /* numeric QR factorization */
x = cs_calloc (S ? S->m2 : 1, sizeof (double)) ; /* get workspace */
ok = (S && N && x) ;
if (ok)
{
cs_ipvec (S->pinv, b, x, m) ; /* x(0:m-1) = b(p(0:m-1) */
for (k = 0 ; k < n ; k++) /* apply Householder refl. to x */
{
cs_happly (N->L, k, N->B [k], x) ;
}
cs_usolve (N->U, x) ; /* x = R\x */
cs_ipvec (S->q, x, b, n) ; /* b(q(0:n-1)) = x(0:n-1) */
}
}
else
{
AT = cs_transpose (A, 1) ; /* Ax=b is underdetermined */
S = cs_sqr (order, AT, 1) ; /* ordering and symbolic analysis */
N = cs_qr (AT, S) ; /* numeric QR factorization of A' */
x = cs_calloc (S ? S->m2 : 1, sizeof (double)) ; /* get workspace */
ok = (AT && S && N && x) ;
if (ok)
{
cs_pvec (S->q, b, x, m) ; /* x(q(0:m-1)) = b(0:m-1) */
cs_utsolve (N->U, x) ; /* x = R'\x */
for (k = m-1 ; k >= 0 ; k--) /* apply Householder refl. to x */
{
cs_happly (N->L, k, N->B [k], x) ;
}
cs_pvec (S->pinv, x, b, n) ; /* b(0:n-1) = x(p(0:n-1)) */
}
}
cs_free (x) ;
cs_sfree (S) ;
cs_nfree (N) ;
cs_spfree (AT) ;
return (ok) ;
}
returnValue ACADOcsparse::solveTranspose(double *b)
{
// CONSISTENCY CHECKS:
// -------------------
if (dim <= 0)
return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
if (nDense <= 0)
return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
if (S == 0)
return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
// CASE: LU
cs_ipvec(N->pinv, b, x, dim); /* x = b(p) */
cs_utsolve(N->U, x); /* x = U'\x */
cs_ltsolve(N->L, x); /* x = L'\x */
cs_ipvec(S->q, x, b, dim); /* b(q) = x */
return SUCCESSFUL_RETURN;
}
void CSparseInternal::solve(double* x, int nrhs, bool transpose){
casadi_assert(prepared_);
casadi_assert(N_!=0);
double *t = &temp_.front();
for(int k=0; k<nrhs; ++k){
if(transpose){
cs_ipvec (N_->pinv, x, t, AT_.n) ; // t = P1\b
cs_lsolve (N_->L, t) ; // t = L\t
cs_usolve (N_->U, t) ; // t = U\t
cs_ipvec (S_->q, t, x, AT_.n) ; // x = P2\t
} else {
cs_pvec (S_->q, x, t, AT_.n) ; // t = P2*b
casadi_assert(N_->U!=0);
cs_utsolve (N_->U, t) ; // t = U'\t
cs_ltsolve (N_->L, t) ; // t = L'\t
cs_pvec (N_->pinv, t, x, AT_.n) ; // x = P1*t
}
x += nrow();
}
}
void CsparseInterface::solve(double* x, int nrhs, bool transpose) {
double time_start=0;
if (CasadiOptions::profiling&& CasadiOptions::profilingBinary) {
time_start = getRealTime(); // Start timer
profileWriteEntry(CasadiOptions::profilingLog, this);
}
casadi_assert(prepared_);
casadi_assert(N_!=0);
double *t = &temp_.front();
for (int k=0; k<nrhs; ++k) {
if (transpose) {
cs_pvec(S_->q, x, t, A_.n) ; // t = P2*b
casadi_assert(N_->U!=0);
cs_utsolve(N_->U, t) ; // t = U'\t
cs_ltsolve(N_->L, t) ; // t = L'\t
cs_pvec(N_->pinv, t, x, A_.n) ; // x = P1*t
} else {
cs_ipvec(N_->pinv, x, t, A_.n) ; // t = P1\b
cs_lsolve(N_->L, t) ; // t = L\t
cs_usolve(N_->U, t) ; // t = U\t
cs_ipvec(S_->q, t, x, A_.n) ; // x = P2\t
}
x += ncol();
}
if (CasadiOptions::profiling && CasadiOptions::profilingBinary) {
double time_stop = getRealTime(); // Stop timer
profileWriteTime(CasadiOptions::profilingLog, this, 1,
time_stop-time_start,
time_stop-time_start);
profileWriteExit(CasadiOptions::profilingLog, this, time_stop-time_start);
}
}
