void casadi_bfgs(const casadi_int* sp_h, T1* h, const T1* dx,
const T1* glag, const T1* glag_old, T1* w) {
// Local variables
casadi_int nx;
T1 *yk, *qk, dxBkdx, omega, theta, phi;
// Dimension
nx = sp_h[0];
// Work vectors
yk = w; w += nx;
qk = w; w += nx;
// yk = glag - glag_old
casadi_copy(glag, nx, yk);
casadi_axpy(nx, -1., glag_old, yk);
// qk = H*dx
casadi_fill(qk, nx, 0.);
casadi_mv(h, sp_h, dx, qk, 0);
// Calculating theta
dxBkdx = casadi_dot(nx, dx, qk);
// C-REPLACE "if_else" "casadi_if_else"
omega = if_else(casadi_dot(nx, yk, dx) < 0.2 * casadi_dot(nx, dx, qk),
0.8 * dxBkdx / (dxBkdx - casadi_dot(nx, dx, yk)), 1);
// yk = omega * yk + (1 - omega) * qk;
casadi_scal(nx, omega, yk);
casadi_axpy(nx, 1 - omega, qk, yk);
theta = 1. / casadi_dot(nx, dx, yk);
phi = 1. / casadi_dot(nx, qk, dx);
// Update H
casadi_rank1(h, sp_h, theta, yk, yk);
casadi_rank1(h, sp_h, -phi, qk, qk);
}
int IdasInterface::jtimesB(double t, N_Vector xz, N_Vector xzdot, N_Vector xzB,
N_Vector xzdotB, N_Vector resvalB, N_Vector vB, N_Vector JvB,
double cjB, void *user_data,
N_Vector tmp1B, N_Vector tmp2B) {
try {
auto m = to_mem(user_data);
auto& s = m->self;
m->arg[0] = &t;
m->arg[1] = NV_DATA_S(xz);
m->arg[2] = NV_DATA_S(xz)+s.nx_;
m->arg[3] = m->p;
m->arg[4] = NV_DATA_S(xzB);
m->arg[5] = NV_DATA_S(xzB)+s.nrx_;
m->arg[6] = m->rp;
m->arg[7] = NV_DATA_S(vB);
m->arg[8] = NV_DATA_S(vB)+s.nrx_;
m->res[0] = NV_DATA_S(JvB);
m->res[1] = NV_DATA_S(JvB) + s.nrx_;
s.calc_function(m, "jtimesB");
// Subtract state derivative to get residual
casadi_axpy(s.nrx_, cjB, NV_DATA_S(vB), NV_DATA_S(JvB));
return 0;
} catch(int flag) { // recoverable error
return flag;
} catch(exception& e) { // non-recoverable error
userOut<true, PL_WARN>() << "jtimesB failed: " << e.what() << endl;
return -1;
}
}
int IdasInterface::resB(double t, N_Vector xz, N_Vector xzdot, N_Vector rxz,
N_Vector rxzdot, N_Vector rr, void *user_data) {
try {
auto m = to_mem(user_data);
auto& s = m->self;
m->arg[0] = NV_DATA_S(rxz);
m->arg[1] = NV_DATA_S(rxz)+s.nrx_;
m->arg[2] = m->rp;
m->arg[3] = NV_DATA_S(xz);
m->arg[4] = NV_DATA_S(xz)+s.nx_;
m->arg[5] = m->p;
m->arg[6] = &t;
m->res[0] = NV_DATA_S(rr);
m->res[1] = NV_DATA_S(rr)+s.nrx_;
s.calc_function(m, "daeB");
// Subtract state derivative to get residual
casadi_axpy(s.nrx_, 1., NV_DATA_S(rxzdot), NV_DATA_S(rr));
return 0;
} catch(int flag) { // recoverable error
return flag;
} catch(exception& e) { // non-recoverable error
userOut<true, PL_WARN>() << "resB failed: " << e.what() << endl;
return -1;
}
}
int IdasInterface::psolveB(double t, N_Vector xz, N_Vector xzdot, N_Vector xzB,
N_Vector xzdotB, N_Vector resvalB, N_Vector rvecB,
N_Vector zvecB, double cjB, double deltaB,
void *user_data, N_Vector tmpB) {
try {
auto m = to_mem(user_data);
auto& s = m->self;
// Get right-hand sides in m->v1, ordered by sensitivity directions
double* vx = NV_DATA_S(rvecB);
double* vz = vx + s.nrx_;
double* v_it = m->v1;
for (int d=0; d<=s.ns_; ++d) {
casadi_copy(vx + d*s.nrx1_, s.nrx1_, v_it);
v_it += s.nrx1_;
casadi_copy(vz + d*s.nrz1_, s.nrz1_, v_it);
v_it += s.nrz1_;
}
// Solve for undifferentiated right-hand-side, save to output
s.linsolB_.solve(m->v1, 1);
vx = NV_DATA_S(zvecB); // possibly different from rvecB
vz = vx + s.nrx_;
casadi_copy(m->v1, s.nrx1_, vx);
casadi_copy(m->v1 + s.nrx1_, s.nrz1_, vz);
// Sensitivity equations
if (s.ns_>0) {
// Second order correction
if (s.second_order_correction_) {
// The outputs will double as seeds for jtimesB
casadi_fill(vx + s.nrx1_, s.nrx_ - s.nrx1_, 0.);
casadi_fill(vz + s.nrz1_, s.nrz_ - s.nrz1_, 0.);
// Get second-order-correction, save to m->v2
m->arg[0] = &t; // t
m->arg[1] = NV_DATA_S(xz); // x
m->arg[2] = NV_DATA_S(xz)+s.nx_; // z
m->arg[3] = m->p; // p
m->arg[4] = NV_DATA_S(xzB); // rx
m->arg[5] = NV_DATA_S(xzB)+s.nrx_; // rz
m->arg[6] = m->rp; // rp
m->arg[7] = vx; // fwd:rx
m->arg[8] = vz; // fwd:rz
m->res[0] = m->v2; // fwd:rode
m->res[1] = m->v2 + s.nrx_; // fwd:ralg
s.calc_function(m, "jtimesB");
// Subtract m->v2 (reordered) from m->v1
v_it = m->v1 + s.nrx1_ + s.nrz1_;
for (int d=1; d<=s.ns_; ++d) {
casadi_axpy(s.nrx1_, -1., m->v2 + d*s.nrx1_, v_it);
v_it += s.nrx1_;
casadi_axpy(s.nrz1_, -1., m->v2 + s.nrx_ + d*s.nrz1_, v_it);
v_it += s.nrz1_;
}
}
// Solve for sensitivity right-hand-sides
s.linsolB_.solve(m->v1 + s.nrx1_ + s.nrz1_, s.ns_);
// Save to output, reordered
v_it = m->v1 + s.nrx1_ + s.nrz1_;
for (int d=1; d<=s.ns_; ++d) {
casadi_copy(v_it, s.nrx1_, vx + d*s.nrx1_);
v_it += s.nrx1_;
casadi_copy(v_it, s.nrz1_, vz + d*s.nrz1_);
v_it += s.nrz1_;
}
}
return 0;
} catch(int flag) { // recoverable error
return flag;
} catch(exception& e) { // non-recoverable error
userOut<true, PL_WARN>() << "psolveB failed: " << e.what() << endl;
return -1;
}
}
void Newton::solve(void* mem) const {
auto m = static_cast<NewtonMemory*>(mem);
// Get the initial guess
casadi_copy(m->iarg[iin_], n_, m->x);
// Perform the Newton iterations
m->iter=0;
bool success = true;
while (true) {
// Break if maximum number of iterations already reached
if (m->iter >= max_iter_) {
log("eval", "Max. iterations reached.");
m->return_status = "max_iteration_reached";
success = false;
break;
}
// Start a new iteration
m->iter++;
// Use x to evaluate J
copy_n(m->iarg, n_in(), m->arg);
m->arg[iin_] = m->x;
m->res[0] = m->jac;
copy_n(m->ires, n_out(), m->res+1);
m->res[1+iout_] = m->f;
calc_function(m, "jac_f_z");
// Check convergence
double abstol = 0;
if (abstol_ != numeric_limits<double>::infinity()) {
for (int i=0; i<n_; ++i) {
abstol = max(abstol, fabs(m->f[i]));
}
if (abstol <= abstol_) {
casadi_msg("Converged to acceptable tolerance - abstol: " << abstol_);
break;
}
}
// Factorize the linear solver with J
linsol_.factorize(m->jac);
linsol_.solve(m->f, 1, false);
// Check convergence again
double abstolStep=0;
if (numeric_limits<double>::infinity() != abstolStep_) {
for (int i=0; i<n_; ++i) {
abstolStep = max(abstolStep, fabs(m->f[i]));
}
if (abstolStep <= abstolStep_) {
casadi_msg("Converged to acceptable tolerance - abstolStep: " << abstolStep_);
break;
}
}
if (print_iteration_) {
// Only print iteration header once in a while
if (m->iter % 10==0) {
printIteration(userOut());
}
// Print iteration information
printIteration(userOut(), m->iter, abstol, abstolStep);
}
// Update Xk+1 = Xk - J^(-1) F
casadi_axpy(n_, -1., m->f, m->x);
}
// Get the solution
casadi_copy(m->x, n_, m->ires[iout_]);
// Store the iteration count
if (success) m->return_status = "success";
casadi_msg("Newton::solveNonLinear():end after " << m->iter << " steps");
}
