void LapackLuDense::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);
}
// Scale the right hand side
if (transpose) {
rowScaling(x, nrhs);
} else {
colScaling(x, nrhs);
}
// Solve the system of equations
int info = 100;
char trans = transpose ? 'T' : 'N';
dgetrs_(&trans, &ncol_, &nrhs, getPtr(mat_), &ncol_, getPtr(ipiv_), x, &ncol_, &info);
if (info != 0) throw CasadiException("LapackLuDense::solve: "
"failed to solve the linear system");
// Scale the solution
if (transpose) {
colScaling(x, nrhs);
} else {
rowScaling(x, nrhs);
}
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);
}
}
void LapackLuDense::prepare() {
double time_start=0;
if (CasadiOptions::profiling && CasadiOptions::profilingBinary) {
time_start = getRealTime(); // Start timer
profileWriteEntry(CasadiOptions::profilingLog, this);
}
prepared_ = false;
// Get the elements of the matrix, dense format
input(0).get(mat_);
if (equilibriate_) {
// Calculate the col and row scaling factors
double colcnd, rowcnd; // ratio of the smallest to the largest col/row scaling factor
double amax; // absolute value of the largest matrix element
int info = -100;
dgeequ_(&ncol_, &nrow_, getPtr(mat_), &ncol_, getPtr(r_),
getPtr(c_), &colcnd, &rowcnd, &amax, &info);
if (info < 0)
throw CasadiException("LapackQrDense::prepare: "
"dgeequ_ failed to calculate the scaling factors");
if (info>0) {
stringstream ss;
ss << "LapackLuDense::prepare: ";
if (info<=ncol_) ss << (info-1) << "-th row (zero-based) is exactly zero";
else ss << (info-1-ncol_) << "-th col (zero-based) is exactly zero";
userOut() << "Warning: " << ss.str() << endl;
if (allow_equilibration_failure_) userOut() << "Warning: " << ss.str() << endl;
else casadi_error(ss.str());
}
// Equilibrate the matrix if scaling was successful
if (info!=0)
dlaqge_(&ncol_, &nrow_, getPtr(mat_), &ncol_, getPtr(r_), getPtr(c_),
&colcnd, &rowcnd, &amax, &equed_);
else
equed_ = 'N';
}
// Factorize the matrix
int info = -100;
dgetrf_(&ncol_, &ncol_, getPtr(mat_), &ncol_, getPtr(ipiv_), &info);
if (info != 0) throw CasadiException("LapackLuDense::prepare: "
"dgetrf_ failed to factorize the Jacobian");
// Success if reached this point
prepared_ = true;
if (CasadiOptions::profiling && CasadiOptions::profilingBinary) {
double time_stop = getRealTime(); // Stop timer
profileWriteTime(CasadiOptions::profilingLog, this, 0, time_stop-time_start,
time_stop-time_start);
profileWriteExit(CasadiOptions::profilingLog, this, time_stop-time_start);
}
}
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);
}
}
void CsparseInterface::prepare() {
double time_start=0;
if (CasadiOptions::profiling && CasadiOptions::profilingBinary) {
time_start = getRealTime(); // Start timer
profileWriteEntry(CasadiOptions::profilingLog, this);
}
if (!called_once_) {
if (verbose()) {
cout << "CsparseInterface::prepare: symbolic factorization" << endl;
}
// ordering and symbolic analysis
int order = 0; // ordering?
if (S_) cs_sfree(S_);
S_ = cs_sqr(order, &A_, 0) ;
}
prepared_ = false;
called_once_ = true;
// Get a referebce to the nonzeros of the linear system
const vector<double>& linsys_nz = input().data();
// Make sure that all entries of the linear system are valid
for (int k=0; k<linsys_nz.size(); ++k) {
casadi_assert_message(!isnan(linsys_nz[k]), "Nonzero " << k << " is not-a-number");
casadi_assert_message(!isinf(linsys_nz[k]), "Nonzero " << k << " is infinite");
}
if (verbose()) {
cout << "CsparseInterface::prepare: numeric factorization" << endl;
cout << "linear system to be factorized = " << endl;
input(0).printSparse();
}
double tol = 1e-8;
if (N_) cs_nfree(N_);
N_ = cs_lu(&A_, S_, tol) ; // numeric LU factorization
if (N_==0) {
DMatrix temp = input();
temp.makeSparse();
if (temp.sparsity().isSingular()) {
stringstream ss;
ss << "CsparseInterface::prepare: factorization failed due to matrix"
" being singular. Matrix contains numerical zeros which are "
"structurally non-zero. Promoting these zeros to be structural "
"zeros, the matrix was found to be structurally rank deficient."
" sprank: " << sprank(temp.sparsity()) << " <-> " << temp.size2() << endl;
if (verbose()) {
ss << "Sparsity of the linear system: " << endl;
input(LINSOL_A).sparsity().print(ss); // print detailed
}
throw CasadiException(ss.str());
} else {
stringstream ss;
ss << "CsparseInterface::prepare: factorization failed, check if Jacobian is singular"
<< endl;
if (verbose()) {
ss << "Sparsity of the linear system: " << endl;
input(LINSOL_A).sparsity().print(ss); // print detailed
}
throw CasadiException(ss.str());
}
}
casadi_assert(N_!=0);
prepared_ = true;
if (CasadiOptions::profiling && CasadiOptions::profilingBinary) {
double time_stop = getRealTime(); // Stop timer
profileWriteTime(CasadiOptions::profilingLog, this, 0,
time_stop-time_start,
time_stop-time_start);
profileWriteExit(CasadiOptions::profilingLog, this, time_stop-time_start);
}
}
void SXFunctionInternal::evalD(const double** arg, double** res,
int* iw, double* w) {
double time_start=0;
double time_stop=0;
if (CasadiOptions::profiling) {
time_start = getRealTime();
if (CasadiOptions::profilingBinary) {
profileWriteEntry(CasadiOptions::profilingLog, this);
} else {
CasadiOptions::profilingLog << "start " << this << ":" <<getOption("name") << std::endl;
}
}
casadi_msg("SXFunctionInternal::evaluate():begin " << getOption("name"));
// NOTE: The implementation of this function is very delicate. Small changes in the
// class structure can cause large performance losses. For this reason,
// the preprocessor macros are used below
if (!free_vars_.empty()) {
std::stringstream ss;
repr(ss);
casadi_error("Cannot evaluate \"" << ss.str() << "\" since variables "
<< free_vars_ << " are free.");
}
#ifdef WITH_OPENCL
if (just_in_time_opencl_) {
// Evaluate with OpenCL
return evaluateOpenCL();
}
#endif // WITH_OPENCL
// Evaluate the algorithm
for (vector<AlgEl>::iterator it=algorithm_.begin(); it!=algorithm_.end(); ++it) {
switch (it->op) {
CASADI_MATH_FUN_BUILTIN(w[it->i1], w[it->i2], w[it->i0])
case OP_CONST: w[it->i0] = it->d; break;
case OP_INPUT: w[it->i0] = arg[it->i1]==0 ? 0 : arg[it->i1][it->i2]; break;
case OP_OUTPUT: if (res[it->i0]!=0) res[it->i0][it->i2] = w[it->i1]; break;
default:
casadi_error("SXFunctionInternal::evalD: Unknown operation" << it->op);
}
}
casadi_msg("SXFunctionInternal::evalD():end " << getOption("name"));
if (CasadiOptions::profiling) {
time_stop = getRealTime();
if (CasadiOptions::profilingBinary) {
profileWriteExit(CasadiOptions::profilingLog, this, time_stop-time_start);
} else {
CasadiOptions::profilingLog
<< (time_stop-time_start)*1e6 << " ns | "
<< (time_stop-time_start)*1e3 << " ms | "
<< this << ":" <<getOption("name")
<< ":0||SX algorithm size: " << algorithm_.size()
<< std::endl;
}
}
}
