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;
}
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 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);
}
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];
}
}
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));
}
}
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;
}
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]);
}
}
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 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;
}
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 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));
}
}
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;
}
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();
}
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;
}
