void Horzcat::evaluateMX(const MXPtrV& input, MXPtrV& output, const MXPtrVV& fwdSeed,
MXPtrVV& fwdSens, const MXPtrVV& adjSeed, MXPtrVV& adjSens,
bool output_given) {
int nfwd = fwdSens.size();
int nadj = adjSeed.size();
// Non-differentiated output
if (!output_given) {
*output[0] = horzcat(getVector(input));
}
// Forward sensitivities
for (int d = 0; d<nfwd; ++d) {
*fwdSens[d][0] = horzcat(getVector(fwdSeed[d]));
}
// Quick return?
if (nadj==0) return;
// Get offsets for each column
vector<int> col_offset(ndep()+1, 0);
for (int i=0; i<ndep(); ++i) {
int ncol = dep(i).sparsity().size2();
col_offset[i+1] = col_offset[i] + ncol;
}
// Adjoint sensitivities
for (int d=0; d<nadj; ++d) {
MX& aseed = *adjSeed[d][0];
vector<MX> s = horzsplit(aseed, col_offset);
aseed = MX();
for (int i=0; i<ndep(); ++i) {
adjSens[d][i]->addToSum(s[i]);
}
}
}
void ImplicitFunctionInternal::evaluateMX(
MXNode* node, const MXPtrV& arg, MXPtrV& res,
const MXPtrVV& fseed, MXPtrVV& fsens, const MXPtrVV& aseed, MXPtrVV& asens,
bool output_given) {
// Evaluate non-differentiated
vector<MX> argv = MXNode::getVector(arg);
MX z; // the solution to the system of equations
if (output_given) {
z = *res[iout_];
} else {
vector<MX> resv = callSelf(argv);
for (int i=0; i<resv.size(); ++i) {
if (res[i]!=0) *res[i] = resv[i];
}
z = resv[iout_];
}
// Quick return if no derivatives
int nfwd = fsens.size();
int nadj = aseed.size();
if (nfwd==0 && nadj==0) return;
// Temporaries
vector<int> col_offset(1, 0);
vector<MX> rhs;
vector<int> rhs_loc;
// Arguments when calling f/f_der
vector<MX> v;
v.reserve(getNumInputs()*(1+nfwd) + nadj);
v.insert(v.end(), argv.begin(), argv.end());
v[iin_] = z;
// Get an expression for the Jacobian
MX J = jac_.call(v).front();
// Directional derivatives of f
Function f_der = f_.derivative(nfwd, nadj);
// Forward sensitivities, collect arguments for calling f_der
for (int d=0; d<nfwd; ++d) {
argv = MXNode::getVector(fseed[d]);
argv[iin_] = MX::zeros(input(iin_).sparsity());
v.insert(v.end(), argv.begin(), argv.end());
}
// Adjoint sensitivities, solve to get arguments for calling f_der
if (nadj>0) {
for (int d=0; d<nadj; ++d) {
for (int i=0; i<getNumOutputs(); ++i) {
if (aseed[d][i]!=0) {
if (i==iout_) {
rhs.push_back(*aseed[d][i]);
col_offset.push_back(col_offset.back()+1);
rhs_loc.push_back(v.size()); // where to store it
v.push_back(MX());
} else {
v.push_back(*aseed[d][i]);
}
}
*aseed[d][i] = MX();
}
}
// Solve for all right-hand-sides at once
rhs = horzsplit(J->getSolve(horzcat(rhs), true, linsol_), col_offset);
for (int d=0; d<rhs.size(); ++d) {
v[rhs_loc[d]] = rhs[d];
}
col_offset.resize(1);
rhs.clear();
}
// Propagate through the implicit function
v = f_der.call(v);
vector<MX>::const_iterator v_it = v.begin();
// Discard non-differentiated evaluation (change?)
v_it += getNumOutputs();
// Forward directional derivatives
if (nfwd>0) {
for (int d=0; d<nfwd; ++d) {
for (int i=0; i<getNumOutputs(); ++i) {
if (i==iout_) {
// Collect the arguments
rhs.push_back(*v_it++);
col_offset.push_back(col_offset.back()+1);
} else {
// Auxiliary output
if (fsens[d][i]!=0) {
*fsens[d][i] = *v_it++;
}
}
}
}
// Solve for all the forward derivatives at once
rhs = horzsplit(J->getSolve(horzcat(rhs), false, linsol_), col_offset);
for (int d=0; d<nfwd; ++d) {
if (fsens[d][iout_]!=0) {
*fsens[d][iout_] = -rhs[d];
}
}
col_offset.resize(1);
rhs.clear();
}
// Collect adjoint sensitivities
for (int d=0; d<nadj; ++d) {
for (int i=0; i<asens[d].size(); ++i, ++v_it) {
if (i!=iin_ && asens[d][i]!=0 && !v_it->isNull()) {
*asens[d][i] += - *v_it;
}
}
}
casadi_assert(v_it==v.end());
}
