void CtcLMI::mkSolver(ibex::MatrixArray& matrices, const IntervalVector &box)
{
// On modifie F0 en fonction des nouvelles bornes de boite
for (int i = 0; i < box.size(); i++)
{
matrices[0][1 + box.size() + 2 * i][1 + box.size() + 2 * i] = box[i].lb();
matrices[0][1 + box.size() + 2 * i + 1][1 + box.size() + 2 * i + 1] = -box[i].ub();
}
solver.setParameterType(SDPA::PARAMETER_DEFAULT);
solver.setParameterMaxIteration(100);
// solver.setNumThreads(4);
// solver.printParameters(stdout);
solver.inputConstraintNumber(matrices.size());
solver.inputBlockNumber(1);
solver.inputBlockSize(1, matrices[0].nb_cols());
solver.inputBlockType(1, SDPA::SDP);
solver.initializeUpperTriangleSpace();
// solver.inputCVec(1, 0);
// solver.inputCVec(2, -1);
for (int i = 0; i < matrices.size(); i++)
for (int j = 1; j < matrices[i].nb_cols() + 1; j++)
for (int k = 1; k < matrices[i].nb_rows() + 1; k++)
solver.inputElement(i, 1, k, j, matrices[i][k - 1][j - 1]);
}
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);
}
void Function::gradient(const IntervalVector& x, IntervalVector& g) const {
assert(g.size()==nb_var());
assert(x.size()==nb_var());
Gradient().gradient(*this,x,g);
// if (!df) ((Function*) this)->df=new Function(*this,DIFF);
// g=df->eval_vector(x);
}
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 SepInverse::separate(IntervalVector& xin, IntervalVector& xout){
assert(xin.size()==f.nb_var() && xout.size() == f.nb_var());
xin &= xout;
Domain tmp=f.eval_domain(xin);
yin.init(Interval::ALL_REALS);
yout.init(Interval::ALL_REALS);
id->backward(tmp, yin);
id->backward(tmp, yout);
s.separate(yin, yout);
if( yin.is_empty())
xin.set_empty();
else
tmp = id->eval_domain(yin);
f.backward(tmp, xin);
if( yout.is_empty())
xout.set_empty();
else
tmp = id->eval_domain(yout);
f.backward(tmp, xout);
}
void midpoint(const IntervalVector& x, Eigen::VectorXd* x_mid)
{
x_mid->resize(x.size());
for (int i = 0; i < x.size(); ++i) {
(*x_mid)[i] = (x[i].get_upper() + x[i].get_lower()) / 2.0;
}
}
std::string print_mma(const IntervalVector& iv) {
std::string res = "{";
for(int i = 0; i < iv.size()-1; ++i) {
res += "{" + std::to_string(iv[i].lb()) + ", " + std::to_string(iv[i].ub()) + "}, ";
}
res += "{" + std::to_string(iv[iv.size()-1].lb()) + ", " + std::to_string(iv[iv.size()-1].ub()) + "}}";
return res;
}
bool TestIbex::almost_eq(const IntervalVector& y_actual, const IntervalVector& y_expected, double err) {
if (y_actual.size()!=y_actual.size()) return false;
if (y_actual.is_empty() && y_expected.is_empty()) return true;
for (int i=0; i<y_actual.size(); i++) {
if (!almost_eq(y_actual[i], y_expected[i],err)) return false;
}
return true;
}
void SetNodeReg::cut(const IntervalVector& box) {
assert(is_leaf());
unsigned int cutvar;
if(father == NULL)
cutvar = 0;
else
var = (father->var+1)%box.size();
pt = box[var].mid();
left = new SetNodeReg(status,this,(var+1)%box.size());
right = new SetNodeReg(status,this,(var+1)%box.size());
}
Affine2Vector::Affine2Vector(const IntervalVector& x, bool b) :
_n(x.size()),
_vec(new Affine2[x.size()]) {
if (!b) {
for (int i = 0; i < x.size(); i++) {
_vec[i] = Affine2(x[i]);
}
} else {
for (int i = 0; i < x.size(); i++) {
_vec[i] = Affine2(x.size(), i + 1, x[i]);
}
}
}
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; }
}
}
}
int Interset(IntervalVector x, IntervalVector y) {
assert(x.size() == y.size());
int res = 2;
int i;
for(i = 0;i<x.size();i++) {
if (!x[i].is_subset(y[i])) {
res = 1;
break;}
}
while(i<x.size()) {
if(!x.intersects(y)) {
res = 0;
return res;
}
i++;}
return res;}
/*
* Restricted propagation procedure. Used when a direction is solved, but no new q-intersection is found.
* Same as propagate, except that upper bounds are not recorded (because we did not find a curr_qinter to work with).
*/
void propagate_no_ub(const Array<IntervalVector>& boxes, IntStack ***dirboxes, int dimension, bool left, IntervalVector& hull_qinter, vector<BitSet *>& nogoods) {
unsigned int n = hull_qinter.size();
unsigned int p = boxes.size();
IntervalVector b(n);
/* Create the new nogood */
BitSet *newNogood = new BitSet(0,p-1,BitSet::empt);
/* We iterate through the boxes to propagate bounds */
/* This does not seem to be optimal ; maybe we should study directly the directions' lists ? */
for (int i=0; i<p; i++) {
b = boxes[i];
/* Check if the box can be added to the new nogood */
if ((left && (b[dimension].lb() < hull_qinter[dimension].lb())) || (!left && (b[dimension].ub() > hull_qinter[dimension].ub()))) {
newNogood->add(i);
}
/* Check if the box is strictly outside our current q-inter hull */
if ((left && (b[dimension].ub() < hull_qinter[dimension].lb())) || (!left && (b[dimension].lb() > hull_qinter[dimension].ub()))) {
for (int k=dimension+1; k<n; k++) {
if (dirboxes[k][0]->contain(i)) dirboxes[k][0]->remove(i);
if (dirboxes[k][1]->contain(i)) dirboxes[k][1]->remove(i);
}
continue;
}
}
nogoods.push_back(newNogood);
}
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));
}
}
IntervalVector Set::inflate_one_float(const IntervalVector& box) {
IntervalVector inflated=box;
for (int i=0;i<box.size();i++) {
double lb=box[i].lb();
double ub=box[i].ub();
inflated[i]=Interval(lb==NEG_INFINITY? lb : previous_float(lb), ub==POS_INFINITY? ub : next_float(ub));
}
return inflated;
}
void split_interval(const IntervalVector& x,
double lower_bound,
IntervalQueue* queue)
{
auto n = x.size();
std::vector<int> split(n, 0);
Eigen::VectorXd mid;
midpoint(x, &mid);
while (true) {
IntervalVector x_split(n);
double volume = 1.0;
for (int i = 0; i < n; ++i) {
double a, b;
if (split[i] == 0) {
a = x[i].get_lower();
b = mid[i];
}
else {
a = mid[i];
b = x[i].get_upper();
}
x_split[i] = Interval<double>(a, b);
volume *= b - a;
}
GlobalQueueEntry entry;
entry.volume = volume;
entry.box = x_split;
entry.best_known_lower_bound = lower_bound;
queue->push(entry);
// Move to the next binary vector
// 000001
// 000010
// 000011
// ...
// 111111
int i = 0;
split[i]++;
while (split[i] > 1) {
split[i] = 0;
i++;
if (i < n) {
split[i]++;
}
else {
break;
}
}
if (i == n) {
break;
}
}
}
/**
* calculate the square of the distance to pt
* for the box of the current cell (box given in argument)
*/
void set_dist(const IntervalVector& box, const Vector pt) {
assert(box.size()==pt.size());
Interval d=Interval::ZERO;
for (int i=0; i<pt.size(); i++) {
d += sqr(box[i]-pt[i]);
}
dist=d.lb();
}
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 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;
}
bool compare(const IntervalVector& box1, const IntervalVector& box2)
{
if(box1 == box2)
return true;
for(int i = 0 ; i < box1.size() ; i++)
if(fabs(box1[i].lb() - box2[i].lb()) > 1.0e-5 || fabs(box1[i].ub() - box2[i].ub()) > 1.0e-5)
return false;
return true;
}
void CtcEllipsoid::mkMatrixArray(const IntervalVector& box)
{
// On inverse P
Matrix P_inv=P;//(P.nb_rows(), P.nb_cols());
// real_inverse(P, P_inv);
// On cree le tableau de matrices
// de taille 1+size(box)
// Chaque matrice de taille (1+3*size(box),1+3*size(box))
matrices = new MatrixArray(1 + box.size(), 1 + 3 * box.size(), 1 + 3 * box.size());
// F0
Matrix f0 = Matrix::zeros(1 + 3 * box.size(), 1 + 3 * box.size());
f0[0][0] = pow(R,2);
f0.put(1, 1, P_inv);
f0.put(0, 1, -C, true);
f0.put(1, 0, -C, false);
for (int i = 0; i < box.size(); i++)
{
f0[1 + P_inv.nb_cols() + 2 * i][1 + P_inv.nb_cols() + 2 * i] = -box[i].lb();
f0[1 + P_inv.nb_cols() + 2 * i + 1][1 + P_inv.nb_cols() + 2 * i + 1] = box[i].ub();
}
(*matrices)[0] = -f0;
for (int i = 1; i < matrices->size(); i++)
{
(*matrices)[i] = Matrix::zeros((*matrices)[i].nb_rows(), (*matrices)[i].nb_rows());
(*matrices)[i][i][0] = 1;
(*matrices)[i][0][i] = 1;
(*matrices)[i][1 + P_inv.nb_rows() + 2 * (i - 1) ][1 + P_inv.nb_rows() + 2 * (i - 1) ] = 1;
(*matrices)[i][1 + P_inv.nb_rows() + 2 * (i - 1) + 1][1 + P_inv.nb_rows() + 2 * (i - 1) + 1] = -1;
}
}
IntervalVector operator|(const Affine2Vector& y,const IntervalVector& x) {
// dimensions are non zero henceforth
if (y.size()!=x.size()) throw InvalidIntervalVectorOp("Cannot make the hull of Affine2Vectores with different dimensions");
if (y.is_empty()&&x.is_empty())
return IntervalVector::empty(y.size());
IntervalVector res(y.size());
for (int i=0; i<y.size(); i++) {
res [i] = y[i] | x[i];
}
return res;
}
//-------------------------------------------------------------------------------------------------------------
void CtcPixelMap::world_to_grid(IntervalVector box) {
for(unsigned int i = 0; i < box.size(); i++) {
box[i] = (box[i] - I.origin_[i]) / I.leaf_size_[i];
// Limit range to image size on pixel_coord
box[i] &= Interval(0,I.grid_size_[i]);
}
for (unsigned int i = 0; i < I.ndim; i++) {
pixel_coords[2*i] = floor(box[i].lb());
pixel_coords[2*i+1] = ceil(box[i].ub()-1);
}
}
void Tube::resample(double new_deltaT) {
IntervalVector temp = (*this);
double ratio = new_deltaT/_deltaT;
temp.resize((int)round((1/ratio)*(*this).size()));
if(ratio<1){
for(int i=0;i<temp.size();i++) {
temp[i]=(*this)[(int)round(i*ratio)];
}
} else {
for(int i=0;i<temp.size();i++) {
temp[i]=Interval::EMPTY_SET;
for(int j=0;j<ratio;j++) {
temp[i]=temp[i]|(*this)[(i*ratio)+j];
}
}
}
(*this).resize((int)round((1/ratio)*(*this).size()));
(*this) = temp;
_deltaT = new_deltaT;
}
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 CtcFirstOrderTest::contract(IntervalVector& box, ContractContext& context) {
if(box.size() == 2) {
return;
}
BxpNodeData* node_data = (BxpNodeData*) context.prop[BxpNodeData::id];
if(node_data == nullptr) {
ibex_error("CtcFirstOrderTest: BxpNodeData must be set");
}
vector<IntervalVector> gradients;
for (int i = 0; i < system_.normal_constraints_.size() - 1; ++i) {
if (!system_.normal_constraints_[i].isSatisfied(box)) {
gradients.push_back(system_.normal_constraints_[i].gradient(box));
}
}
for (int i = 0; i < system_.sic_constraints_.size(); ++i) {
if (!system_.sic_constraints_[i].isSatisfied(box, node_data->sic_constraints_caches[i])) {
gradients.push_back(system_.sic_constraints_[i].gradient(box, node_data->sic_constraints_caches[i]));
}
}
// Without the goal variable
IntervalMatrix matrix(nb_var - 1, gradients.size() + 1);
matrix.set_col(0, system_.goal_function_->gradient(box.subvector(0, nb_var - 2)));
for (int i = 0; i < gradients.size(); ++i) {
matrix.set_col(i + 1, gradients[i].subvector(0, nb_var - 2));
}
bool testfailed = true;
if (matrix.nb_cols() == 1) {
if (matrix.col(0).contains(Vector::zeros(nb_var - 2))) {
testfailed = false;
}
} else {
int* pr = new int[matrix.nb_rows()];
int* pc = new int[matrix.nb_cols()];
IntervalMatrix LU(matrix.nb_rows(), matrix.nb_cols());
testfailed = true;
try {
interval_LU(matrix, LU, pr, pc);
} catch(SingularMatrixException&) {
testfailed = false;
}
delete[] pr;
delete[] pc;
}
if(testfailed) {
box.set_empty();
}
}
void Solver::start(const IntervalVector& init_box) {
buffer.flush();
assert(init_box.size()==ctc.nb_var);
Cell* root=new Cell(init_box);
// add data required by this solver
root->add<BisectedVar>();
// add data required by the bisector
bsc.add_backtrackable(*root);
buffer.push(root);
int nb_var=init_box.size();
IntervalVector tmpbox(ctc.nb_var);
impact.fill(0,ctc.nb_var-1);
Timer::start();
}
std::vector<IntervalVector> bisectAllDim(const IntervalVector& iv) {
vector<IntervalVector> res;
res.emplace_back(iv);
for (int i = 0; i < iv.size(); ++i) {
const int res_size = res.size();
for (int j = 0; j < res_size; ++j) {
if (res[j][i].is_bisectable() && res[j][i].diam() > 1e-10) {
auto pair = res[j].bisect(i);
res[j] = pair.first;
res.emplace_back(pair.second);
}
}
}
return res;
}
/*
* Bound propagation and nogood recording. Assumes that dimensions are processed in the ascending order and left side first.
*/
void propagate(const Array<IntervalVector>& boxes, IntStack ***dirboxes, int dimension, bool left, IntervalVector& curr_qinter, vector<BitSet *>& nogoods) {
unsigned int n = curr_qinter.size();
unsigned int p = boxes.size();
IntervalVector b(n);
/* Create the new nogood */
BitSet *newNogood = new BitSet(0,p-1,BitSet::empt);
/* We iterate through the boxes to propagate bounds */
/* This does not seem to be optimal ; maybe we should study directly the directions' lists ? */
for (int i=0; i<p; i++) {
b = boxes[i];
/* Check if the box can be added to the new nogood */
if ((left && (b[dimension].lb() < curr_qinter[dimension].lb())) || (!left && (b[dimension].ub() > curr_qinter[dimension].ub()))) {
newNogood->add(i);
}
/* First check : the q-inter is a valid upper bound for the opposite direction */
if (left && (b[dimension].ub() <= curr_qinter[dimension].ub())) {
if (dirboxes[dimension][1]->contain(i)) dirboxes[dimension][1]->remove(i);
}
/* Second check : check if the box is strictly outside our current q-inter hull */
if ((left && (b[dimension].ub() < curr_qinter[dimension].lb())) || (!left && (b[dimension].lb() > curr_qinter[dimension].ub()))) {
for (int k=dimension+1; k<n; k++) {
if (dirboxes[k][0]->contain(i)) dirboxes[k][0]->remove(i);
if (dirboxes[k][1]->contain(i)) dirboxes[k][1]->remove(i);
}
continue;
}
/* Third check : the q-intersection provides a valid upper bound for the orthogonal dimensions. */
for (int j=dimension+1; j<n; j++) {
if (b[j].lb() >= curr_qinter[j].lb()) {
if (dirboxes[j][0]->contain(i)) dirboxes[j][0]->remove(i);
}
if (b[j].ub() <= curr_qinter[j].ub()) {
if (dirboxes[j][1]->contain(i)) dirboxes[j][1]->remove(i);
}
}
}
nogoods.push_back(newNogood);
}
Affine2 operator*(const Affine2Vector& v1, const IntervalVector& v2) {
assert(v1.size()==v2.size());
int n=v1.size();
Affine2 y(0);
if (v1.is_empty() || v2.is_empty()) {
y.set_empty();
return y;
}
for (int i=0; i<n; i++) {
y+=v1[i] * v2[i];
}
return y;
}
