void gauss_seidel(const IntervalMatrix& A, const IntervalVector& b, IntervalVector& x, double ratio) {
int n=(A.nb_rows());
assert(n == (A.nb_cols())); // throw NotSquareMatrixException();
assert(n == (x.size()) && n == (b.size()));
double red;
Interval old, proj, tmp;
do {
red = 0;
for (int i=0; i<n; i++) {
old = x[i];
proj = b[i];
for (int j=0; j<n; j++) if (j!=i) proj -= A[i][j]*x[j];
tmp=A[i][i];
bwd_mul(proj,tmp,x[i]);
if (x[i].is_empty()) { x.set_empty(); return; }
double gain=old.rel_distance(x[i]);
if (gain>red) red=gain;
}
} while (red >= ratio);
}
bool inflating_gauss_seidel(const IntervalMatrix& A, const IntervalVector& b, IntervalVector& x, double min_dist, double mu_max) {
int n=(A.nb_rows());
assert(n == (A.nb_cols()));
assert(n == (x.size()) && n == (b.size()));
assert(min_dist>0);
//cout << " ====== inflating Gauss-Seidel ========= " << endl;
double red;
IntervalVector xold(n);
Interval proj;
double d=DBL_MAX; // Hausdorff distances between 2 iterations
double dold;
double mu; // ratio of dist(x_k,x_{k-1)) / dist(x_{k-1},x_{k-2}).
do {
dold = d;
xold = x;
for (int i=0; i<n; i++) {
proj = b[i];
for (int j=0; j<n; j++) if (j!=i) proj -= A[i][j]*x[j];
x[i] = proj/A[i][i];
}
d=distance(xold,x);
mu=d/dold;
//cout << " x=" << x << " d=" << d << " mu=" << mu << endl;
} while (mu<mu_max && d>min_dist);
//cout << " ======================================= " << endl;
return (mu<mu_max);
}
void Function::hansen_matrix(const IntervalVector& box, IntervalMatrix& H) const {
int n=nb_var();
int m=expr().dim.vec_size();
assert(H.nb_cols()==n);
assert(box.size()==n);
assert(expr().dim.is_vector());
assert(H.nb_rows()==m);
IntervalVector x=box.mid();
IntervalMatrix J(m,n);
// test!
// int tab[box.size()];
// box.sort_indices(false,tab);
// int var;
for (int var=0; var<n; var++) {
//var=tab[i];
x[var]=box[var];
jacobian(x,J);
H.set_col(var,J.col(var));
}
}
void Function::jacobian(const IntervalVector& x, IntervalMatrix& J) const {
assert(J.nb_cols()==nb_var());
assert(x.size()==nb_var());
assert(J.nb_rows()==image_dim());
// calculate the gradient of each component of f
for (int i=0; i<image_dim(); i++) {
(*this)[i].gradient(x,J[i]);
}
}
bool proj_mul(const IntervalMatrix& y, Interval& x1, IntervalMatrix& x2) {
int n=(y.nb_rows());
assert((x2.nb_rows())==n && (x2.nb_cols())==(y.nb_cols()));
for (int i=0; i<n; i++) {
if (!proj_mul(y[i],x1,x2[i])) {
x2.set_empty();
return false;
}
}
return true;
}
void Function::jacobian(const IntervalVector& box, IntervalMatrix& J, const VarSet& set) const {
assert(J.nb_cols()==set.nb_var);
assert(box.size()==nb_var());
assert(J.nb_rows()==image_dim());
IntervalVector g(nb_var());
// calculate the gradient of each component of f
for (int i=0; i<image_dim(); i++) {
(*this)[i].gradient(box,g);
J.set_row(i,set.var_box(g));
}
}
void hansen_bliek(const IntervalMatrix& A, const IntervalVector& B, IntervalVector& x) {
int n=A.nb_rows();
assert(n == A.nb_cols()); // throw NotSquareMatrixException();
assert(n == x.size() && n == B.size());
Matrix Id(n,n);
for (int i=0; i<n; i++)
for (int j=0; j<n; j++) {
Id[i][j] = i==j;
}
IntervalMatrix radA(A-Id);
Matrix InfA(Id-abs(radA.lb()));
Matrix M(n,n);
real_inverse(InfA, M); // may throw SingularMatrixException...
for (int i=0; i<n; i++)
for (int j=0; j<n; j++)
if (! (M[i][j]>=0.0)) throw NotInversePositiveMatrixException();
Vector b(B.mid());
Vector delta = (B.ub())-b;
Vector xstar = M * (abs(b)+delta);
double xtildek, xutildek, nuk, max, min;
for (int k=0; k<n; k++) {
xtildek = (b[k]>=0) ? xstar[k] : xstar[k] + 2*M[k][k]*b[k];
xutildek = (b[k]<=0) ? -xstar[k] : -xstar[k] + 2*M[k][k]*b[k];
nuk = 1/(2*M[k][k]-1);
max = nuk*xutildek;
if (max < 0) max = 0;
min = nuk*xtildek;
if (min > 0) min = 0;
/* compute bounds of x(k) */
if (xtildek >= max) {
if (xutildek <= min) x[k] = Interval(xutildek,xtildek);
else x[k] = Interval(max,xtildek);
} else {
if (xutildek <= min) x[k] = Interval(xutildek,min);
else { x.set_empty(); return; }
}
}
}
void precond(IntervalMatrix& A) {
int n=(A.nb_rows());
assert(n == A.nb_cols()); //throw NotSquareMatrixException(); // not well-constraint problem
Matrix C(n,n);
try { real_inverse(A.mid(), C); }
catch (SingularMatrixException&) {
try { real_inverse(A.lb(), C); }
catch (SingularMatrixException&) {
real_inverse(A.ub(), C);
}
}
A = C*A;
}
void Gradient::jacobian(const Array<Domain>& d, IntervalMatrix& J) {
if (!f.expr().dim.is_vector()) {
ibex_error("Cannot called \"jacobian\" on a real-valued function");
}
int m=f.expr().dim.vec_size();
// calculate the gradient of each component of f
for (int i=0; i<m; i++) {
const Function* fi=dynamic_cast<const Function*>(&f[i]);
if (fi!=NULL) {
// if this is a Function object we can
// directly calculate the gradient with d
fi->deriv_calculator().gradient(d,J[i]);
} else {
// otherwise we must give a box in argument
// TODO add gradient with Array<Domain> in argument
// in Function interface?
// But, for the moment, cannot happen, because
// this function is called by apply_bwd.
IntervalVector box(f.nb_var());
load(box,d);
f[i].gradient(box,J[i]);
if (J[i].is_empty()) { J.set_empty(); return; }
}
}
}
IntervalMatrix::IntervalMatrix(const IntervalMatrix& m) : _nb_rows(m.nb_rows()), _nb_cols(m.nb_cols()){
M = new IntervalVector[_nb_rows];
for (int i=0; i<_nb_rows; i++) {
M[i].resize(_nb_cols);
for (int j=0; j<_nb_cols; j++) M[i].vec[j]=m[i][j];
}
}
bool proj_mul(const IntervalVector& y, IntervalVector& x1, IntervalMatrix& x2, double ratio) {
IntervalMatrix x2t=x2.transpose();
bool res=proj_mul(y,x2t,x1,ratio);
x2=x2t.transpose();
return res;
}
bool proj_mul(const IntervalMatrix& y, IntervalMatrix& x1, IntervalMatrix& x2, double ratio) {
int m=y.nb_rows();
int n=y.nb_cols();
assert(x1.nb_cols()==x2.nb_rows());
assert(x1.nb_rows()==m);
assert(x2.nb_cols()==n);
// each coefficient (i,j) of y is considered as a binary "dot product" constraint
// between the ith row of x1 and the jth column of x2
// (advantage: we have exact projection for the dot product)
//
// we propagate these constraints using a simple agenda.
Agenda a(m*n);
//init
for (int i=0; i<m; i++)
for (int j=0; j<n; j++)
a.push(i*n+j);
int k;
while (!a.empty()) {
a.pop(k);
int i=k/n;
int j=k%n;
IntervalVector x1old=x1[i];
IntervalVector x2j=x2.col(j);
IntervalVector x2old=x2j;
if (!proj_mul(y[i][j],x1[i],x2j)) {
x1.set_empty();
x2.set_empty();
return false;
} else {
if (x1old.rel_distance(x1[i])>=ratio) {
for (int j2=0; j2<n; j2++)
if (j2!=j) a.push(i*n+j2);
}
if (x2old.rel_distance(x2j)>=ratio) {
for (int i2=0; i2<m; i2++)
if (i2!=i) a.push(i2*n+j);
}
x2.set_col(j,x2j);
}
}
return true;
}
bool proj_mul(const IntervalVector& y, IntervalMatrix& x1, IntervalVector& x2, double ratio) {
assert(x1.nb_rows()==y.size());
assert(x1.nb_cols()==x2.size());
int last_row=0;
int i=0;
int n=y.size();
do {
IntervalVector x2old=x2;
if (!proj_mul(y[i],x1[i],x2)) {
x1.set_empty();
return false;
}
if (x2old.rel_distance(x2)>ratio) last_row=i;
i=(i+1)%n;
} while(i!=last_row);
return true;
}
void precond(IntervalMatrix& A, IntervalVector& b) {
int n=(A.nb_rows());
assert(n == A.nb_cols()); //throw NotSquareMatrixException(); // not well-constraint problem
assert(n == b.size());
Matrix C(n,n);
try { real_inverse(A.mid(), C); }
catch (SingularMatrixException&) {
try { real_inverse(A.lb(), C); }
catch (SingularMatrixException&) {
real_inverse(A.ub(), C);
}
}
// cout << "A=" << (A.nb_cols()) << "x" << (A.nb_rows()) << " " << "b=" << (b.size()) << " " << "C="
// << (C.nb_cols()) << "x" << (C.nb_rows()) << endl;
//cout << "C=" << C << endl;
A = C*A;
b = C*b;
}
void Function::hansen_matrix(const IntervalVector& box, IntervalMatrix& H, const VarSet& set) const {
int n=set.nb_var;
int m=image_dim();
assert(H.nb_cols()==n);
assert(box.size()==nb_var());
assert(H.nb_rows()==m);
IntervalVector var_box=set.var_box(box);
IntervalVector param_box=set.param_box(box);
IntervalVector x=var_box.mid();
IntervalMatrix J(m,n);
for (int var=0; var<n; var++) {
//var=tab[i];
x[var]=var_box[var];
jacobian(set.full_box(x,param_box),J,set);
H.set_col(var,J.col(var));
}
}
CtcFwdBwd::CtcFwdBwd(Function& f, const IntervalMatrix& y, FwdMode mode) : Ctc(f.nb_var()), f(f), d(f.expr().dim), hc4r(mode) {
assert(f.expr().dim==Dim::matrix(y.nb_rows(),y.nb_cols()));
d.m() = y;
init();
}
void FncKhunTucker::jacobian(const IntervalVector& x_lambda, IntervalMatrix& J, const BitSet& components, int v) const {
if (components.size()!=n+nb_mult) {
not_implemented("FncKhunTucker: 'jacobian' for selected components");
//J.resize(n+nb_mult,n+nb_mult);
}
IntervalVector x=x_lambda.subvector(0,n-1);
int lambda0=n; // The index of lambda0 in the box x_lambda is nb_var.
int l=lambda0; // mutipliers indices counter. The first multiplier is lambda0.
// matrix corresponding to the "Hessian expression" lambda_0*d^2f+lambda_1*d^2g_1+...=0
IntervalMatrix hessian=x_lambda[l] * df.jacobian(x,v<n? v : -1); // init
if (v==-1 || v==l) J.put(0, l, df.eval_vector(x), false);
l++;
IntervalVector gx;
if (!active.empty())
gx = sys.f_ctrs.eval_vector(x,active);
// normalization equation (init)
J[lambda0].put(0,Vector::zeros(n));
J[lambda0][lambda0]=1.0;
IntervalVector dgi(n); // store dg_i([x]) (used in several places)
for (BitSet::const_iterator i=ineq.begin(); i!=ineq.end(); ++i) {
hessian += x_lambda[l] * dg[i].jacobian(x,v<n? v : -1);
dgi=dg[i].eval_vector(x);
J.put(0, l, dgi, false);
J.put(l, 0, (x_lambda[l]*dgi), true);
J.put(l, n, Vector::zeros(nb_mult), true);
J[l][l]=gx[i];
J[lambda0][l] = 1.0;
l++;
}
for (BitSet::const_iterator i=ineq.begin(); i!=ineq.end(); ++i) {
hessian += x_lambda[l] * dg[i].jacobian(x,v<n? v : -1);
dgi=dg[i].eval_vector(x);
J.put(0, l, dgi, false);
J.put(l, 0, dgi, true);
J.put(l, n, Vector::zeros(nb_mult), true);
J[lambda0][l] = 2*x_lambda[l];
l++;
}
for (BitSet::const_iterator v=bound_left.begin(); v!=bound_left.end(); ++v) {
// this constraint does not contribute to the "Hessian expression"
dgi=Vector::zeros(n);
dgi[v]=-1.0;
J.put(0, l, dgi, false);
J.put(l, 0, (x_lambda[l]*dgi), true);
J.put(l, n, Vector::zeros(nb_mult), true);
J[l][l]=(-x[v]+sys.box[v].lb());
J[lambda0][l] = 1.0;
l++;
}
for (BitSet::const_iterator v=bound_right.begin(); v!=bound_right.end(); ++v) {
// this constraint does not contribute to the "Hessian expression"
dgi=Vector::zeros(n);
dgi[v]=1.0;
J.put(0, l, dgi, false);
J.put(l, 0, (x_lambda[l]*dgi), true);
J.put(l, n, Vector::zeros(nb_mult), true);
J[l][l]=(x[v]-sys.box[v].ub());
J[lambda0][l] = 1.0;
l++;
}
assert(l==nb_mult+n);
J.put(0,0,hessian);
}
bool proj_sub(const IntervalMatrix& y, IntervalMatrix& x1, IntervalMatrix& x2) {
x1 &= y+x2;
x2 &= x1-y;
return !x1.is_empty() && !x2.is_empty();
}
bool proj_add(const IntervalMatrix& y, IntervalMatrix& x1, IntervalMatrix& x2) {
x1 &= y-x2;
x2 &= y-x1;
return !x1.is_empty() && !x2.is_empty();
}
ExprConstant::ExprConstant(const IntervalMatrix& m)
: ExprLeaf(Dim::matrix(m.nb_rows(),m.nb_cols())),
value(Dim::matrix(m.nb_rows(),m.nb_cols())) {
value.m() = m;
}
CtcIn::CtcIn(Function& f, const IntervalMatrix& y) :
Ctc(f.nb_var()), _f(f), _d(_f.expr().dim) {
assert(_f.expr().dim==Dim(1,y.nb_rows(),y.nb_cols()));
_d.m() = y;
}
