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));
}
}
int LSmear::var_to_bisect(IntervalMatrix& J, const IntervalVector& box) const {
int lvar = -1;
//Linearization
LPSolver::Status_Sol stat = LPSolver::UNKNOWN;
Vector dual_solution(1);
if (lsmode==LSMEAR_MG) { //compute the Jacobian in the midpoint
IntervalMatrix J2(sys.f_ctrs.image_dim(), sys.nb_var);
IntervalVector box2(IntervalVector(box.mid()).inflate(1e-8));
// IntervalVector box2(IntervalVector(box.random()));
box2 &= box;
sys.f_ctrs.jacobian(box2,J2);
stat = getdual(J2, box, dual_solution);
} else if (lsmode==LSMEAR) {
stat = getdual(J, box, dual_solution);
}
if (stat == LPSolver::OPTIMAL) {
double max_Lmagn = 0.0;
int k=0;
for (int j=0; j<nbvars; j++) {
Interval lsmear=Interval(0.0);
if ((!too_small(box,j)) && (box[j].mag() <1 || box[j].diam()/ box[j].mag() >= prec(j))){
lsmear=dual_solution[j];
for (int i=0; i<sys.f_ctrs.image_dim(); i++){
lsmear += dual_solution[sys.nb_var+i] * J[i][j];
}
}
lsmear*=(box[j].diam());
if (lsmear.mag() > 1e-10 && (j!=goal_var() || mylinearsolver->get_obj_value().mid() > box[goal_var()].lb() )) {
k++;
if (lsmear.mag() > max_Lmagn) {
max_Lmagn = lsmear.mag();
lvar = j;
}
}
}
if (k==1 && lvar==goal_var()) { lvar=-1; }
}
if (lvar==-1) {
// std::cout << "ssr " << std::endl;
lvar=SmearSumRelative::var_to_bisect(J, box);
}
// std::cout << "lsmear " << lvar << std::endl;
return lvar;
}
Interval centeredFormEval(const Function& function, const IntervalVector& arg) {
/*Interval natural_extension = function.eval(arg);
Interval centered_form = function.eval(arg.mid()) + function.gradient(arg) * (arg - arg.mid());*/
IntervalVector new_arg_ub(arg);
IntervalVector new_arg_lb(arg);
IntervalVector grad = function.gradient(arg);
for(int i = 0; i < arg.size(); ++i) {
if(grad[i].lb() > 0) {
new_arg_ub[i] = arg[i].ub();
new_arg_lb[i] = arg[i].lb();
} else if(grad[i].ub() < 0) {
new_arg_ub[i] = arg[i].lb();
new_arg_lb[i] = arg[i].ub();
}
}
Interval natural_extension = function.eval(arg);
Interval centered_form = function.eval(arg.mid()) + grad * (arg - arg.mid());
Interval ub_form = function.eval(arg.ub()) + grad * (arg - arg.ub());
Interval lb_form = function.eval(arg.lb()) + grad * (arg - arg.lb());
Interval res = natural_extension & centered_form & ub_form & lb_form;
res &= Interval(function.eval(new_arg_lb).lb(), function.eval(new_arg_ub).ub());
return res;
}
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; }
}
}
}
BoolInterval PdcHansenFeasibility::test(const IntervalVector& box) {
int n=f.nb_var();
int m=f.image_dim();
IntervalVector mid=box.mid();
/* Determine the "most influencing" variable thanks to
* the pivoting of Gauss elimination */
// ==============================================================
Matrix A=f.jacobian(mid).mid();
Matrix LU(m,n);
int pr[m];
int pc[n]; // the interesting output: the variables permutation
try {
real_LU(A,LU,pr,pc);
} catch(SingularMatrixException&) {
// means in particular that we could not extract an
// invertible m*m submatrix
return MAYBE;
}
// ==============================================================
PartialFnc pf(f,pc,m,mid);
IntervalVector box2(pf.chop(box));
IntervalVector savebox(box2);
if (inflating) {
if (inflating_newton(pf,box2)) {
_solution = pf.extend(box2);
return YES;
}
}
else {
try {
newton(pf,box2);
if (box2.is_strict_subset(savebox)) {
_solution = pf.extend(box2);
return YES;
}
} catch (EmptyBoxException& ) { }
}
_solution.set_empty();
return MAYBE;
}
void contract(IntervalVector& box) {
box=box.mid()+0.5*Interval(-1,1)*box.rad();
}
Optimizer::Status Optimizer::optimize(const IntervalVector& init_box, double obj_init_bound) {
loup=obj_init_bound;
pseudo_loup=obj_init_bound;
buffer.contract(loup);
uplo=NEG_INFINITY;
uplo_of_epsboxes=POS_INFINITY;
nb_cells=0;
nb_simplex=0;
diam_simplex=0;
nb_rand=0;
diam_rand=0;
buffer.flush();
Cell* root=new Cell(IntervalVector(n+1));
write_ext_box(init_box,root->box);
// add data required by the bisector
bsc.add_backtrackable(*root);
// add data "pu" and "pf" (if required)
buffer.cost2().add_backtrackable(*root);
// add data required by optimizer + Fritz John contractor
root->add<EntailedCtr>();
//root->add<Multipliers>();
entailed=&root->get<EntailedCtr>();
entailed->init_root(user_sys,sys);
loup_changed=false;
initial_loup=obj_init_bound;
loup_point=init_box.mid();
time=0;
Timer::start();
handle_cell(*root,init_box);
update_uplo();
try {
while (!buffer.empty()) {
// if (trace >= 2) cout << " buffer " << buffer << endl;
if (trace >= 2) buffer.print(cout);
// cout << "buffer size " << buffer.size() << " " << buffer2.size() << endl;
// removes from the heap buffer, the cells already chosen in the other buffer
if (buffer.empty()) {
//cout << " buffer empty " << buffer.empty() << " " << buffer2.empty() << endl;
// this update is only necessary when buffer was not
// initially empty
update_uplo();
break;
}
loup_changed=false;
Cell *c;
// random choice between the 2 buffers corresponding to two criteria implemented in two heaps)
// critpr chances over 100 to choose the second heap (see CellDoubleHeap)
c=buffer.top();
try {
pair<IntervalVector,IntervalVector> boxes=bsc.bisect(*c);
pair<Cell*,Cell*> new_cells=c->bisect(boxes.first,boxes.second);
buffer.pop();
delete c; // deletes the cell.
handle_cell(*new_cells.first, init_box);
handle_cell(*new_cells.second, init_box);
if (uplo_of_epsboxes == NEG_INFINITY) {
cout << " possible infinite minimum " << endl;
break;
}
if (loup_changed) {
// In case of a new upper bound (loup_changed == true), all the boxes
// with a lower bound greater than (loup - goal_prec) are removed and deleted.
// Note: if contraction was before bisection, we could have the problem
// that the current cell is removed by contractHeap. See comments in
// older version of the code (before revision 284).
double ymax=compute_ymax();
buffer.contract(ymax);
//cout << " now buffer is contracted and min=" << buffer.minimum() << endl;
if (ymax <= NEG_INFINITY) {
if (trace) cout << " infinite value for the minimum " << endl;
break;
}
if (trace) cout << setprecision(12) << "ymax=" << ymax << " uplo= " << uplo<< endl;
}
update_uplo();
time_limit_check();
}
catch (NoBisectableVariableException& ) {
update_uplo_of_epsboxes((c->box)[ext_sys.goal_var()].lb());
buffer.pop();
delete c; // deletes the cell.
update_uplo(); // the heap has changed -> recalculate the uplo
}
}
}
catch (TimeOutException& ) {
return TIME_OUT;
}
Timer::stop();
time+= Timer::VIRTUAL_TIMELAPSE();
if (uplo_of_epsboxes == POS_INFINITY && (loup==POS_INFINITY || (loup==initial_loup && goal_abs_prec==0 && goal_rel_prec==0)))
return INFEASIBLE;
else if (loup==initial_loup)
return NO_FEASIBLE_FOUND;
else if (uplo_of_epsboxes == NEG_INFINITY)
return UNBOUNDED_OBJ;
else
return SUCCESS;
}
/// Processes the data using contractors and bissections. Classifies the boxes in outside (grey), back_in(yellow) and unsafe (red)
void Sivia::do_Sivia(Ctc& tubeConstraints, Data &data, Function gdot, bool calcInner){
QTime tSivia;
tSivia.start();
if (calcInner) //inner approximation calculation
{
int count=0;
while (!data.boxes.empty()) {
IntervalVector currentBox = data.boxes.front(); //start from the first one
data.boxes.pop_front(); //once it has been copied remove the first box
IntervalVector auxBox=currentBox; //store it in aux variable to compare later
tubeConstraints.contract(currentBox); //contract the current box using the previously calculated constraints
if (currentBox!=auxBox){ //if the box has been contracted
IntervalVector* removedByContractorInner;
int setDiff=auxBox.diff(currentBox, removedByContractorInner); //set difference between the contracted box and the original box
for (int i = 0; i < setDiff; ++i) {
bool testInside=true;
IntervalVector gg=data.g->eval_vector(removedByContractorInner[i]);
for(int j = 0; j<gg.size(); j++){
testInside = testInside && (gg[j].ub()<=0);
}
if (testInside) {
data.boxesInside.append(removedByContractorInner[i]);
}
}
delete[] removedByContractorInner;
}
if(data.realTimeDraw){ //draw the boxes processing in real time
draw_update(data, auxBox, currentBox);
}
bool allBoxesLessEpsilon=true; //check if all the boxes are smaler than epsilon
for (int i=0;(i<(currentBox.size()-1));i++){
allBoxesLessEpsilon = (allBoxesLessEpsilon && ((currentBox.diam()[i])<=data.epsilons[i]));
}
allBoxesLessEpsilon = (allBoxesLessEpsilon && ((currentBox[currentBox.size()-1].diam())<=data.dt)); //check the time box also
bool boxesLessEpsilon=false; //check if at least one box is smaller than epsilon
for (int i=0;(i<(currentBox.size()-1));i++){
boxesLessEpsilon = boxesLessEpsilon||((currentBox[i].diam())<=data.epsilons[i]);
}
boxesLessEpsilon = boxesLessEpsilon&&((currentBox[currentBox.size()-1].diam())<=data.dt); //check time box
if (allBoxesLessEpsilon) { //if allBoxesLessEpsilon = true the box is unsafe and I continue my loop
(data.boxesInsideUnsafe).push_back(currentBox);
count++;
if (count >=data.maxNumUnsafeBoxes && data.maxNumUnsafeBoxesActivated){ //If I have more boxes than nbPerhaps I stop the loop and I display the results
break;
}
}
else { //Otherwise we bissect following the widest diameter
double l = 0;
double l_temp = 0;
int v = -1;
for(int i = 0; i<currentBox.size()-1; i++){ //test that the diameter of the boxes doesnt depend on time
if(currentBox[i].is_bisectable()||!(currentBox[i].is_degenerated())){
l_temp = currentBox[i].diam();
if(l_temp>=data.epsilons[i] && l_temp/(data.epsilons[i]) > l){
l = l_temp/(data.epsilons[i]);
v = i;
}
}
}
l_temp = currentBox[currentBox.size()-1].diam(); //test the time interval
if(l_temp>=data.dt && l_temp/(data.dt) > l){
v = currentBox.size()-1;
}
if(v != -1 && currentBox[v].is_bisectable()){ // then the test interval of the state variables, and then it bisects the interval which has the largest diameter
pair<IntervalVector,IntervalVector> boxes=currentBox.bisect(v, 0.5);
(data.boxes).push_back(boxes.first);
(data.boxes).push_back(boxes.second);
}
else{
if (data.myDebug){
std::cout<<"Cannot be bisected \n";
}
}
}
}
}
else //outer approximation
{
int count=0;
//process all the boxes in data
while (!data.boxes.empty()) {
IntervalVector currentBox = data.boxes.front(); //start from the first one
data.boxes.pop_front(); //once it has been copied remove the first box
IntervalVector auxBox=currentBox; //store it in aux variable to compare later
tubeConstraints.contract(currentBox); //contract the current box using the previously calculated constraints
if (currentBox!=auxBox){ //if the box has been contracted
IntervalVector* removedByContractor;
int setDiff=auxBox.diff(currentBox, removedByContractor); //set difference between the contracted box and the original box
for (int i = 0; i < setDiff; ++i) {
data.boxesOutside.push_back(removedByContractor[i]); //add the areas removed by the contractor to the outside set
}
delete[] removedByContractor;
}
if(data.realTimeDraw){ //draw the boxes processing in real time
draw_update(data, auxBox, currentBox);
}
bool allBoxesLessEpsilon=true; //check if all the boxes are smaler than epsilon
for (int i=0;(i<(currentBox.size()-1));i++){
allBoxesLessEpsilon = (allBoxesLessEpsilon && ((currentBox.diam()[i])<=data.epsilons[i]));
}
allBoxesLessEpsilon = (allBoxesLessEpsilon && ((currentBox[currentBox.size()-1].diam())<=data.dt)); //check the time box also
bool boxesLessEpsilon=false; //check if at least one box is smaller than epsilon
for (int i=0;(i<(currentBox.size()-1));i++){
boxesLessEpsilon = boxesLessEpsilon||((currentBox[i].diam())<=data.epsilons[i]);
}
boxesLessEpsilon = boxesLessEpsilon&&((currentBox[currentBox.size()-1].diam())<=data.dt); //check time box
if (boxesLessEpsilon && !allBoxesLessEpsilon){
IntervalVector xnext = currentBox.subvector(0, data.numVarF-1).mid(); //using the middle point of the box calculate the future positions using euler method
IntervalVector x = currentBox.mid();
bool testBackIn;
for (int i = 0;i<data.numFuturePos;i++){ // Euler method: x(n+1)=x(n)+dt*fx
x[data.numVarF]= x[data.numVarF].mid();
testBackIn = true;
xnext=xnext+(data.dt)*data.f->eval_vector(x);
x.put(0, xnext);
x[data.numVarF] = x[data.numVarF]+(data.dt);
IntervalVector gg=data.g->eval_vector(x);
for(int j = 0; j<gg.size(); j++){
testBackIn = testBackIn && (gg[j].ub()<0); //test if it comes back to the bubble
}
if(testBackIn == true){
break;
}
}
if(testBackIn == true && data.enableBackIn){ //If my box was back in the bubble after integration, I store it in boxesbackin
(data.boxesBackIn).append(currentBox);
continue;
}
}
if (allBoxesLessEpsilon) { //if allBoxesLessEpsilon = true the box is unsafe and I continue my loop
(data.boxesUnsafe).push_back(currentBox);
count++;
if (count >=data.maxNumUnsafeBoxes && data.maxNumUnsafeBoxesActivated){ //If I have more boxes than nbPerhaps I stop the loop and I display the results
break;
}
}
else { //Otherwise we bissect following the widest diameter
double l = 0;
double l_temp = 0;
int v = -1;
for(int i = 0; i<currentBox.size()-1; i++){ //test that the diameter of the boxes doesnt depend on time
if(currentBox[i].is_bisectable()||!(currentBox[i].is_degenerated())){
l_temp = currentBox[i].diam();
if(l_temp>=data.epsilons[i] && l_temp/(data.epsilons[i]) > l){
l = l_temp/(data.epsilons[i]);
v = i;
}
}
}
l_temp = currentBox[currentBox.size()-1].diam(); //test the time interval
if(l_temp>=data.dt && l_temp/(data.dt) > l){
v = currentBox.size()-1;
}
if(v != -1 && currentBox[v].is_bisectable()){ // then the test interval of the state variables, and then it bisects the interval which has the largest diameter
pair<IntervalVector,IntervalVector> boxes=currentBox.bisect(v, 0.5);
(data.boxes).push_back(boxes.first);
(data.boxes).push_back(boxes.second);
}
else{
if (data.myDebug){
std::cout<<"Can not be bisected \n";
}
}
}
}
double maxGValues[data.numVarG-1]; //init vector to store the max values of G
for (int i = 0; i < data.numVarG-1; ++i) {
maxGValues[i]=0; }
for(int i=0; i<data.boxesUnsafe.size();i++) { //process unsafe boxes
IntervalVector currentBox=data.boxesUnsafe.at(i);
IntervalVector nextBox = currentBox.subvector(0, data.numVarF-1);
if (data.intMethod==0){ //Guaranteed integration
// State variables
Variable y(data.numVarF);
// Initial conditions
IntervalVector yinit(data.numVarF);
for (int i = 0; i < data.numVarF; ++i) {
yinit[i] = currentBox[i];
cout<<currentBox[i]<<endl;
}
// system fn has to be re entered here, cannot be loaded directly from text file
//pendulum
Function ydot = Function (y,Return (y[1], -sin(y[0])-0.15*y[1]));
//non holonomic
// Interval t = currentBox[data.numVarF];
// Interval xd = 7*t;
// Interval xdd = 7;
// Interval yd = sin(0.1*t);
// Interval ydd = 0.1*cos(0.1*t);
// Interval xdiff = (xd-y[0]+xdd);
// Interval ydiff = (yd-y[1]+ydd);
// Interval norm = ( sqrt((xdiff)^2 +(ydiff)^2) );
// Function ydot = Function (y,Return (( sqrt((xd-y[0]+xdd)*(xd-y[0]+xdd) +((yd-y[1]+ydd))*(yd-y[1]+ydd)) )*cos(y[2]), ( sqrt(((xd-y[0]+xdd))*(xd-y[0]+xdd) +((yd-y[1]+ydd))*(yd-y[1]+ydd)) )*sin(y[2]), 10*(cos(y[2])*((yd-y[1]+ydd))-sin(y[2])*((xd-y[0]+xdd)))/( sqrt(((xd-y[0]+xdd))*(xd-y[0]+xdd) +((yd-y[1]+ydd))*(yd-y[1]+ydd)) ))); // Ivp contruction (initial time is 0.0)
QTime t1;
t1.start();
ivp_ode problem = ivp_ode (ydot, 0.0 , yinit);
// Simulation construction and run
simulation simu = simulation (&problem,data.dt*data.numFuturePos, __METH__, __PREC__); //uses Runge-kutta4 method
data.boxesUnsafeFuture.append(simu.run_simulation()); //modified ibex_simulation.h to make it return a list with all the solutions, not just the last one
double timeSiviaCalculations1=t1.elapsed()/1000.0;
double timeSiviaCalculationsTotal=tSivia.elapsed()/1000.0;
cout<<endl<<"Unsafe # "<<i<<" , Box time = "<<timeSiviaCalculations1<<" , Total elapsed time = "<<timeSiviaCalculationsTotal<<endl;
}
if (data.intMethod==1){ //euler method
for (int i = 0;i<data.numFuturePos;i++){
IntervalVector gdotValue=gdot.eval_vector(currentBox); //evaluate the g and gdot functions to inspect the constraints
IntervalVector gValue=data.g->eval_vector(currentBox);
if (data.myDebug){
cout<<"box = "<<currentBox<<endl;
for(int j = 0; j<gValue.size(); j++){
cout<<"gdot"<<j<<" = "<<gdotValue<<" / "<<(gdotValue[j].lb()>0) <<endl; //gdot i values
}
}
for(int j = 0; j<gValue.size(); j++){
if (data.myDebug){
cout<<"g"<<j<<" = "<<gValue[j]<<" / "<<((gValue[j].ub()>0)&&(gValue[j].lb()<0)) <<endl;} //print gi values
if((gValue[j].ub()>maxGValues[j])&&(gValue[j].ub()<999999)){ //check max values for each gi, ignore if system goes to infinity
maxGValues[j]=gValue[j].ub();}
}
nextBox=nextBox+(data.dt)*data.f->eval_vector(currentBox); //euler method
data.boxesUnsafeFuture.append(nextBox);
currentBox.put(0, nextBox);
currentBox[data.numVarF] = currentBox[data.numVarF]+(data.dt); //increase time for the next step
}
}
}
for(int i=0; i<data.boxesBackIn.size();i++){ //process back_in boxes
IntervalVector currentBox=data.boxesBackIn.at(i);
IntervalVector nextBox = currentBox.subvector(0, data.numVarF-1);
if (data.intMethod==0){ //guaranteed integration //Guaranteed integration
// State variables
Variable y(data.numVarF);
// Initial conditions
IntervalVector yinit(data.numVarF);
for (int i = 0; i < data.numVarF; ++i) {
yinit[i] = currentBox[i];
cout<<currentBox[i]<<endl;
}
QTime t2;
t2.start();
// system fn has to be re entered here, cannot be loaded directly from text file
//pendulum
Function ydot = Function (y,Return (y[1], -sin(y[0])-0.15*y[1]));
//non holonomic
// Interval t = currentBox[data.numVarF];
// Interval xd = 7*t;
// Interval xdd = 7;
// Interval yd = sin(0.1*t);
// Interval ydd = 0.1*cos(0.1*t);
// Interval xdiff = (xd-y[0]+xdd);
// Interval ydiff = (yd-y[1]+ydd);
// Interval norm = ( sqrt((xdiff)^2 +(ydiff)^2) );
// Function ydot = Function (y,Return (( sqrt((xd-y[0]+xdd)*(xd-y[0]+xdd) +((yd-y[1]+ydd))*(yd-y[1]+ydd)) )*cos(y[2]), ( sqrt(((xd-y[0]+xdd))*(xd-y[0]+xdd) +((yd-y[1]+ydd))*(yd-y[1]+ydd)) )*sin(y[2]), 10*(cos(y[2])*((yd-y[1]+ydd))-sin(y[2])*((xd-y[0]+xdd)))/( sqrt(((xd-y[0]+xdd))*(xd-y[0]+xdd) +((yd-y[1]+ydd))*(yd-y[1]+ydd)) ))); // Ivp contruction (initial time is 0.0)
ivp_ode problem = ivp_ode (ydot, currentBox[data.numVarF].lb() , yinit);
// Simulation construction and run
simulation simu = simulation (&problem,data.dt*data.numFuturePos, __METH__, __PREC__); //uses Runge-kutta4 method
data.boxesUnsafeFuture.append(simu.run_simulation()); //modified ibex_simulation.h to make it return a list with all the solutions, not just the last one
double timeSiviaCalculations2=t2.elapsed()/1000.0;
double timeSiviaCalculationsTotal=tSivia.elapsed()/1000.0;
cout<<endl<<"Back_in # "<<i<<" , Box time = "<<timeSiviaCalculations2<<" , Total elapsed time = "<<timeSiviaCalculationsTotal<<endl;
}
for (int i = 0;i<data.numFuturePos;i++){ //euler method
if (data.intMethod==1){
IntervalVector gdotValue=gdot.eval_vector(currentBox); //evaluate the g and gdot functions to inspect the constraints
IntervalVector gValue=data.g->eval_vector(currentBox);
if (data.myDebug){
cout<<"box = "<<currentBox<<endl;
for(int j = 0; j<gValue.size(); j++){
cout<<"gdot"<<j<<" = "<<gdotValue<<" / "<<(gdotValue[j].lb()>0) <<endl; //gdoti values
}
}
bool testBackIn= true;
for(int j = 0; j<gValue.size(); j++){
if (data.myDebug){
cout<<"g"<<j<<" = "<<gValue[j]<<" / "<<((gValue[j].ub()>0)&&(gValue[j].lb()<0)) <<endl;} //print gi values
testBackIn = testBackIn && (gValue[j].ub()<0); //test if it comes back to the bubble
if((gValue[j].ub()>maxGValues[j])&&(gValue[j].ub()<999999)){ //check max values for each gi, ignore if system goes to infinity
maxGValues[j]=gValue[j].ub();
}
}
nextBox=nextBox+(data.dt)*data.f->eval_vector(currentBox); // euler method
if (!testBackIn){
data.boxesBackInFuture.append(nextBox);
}
currentBox.put(0, nextBox);
currentBox[data.numVarF] = currentBox[data.numVarF]+(data.dt); //increase time for the next step
}
}
}
if (data.myDebug){
for (int i = 0; i < data.numVarG-1; ++i) {
cout<<"Max G"<<i<<" = "<<maxGValues[i]<<endl;}
}
}
}
int LinearizerDuality::linearize(const IntervalVector& box, LPSolver& lp_solver, BoxProperties& prop) {
// ========= get active constraints ===========
/* Using system cache seems not interesting. */
//BxpSystemCache* cache=(BxpSystemCache*) prop[BxpSystemCache::get_id(sys)];
BxpSystemCache* cache=NULL;
//--------------------------------------------------------------------------
BitSet* active;
if (cache!=NULL) {
active = &cache->active_ctrs();
} else {
active = new BitSet(sys.active_ctrs(box));
}
// ============================================
size_t n = sys.nb_var;
size_t m = sys.f_ctrs.image_dim();
size_t n_total = n + m*n;
int nb_ctr=0; // number of inequalities added in the LP solver
// BxpLinearRelaxArgMin* argmin=(BxpLinearRelaxArgMin*) prop[BxpLinearRelaxArgMin::get_id(sys)];
//
// if (argmin && argmin->argmin()) {
// pt=*argmin->argmin();
// } else
pt=box.mid();
if (!active->empty()) {
//IntervalMatrix J=cache? cache->active_ctrs_jacobian() : sys.f_ctrs.jacobian(box,active);
//IntervalMatrix J=sys.f_ctrs.jacobian(box,*active);
IntervalMatrix J(active->size(),n); // derivatives over the box
sys.f_ctrs.hansen_matrix(box,pt,J,*active);
if (J.is_empty()) {
if (cache==NULL) delete active;
return -1;
}
// the evaluation of the constraints in the point
IntervalVector gx(sys.f_ctrs.eval_vector(pt,*active));
if (gx.is_empty()) {
if (cache==NULL) delete active;
return 0;
}
int i=0; // counter of active constraints
for (BitSet::iterator c=active->begin(); c!=active->end(); ++c, i++) {
if (!sys.f_ctrs.deriv_calculator().is_linear[c]) {
for (size_t j=0; j<n; j++) {
Vector row(n_total,0.0);
row[j]=1;
row[n + c*n +j]=1;
double rhs = pt[j] - lp_solver.get_epsilon();
lp_solver.add_constraint(row, LEQ, rhs);
nb_ctr++;
}
}
Vector row(n_total,0.0);
row.put(0,J[i].lb());
IntervalVector gl(J[i].lb());
Vector diam_correctly_rounded = (IntervalVector(J[i].ub())-gl).lb();
for (size_t j=0; j<n; j++) {
if (diam_correctly_rounded[j]<0)
ibex_error("negative diameter");
}
row.put(n + c*n,-diam_correctly_rounded);
double rhs = (-gx[i] + (gl*pt)).lb()- lp_solver.get_epsilon();
lp_solver.add_constraint(row, LEQ, rhs);
nb_ctr++;
}
}
if (cache==NULL) delete active;
return nb_ctr;
}
bool inflating_newton(const Fnc& f, const VarSet* vars, const IntervalVector& full_box, IntervalVector& box_existence, IntervalVector& box_unicity, int k_max, double mu_max, double delta, double chi) {
int n=vars ? vars->nb_var : f.nb_var();
assert(f.image_dim()==n);
assert(full_box.size()==f.nb_var());
if (full_box.is_empty()) {
box_existence.set_empty();
box_unicity.set_empty();
return false;
}
int k=0;
bool success=false;
IntervalVector mid(n); // Midpoint of the current box
IntervalVector Fmid(n); // Evaluation of f at the midpoint
IntervalMatrix J(n, n); // Hansen matrix of f % variables
// Following variables are introduced just to use a
// centered-form on parameters when evaluating Fmid
IntervalVector* p=NULL; // Parameter box
IntervalVector* midp=NULL; // Parameter box midpoint
// -------------------------------------------------
IntervalMatrix* Jp=NULL; // Jacobian % parameters
//
if (vars) {
p=new IntervalVector(vars->param_box(full_box));
midp=new IntervalVector(p->mid());
Jp=new IntervalMatrix(n,vars->nb_param);
}
IntervalVector y(n);
IntervalVector y1(n);
IntervalVector box = vars ? vars->var_box(full_box) : full_box;
IntervalVector& full_mid = vars ? *new IntervalVector(full_box) : mid;
// Warning: box_existence is used to store the full box of the
// current iteration (that is, param_box x box)
// It will eventually (at return) be the
// existence box in case of success. Nothing is proven inside
// box_existence until success==true in the loop (note: inflation
// stops when success is true and existence is thus preserved
// until the end)
box_existence = full_box;
// Just to quickly initialize the domains of parameters
box_unicity = full_box;
y1 = box.mid();
while (k<k_max) {
//cout << "current box=" << box << endl << endl;
if (vars)
f.hansen_matrix(box_existence, J, *Jp, *vars);
else
f.hansen_matrix(box_existence, J);
if (J.is_empty()) break;
mid = box.mid();
if (vars) vars->set_var_box(full_mid, mid);
Fmid=f.eval_vector(full_mid);
// Use the jacobian % parameters to calculate
// a mean-value form for Fmid
if (vars) {
Fmid &= f.eval_vector(vars->full_box(mid,*midp))+(*Jp)*(*p-*midp);
}
y = mid-box;
//if (y==y1) break; <--- allowed in Newton inflation
y1=y;
try {
precond(J, Fmid);
} catch(LinearException&) {
break; // should be false
}
// Note: giving mu_max to gauss-seidel (GS) is slightly different from checking the condition "mu<mu_max" in the
// Newton procedure itself. If GS transforms x0 to x1 in n iterations, and then x1 to x2 in n other iterations
// it is possible that each of these 2n iterations satisfies mu<mu_max, whereas the two global Newton iterations
// do not, i.e., d(x2,x1) > mu_max d(x1,x0).
if (!inflating_gauss_seidel(J, Fmid, y, 1e-12, mu_max)) {// TODO: replace hardcoded value 1e-12
// when k~kmax, "divergence" may also mean "cannot contract more" (d/dold~1)
break;
}
IntervalVector box2=mid-y;
if (box2.is_subset(box)) {
assert(!box2.is_empty());
if (!success) { // to get the largest unicity box, we do this
// only when the first contraction occurs
if (vars) vars->set_var_box(box_unicity,box);
else box_unicity = box;
//=================================================
// We now try to enlarge the unicity box as possible
// =================================================
// IntervalVector box2copy=box2;
//
// bool inflate_ok=true;
//
// while (inflate_ok) {
//
// box2copy.inflate(delta,0.0);
//
// // box_existence is also used inside this iteration
// // to store the "full box"
// if (vars) vars->set_var_box(box_existence,box2copy);
// else box_existence = box2copy;
//
// newton(f,vars,box_existence,0.0,default_gauss_seidel_ratio);
//
// if (vars) {
// if (vars->var_box(box_existence).is_interior_subset(box2))
// vars->set_var_box(box_unicity,box2copy);
// else inflate_ok=false;
// } else {
// if (box_existence.is_interior_subset(box2))
// box_unicity = box2copy;
// else inflate_ok=false;
// }
// }
}
success=true; // we don't return now, to let the box being contracted more
}
box = success? box2 : box2.inflate(delta,chi);
k++;
// we update box_existence inside the loop because
// the Jacobian has to be recalculated on the current
// full box at each iteration
if (vars) vars->set_var_box(box_existence,box);
else box_existence = box;
}
if (vars) {
delete p;
delete midp;
delete Jp;
delete &full_mid;
}
if (!success) {
box_existence.set_empty();
box_unicity.set_empty();
}
return success;
}
LPSolver::Status_Sol LSmear::getdual(IntervalMatrix& J, const IntervalVector& box, Vector& dual) const {
int _goal_var = goal_var();
bool minimize=true;
if (_goal_var == -1){
_goal_var = RNG::rand()%box.size();
minimize=RNG::rand()%2;
}
// The linear system is created
mylinearsolver->clean_ctrs();
mylinearsolver->set_bounds(box);
mylinearsolver->set_bounds_var(_goal_var, Interval(-1e10,1e10));
int nb_lctrs[sys.f_ctrs.image_dim()]; /* number of linear constraints generated by nonlinear constraint*/
for (int i=0; i<sys.f_ctrs.image_dim(); i++) {
Vector row1(sys.nb_var);
Interval ev(0.0);
for (int j=0; j<sys.nb_var; j++) {
row1[j] = J[i][j].mid();
ev -= Interval(row1[j])*box[j].mid();
}
ev+= sys.f_ctrs.eval(i,box.mid()).mid();
nb_lctrs[i]=1;
if (i!=goal_ctr()) {
if (sys.ops[i] == LEQ || sys.ops[i] == LT){
mylinearsolver->add_constraint( row1, sys.ops[i], (-ev).ub());
}
else if (sys.ops[i] == GEQ || sys.ops[i] == GT)
mylinearsolver->add_constraint( row1, sys.ops[i], (-ev).lb());
else { //op=EQ
mylinearsolver->add_constraint( row1, LT, (-ev).ub());
mylinearsolver->add_constraint( row1, GT, (-ev).lb());
nb_lctrs[i]=2;
}
}
else if (goal_to_consider(J,i))
mylinearsolver->add_constraint( row1, LEQ, (-ev).ub());
else // the goal is equal to a variable : the goal constraint is useless.
nb_lctrs[i]=0;
}
//the linear system is solved
LPSolver::Status_Sol stat=LPSolver::UNKNOWN;
try {
mylinearsolver->set_obj_var(_goal_var, (minimize)? 1.0:-1.0);
stat = mylinearsolver->solve();
if (stat == LPSolver::OPTIMAL) {
// the dual solution : used to compute the bound
dual.resize(mylinearsolver->get_nb_rows());
dual = mylinearsolver->get_dual_sol();
int k=0; //number of multipliers != 0
int ii=0;
for (int i=0; i<sys.f_ctrs.image_dim(); i++) {
if (nb_lctrs[i]==2) {
dual[sys.nb_var+i]=dual[sys.nb_var+ii]+dual[sys.nb_var+ii+1]; ii+=2;
} else {
dual[sys.nb_var+i]=dual[sys.nb_var+ii]; ii++;
}
if (std::abs(dual[sys.nb_var+i])>1e-10) k++;
}
if(k<2) { stat = LPSolver::UNKNOWN; }
}
} catch (LPException&) {
stat = LPSolver::UNKNOWN;
}
return stat;
}
/// Processes the data using contractors and bissections. Classifies the boxes in outside (grey), back_in(yellow) and unsafe (red)
void Sivia::do_Sivia(Ctc& tubeConstraints, Data &data, Function gdot, bool calcInner) {
QTime tSivia;
tSivia.start();
if (calcInner) //inner approximation calculation
{
int count=0;
while (!data.boxes.empty()) {
IntervalVector currentBox = data.boxes.front(); //start from the first one
data.boxes.pop_front(); //once it has been copied remove the first box
IntervalVector auxBox=currentBox; //store it in aux variable to compare later
tubeConstraints.contract(currentBox); //contract the current box using the previously calculated constraints
if (currentBox!=auxBox){ //if the box has been contracted
IntervalVector* removedByContractorInner;
int setDiff=auxBox.diff(currentBox, removedByContractorInner); //set difference between the contracted box and the original box
for (int i = 0; i < setDiff; ++i) {
//data.boxesOutside.push_back(removedByContractor[i]); //add the areas removed by the contractor to the outside set
bool testInside=true;
IntervalVector gg=data.g->eval_vector(removedByContractorInner[i]);
for(int j = 0; j<gg.size(); j++){
testInside = testInside && (gg[j].ub()<=0);
}
if (testInside) {
data.boxesInside.append(removedByContractorInner[i]);
}
}
delete[] removedByContractorInner;
}
if(data.realTimeDraw){ //draw the boxes processing in real time
draw_update(data, auxBox, currentBox);
}
bool allBoxesLessEpsilon=true; //check if all the boxes are smaler than epsilon
for (int i=0;(i<(currentBox.size()-1));i++){
allBoxesLessEpsilon = (allBoxesLessEpsilon && ((currentBox.diam()[i])<=data.epsilons[i]));
}
allBoxesLessEpsilon = (allBoxesLessEpsilon && ((currentBox[currentBox.size()-1].diam())<=data.dt)); //check the time box also
bool boxesLessEpsilon=false; //check if at least one box is smaller than epsilon
for (int i=0;(i<(currentBox.size()-1));i++){
boxesLessEpsilon = boxesLessEpsilon||((currentBox[i].diam())<=data.epsilons[i]);
}
boxesLessEpsilon = boxesLessEpsilon&&((currentBox[currentBox.size()-1].diam())<=data.dt); //check time box
if (boxesLessEpsilon && !allBoxesLessEpsilon){
IntervalVector xnext = currentBox.subvector(0, data.numVarF-1).mid(); //using the middle point of the box calculate the future positions using euler method
IntervalVector x = currentBox.mid();
bool testBackIn;
for (int i = 0;i<data.numFuturePos;i++){ // Euler method: x(n+1)=x(n)+dt*fx
x[data.numVarF]= x[data.numVarF].mid();
testBackIn = true;
xnext=xnext+(data.dt)*data.f->eval_vector(x);
x.put(0, xnext);
x[data.numVarF] = x[data.numVarF]+(data.dt);
IntervalVector gg=data.g->eval_vector(x);
for(int j = 0; j<gg.size(); j++){
testBackIn = testBackIn && (gg[j].ub()<0); //test if it comes back to the bubble
}
if(testBackIn == true){ //If so we calculate the max deviation
break;
}
}
if(testBackIn == true){ //If my box was back in the bubble after integration, I store it in boxesbackin
(data.boxesInsideBackIn).append(currentBox);
continue;
}
}
if (allBoxesLessEpsilon) { //if allBoxesLessEpsilon = true the box is unsafe and I continue my loop
(data.boxesInsideUnsafe).push_back(currentBox);
count++;
if (count >=data.maxNumUnsafeBoxes && data.maxNumUnsafeBoxesActivated){ //If I have more boxes than nbPerhaps I stop the loop and I display the results
break;
}
}
else { //Otherwise we bissect following the widest diameter
double l = 0;
double l_temp = 0;
int v = -1;
for(int i = 0; i<currentBox.size()-1; i++){ //test that the diameter of the boxes doesnt depend on time
if(currentBox[i].is_bisectable()||!(currentBox[i].is_degenerated())){
l_temp = currentBox[i].diam();
if(l_temp>=data.epsilons[i] && l_temp/(data.epsilons[i]) > l){
l = l_temp/(data.epsilons[i]);
v = i;
}
}
}
l_temp = currentBox[currentBox.size()-1].diam(); //test the time interval
if(l_temp>=data.dt && l_temp/(data.dt) > l){
v = currentBox.size()-1;
}
if(v != -1 && currentBox[v].is_bisectable()){ // then the test interval of the state variables, and then it bisects the interval which has the largest diameter
pair<IntervalVector,IntervalVector> boxes=currentBox.bisect(v, 0.5);
(data.boxes).push_back(boxes.first);
(data.boxes).push_back(boxes.second);
}
else{
if (data.myDebug){
std::cout<<"Cannot be bisected \n";
}
}
}
}
}
else //outer approximation
{
int count=0;
//SIVIA
//process all the boxes in data
while (!data.boxes.empty()) {
IntervalVector currentBox = data.boxes.front(); //start from the first one
data.boxes.pop_front(); //once it has been copied remove the first box
IntervalVector auxBox=currentBox; //store it in aux variable to compare later
tubeConstraints.contract(currentBox); //contract the current box using the previously calculated constraints
if (currentBox!=auxBox){ //if the box has been contracted
IntervalVector* removedByContractor;
int setDiff=auxBox.diff(currentBox, removedByContractor); //set difference between the contracted box and the original box
for (int i = 0; i < setDiff; ++i) {
data.boxesOutside.push_back(removedByContractor[i]); //add the areas removed by the contractor to the outside set
}
delete[] removedByContractor;
}
if(data.realTimeDraw){ //draw the boxes processing in real time
draw_update(data, auxBox, currentBox);
}
bool allBoxesLessEpsilon=true; //check if all the boxes are smaler than epsilon
for (int i=0;(i<(currentBox.size()-1));i++){
allBoxesLessEpsilon = (allBoxesLessEpsilon && ((currentBox.diam()[i])<=data.epsilons[i]));
}
allBoxesLessEpsilon = (allBoxesLessEpsilon && ((currentBox[currentBox.size()-1].diam())<=data.dt)); //check the time box also
bool boxesLessEpsilon=false; //check if at least one box is smaller than epsilon
for (int i=0;(i<(currentBox.size()-1));i++){
boxesLessEpsilon = boxesLessEpsilon||((currentBox[i].diam())<=data.epsilons[i]);
}
boxesLessEpsilon = boxesLessEpsilon&&((currentBox[currentBox.size()-1].diam())<=data.dt); //check time box
if (boxesLessEpsilon && !allBoxesLessEpsilon){
IntervalVector xnext = currentBox.subvector(0, data.numVarF-1).mid(); //using the middle point of the box calculate the future positions using euler method
IntervalVector x = currentBox.mid();
bool testBackIn;
for (int i = 0;i<data.numFuturePos;i++){ // Euler method: x(n+1)=x(n)+dt*fx
x[data.numVarF]= x[data.numVarF].mid();
testBackIn = true;
xnext=xnext+(data.dt)*data.f->eval_vector(x);
x.put(0, xnext);
x[data.numVarF] = x[data.numVarF]+(data.dt);
IntervalVector gg=data.g->eval_vector(x);
for(int j = 0; j<gg.size(); j++){
testBackIn = testBackIn && (gg[j].ub()<0); //test if it comes back to the bubble
}
if(testBackIn == true){ //If so we calculate the max deviation
break;
}
}
// if(testBackIn == true){ //If my box was back in the bubble after integration, I store it in boxesbackin
// (data.boxesBackIn).append(currentBox);
// continue;
// }
}
if (allBoxesLessEpsilon) { //if allBoxesLessEpsilon = true the box is unsafe and I continue my loop
(data.boxesUnsafe).push_back(currentBox);
count++;
if (count >=data.maxNumUnsafeBoxes && data.maxNumUnsafeBoxesActivated){ //If I have more boxes than nbPerhaps I stop the loop and I display the results
break;
}
}
else { //Otherwise we bissect following the widest diameter
double l = 0;
double l_temp = 0;
int v = -1;
for(int i = 0; i<currentBox.size()-1; i++){ //test that the diameter of the boxes doesnt depend on time
if(currentBox[i].is_bisectable()||!(currentBox[i].is_degenerated())){
l_temp = currentBox[i].diam();
if(l_temp>=data.epsilons[i] && l_temp/(data.epsilons[i]) > l){
l = l_temp/(data.epsilons[i]);
v = i;
}
}
}
l_temp = currentBox[currentBox.size()-1].diam(); //test the time interval
if(l_temp>=data.dt && l_temp/(data.dt) > l){
v = currentBox.size()-1;
}
if(v != -1 && currentBox[v].is_bisectable()){ // then the test interval of the state variables, and then it bisects the interval which has the largest diameter
pair<IntervalVector,IntervalVector> boxes=currentBox.bisect(v, 0.5);
(data.boxes).push_back(boxes.first);
(data.boxes).push_back(boxes.second);
}
else{
if (data.myDebug){
std::cout<<"Cannot be bisected \n";
}
}
}
}
for(int i=0; i<data.boxesUnsafe.size();i++) { //process unsafe boxes
IntervalVector currentBox=data.boxesUnsafe.at(i);
IntervalVector nextBox = currentBox.subvector(0, data.numVarF-1);
if (data.intMethod==0){ //Guaranteed integration
// State variables
Variable y(data.numVarF);
// Initial conditions
IntervalVector yinit(data.numVarF);
for (int i = 0; i < data.numVarF; ++i) {
yinit[i] = currentBox[i];
cout<<currentBox[i]<<endl;
}
Interval t = currentBox[data.numVarF];
Interval xd = 7*t;
Interval xdd = 7;
Interval yd = sin(0.1*t);
Interval ydd = 0.1*cos(0.1*t);
// Interval xdiff = (xd-y[0]+xdd);
// Interval ydiff = (yd-y[1]+ydd);
// Interval norm = (sqrt((xdiff)^2 +(ydiff)^2));
// system fn has to be re entered here, cannot be loaded directly from text file
//pendulum
Function ydot = Function (y,Return (y[1],-1*sin(y[0])-0.15*y[1])); // Ivp contruction (initial time is 0.0)
//non holonomic
// Function ydot = Function (y, Return ((sqrt(((xd-y[0]+xdd)*(xd-y[0]+xdd)) +((yd-y[1]+ydd)*(yd-y[1]+ydd))))*cos(y[2]), (sqrt(((xd-y[0]+xdd)*(xd-y[0]+xdd)) +((yd-y[1]+ydd)*(yd-y[1]+ydd))))*sin(y[2]),10*(cos(y[2])*((yd-y[1]+ydd))-sin(y[2])*((xd-y[0]+xdd)))/(sqrt(((xd-y[0]+xdd)*(xd-y[0]+xdd)) +((yd-y[1]+ydd)*(yd-y[1]+ydd)))))); // Ivp contruction (initial time is 0.0)
QTime t1;
t1.start();
ivp_ode problem = ivp_ode (ydot,0.0 , yinit);
// Simulation construction and run
simulation simu = simulation (&problem,data.dt*data.numFuturePos, __METH__, __PREC__); //uses Runge-kutta4 method
data.boxesUnsafeFuture.append(simu.run_simulation()); //modified ibex_simulation.h to make it return a list with all the solutions, not just the last one
double timeSiviaCalculations1=t1.elapsed()/1000.0;
double timeSiviaCalculationsTotal=tSivia.elapsed()/1000.0;
cout<<endl<<"Unsafe # "<<i<<" , Box time = "<<timeSiviaCalculations1<<" , Total elapsed time = "<<timeSiviaCalculationsTotal<<endl;
}
if (data.intMethod==1){ //euler method
for (int i = 0;i<data.numFuturePos;i++){
IntervalVector gdotValue=gdot.eval_vector(currentBox); //evaluate the g and gdot functions to inspect the constraints
IntervalVector gValue=data.g->eval_vector(currentBox);
if (data.myDebug){
cout<<"box = "<<currentBox<<endl;
for(int j = 0; j<gValue.size(); j++){
cout<<"gdot"<<j<<" = "<<gdotValue<<" / "<<(gdotValue[j].lb()>0) <<endl; //gdot i values
}
}
nextBox=nextBox+(data.dt)*data.f->eval_vector(currentBox); //euler method
data.boxesUnsafeFuture.append(nextBox);
currentBox.put(0, nextBox);
currentBox[data.numVarF] = currentBox[data.numVarF]+(data.dt); //increase time for the next step
}
}
if (data.intMethod==2){ //vertex unsafe
//check derivative of the unsafe boxes of outer g wrt the inner g
// Function g_inner("g_inner.txt");
// Function dg_inner(g_inner, Function::DIFF); // d/dx(gi)(x,t)
// Variable x(data.numVarF),t; //we have x[] and t as variables for our fns
// // initialize auxMat and auxVector to the correct sizes and fill with zeros
// IntervalMatrix auxMat(data.numVarF+1, data.numVarF,Interval::ZERO);
// IntervalVector auxVector(data.numVarF+1,Interval::ZERO);
// //put 1 in the diagonal of auxMat
// for (int i=0; i<data.numVarF; i++){
// auxMat[i][i]=1;}
// auxVector[data.numVarF]=1;
// Function f=("f.txt");
// Function gdot_inner(x,t,dg_inner(x,t)*(auxMat*transpose(f(x,t))+auxVector));
// cout<<"gdot: "<<gdot_inner<<endl;
// IntervalVector gdot_inner_Result=gdot_inner.eval_vector(currentBox);
// bool testNegative=true;
// for(int j = 0; j<gdot_inner_Result.size(); j++){
// testNegative = testNegative && (gdot_inner_Result[j].ub()<0);
// }
// if (testNegative) {
// data.boxesOuterGSafeForInnerG.append(currentBox);
// cout<<"Safe for inner G: "<<currentBox<<" result: "<<gdot_inner_Result<<endl;
// }
// else{
// data.boxesOuterGUnsafeForInnerG.append(currentBox);
// cout<<"Unsafe for inner G: "<<currentBox<<" result: "<<gdot_inner_Result<<endl;
// }
//system
double x_k_lb[currentBox.size()], x_k_ub[currentBox.size()];
for (int j = 0; j < currentBox.size(); ++j) {
x_k_ub[j]=currentBox[j].ub();
x_k_lb[j]=currentBox[j].lb();
}
int numVar=currentBox.size()-1;
int numSamples=data.dt*data.numFuturePos*1000;
double x_k_uu[numVar][numSamples];
double x_k_ul[numVar][numSamples];
double x_k_lu[numVar][numSamples];
double x_k_ll[numVar][numSamples];
x_k_uu[0][0]=x_k_ub[0];
x_k_ll[0][0]=x_k_lb[0];
x_k_lu[0][0]=x_k_lb[0];
x_k_ul[0][0]=x_k_ub[0];
x_k_uu[1][0]=x_k_ub[1];
x_k_ll[1][0]=x_k_lb[1];
x_k_lu[1][0]=x_k_ub[1];
x_k_ul[1][0]=x_k_lb[1];
double g_outer_uu, g_outer_lu, g_outer_ul, g_outer_ll, g_inner_uu, g_inner_lu, g_inner_ul, g_inner_ll;
bool unsafeBox=false;
bool safeBox=false;
//pendulum system
for (int j = 1; j < data.dt*data.numFuturePos*1000; ++j) {
x_k_uu[1][j]=data.dt/10.0*(-sin(x_k_uu[0][j-1])-0.15*x_k_uu[1][j-1])+x_k_uu[1][j-1];
x_k_ll[1][j]=data.dt/10.0*(-sin(x_k_ll[0][j-1])-0.15*x_k_ll[1][j-1])+x_k_ll[1][j-1];
x_k_lu[1][j]=data.dt/10.0*(-sin(x_k_lu[0][j-1])-0.15*x_k_lu[1][j-1])+x_k_lu[1][j-1];
x_k_ul[1][j]=data.dt/10.0*(-sin(x_k_ul[0][j-1])-0.15*x_k_ul[1][j-1])+x_k_ul[1][j-1];
x_k_uu[0][j]=(x_k_uu[1][j-1])*(data.dt/10.0)+x_k_uu[0][j-1];
x_k_ll[0][j]=(x_k_ll[1][j-1])*(data.dt/10.0)+x_k_uu[0][j-1];
x_k_lu[0][j]=(x_k_lu[1][j-1])*(data.dt/10.0)+x_k_uu[0][j-1];
x_k_ul[0][j]=(x_k_ul[1][j-1])*(data.dt/10.0)+x_k_uu[0][j-1];
//add discretized sys
//check if they leave the outer g
g_outer_uu= (x_k_uu[0][j]*x_k_uu[0][j])+(x_k_uu[1][j]*x_k_uu[1][j])-1;
g_outer_lu= (x_k_lu[0][j]*x_k_lu[0][j])+(x_k_lu[1][j]*x_k_lu[1][j])-1;
g_outer_ul= (x_k_ul[0][j]*x_k_ul[0][j])+(x_k_ul[1][j]*x_k_ul[1][j])-1;
g_outer_ll= (x_k_ll[0][j]*x_k_ll[0][j])+(x_k_ll[1][j]*x_k_ll[1][j])-1;
//check if the trajectories reenter the inner g
if (data.ellipseInner) {
g_inner_uu= (x_k_uu[0][j]*x_k_uu[0][j])/0.81+(x_k_uu[1][j]*x_k_uu[1][j])/(0.4*0.4)-1;
g_inner_lu= (x_k_lu[0][j]*x_k_lu[0][j])/0.81+(x_k_lu[1][j]*x_k_lu[1][j])/(0.4*0.4)-1;
g_inner_ul= (x_k_ul[0][j]*x_k_ul[0][j])/0.81+(x_k_ul[1][j]*x_k_ul[1][j])/(0.4*0.4)-1;
g_inner_ll= (x_k_ll[0][j]*x_k_ll[0][j])/0.81+(x_k_ll[1][j]*x_k_ll[1][j])/(0.4*0.4)-1;
}
else{
g_inner_uu= (x_k_uu[0][j]*x_k_uu[0][j])/(0.99*0.99)+(x_k_uu[1][j]*x_k_uu[1][j])/(0.96*0.96)-1;
g_inner_lu= (x_k_lu[0][j]*x_k_lu[0][j])/(0.99*0.99)+(x_k_lu[1][j]*x_k_lu[1][j])/(0.96*0.96)-1;
g_inner_ul= (x_k_ul[0][j]*x_k_ul[0][j])/(0.99*0.99)+(x_k_ul[1][j]*x_k_ul[1][j])/(0.96*0.96)-1;
g_inner_ll= (x_k_ll[0][j]*x_k_ll[0][j])/(0.99*0.99)+(x_k_ll[1][j]*x_k_ll[1][j])/(0.96*0.96)-1;
}
double myFutureBox[j][2][2];
double myMaxPos= qMax(qMax(qMax(x_k_uu[0][j],x_k_ll[0][j]),x_k_lu[0][j]),x_k_ul[0][j]);
double myMinPos= qMin(qMin(qMin(x_k_uu[0][j],x_k_ll[0][j]),x_k_lu[0][j]),x_k_ul[0][j]);
double myMaxVel= qMax(qMax(qMax(x_k_uu[1][j],x_k_ll[1][j]),x_k_lu[1][j]),x_k_ul[1][j]);
double myMinVel= qMin(qMin(qMin(x_k_uu[1][j],x_k_ll[1][j]),x_k_lu[1][j]),x_k_ul[1][j]);
myFutureBox[j][0][0]=myMinPos;
myFutureBox[j][0][1]=myMaxPos;
myFutureBox[j][1][0]=myMinVel;
myFutureBox[j][1][1]=myMaxVel;
IntervalVector BoxFuture(data.numVarG, myFutureBox[j]);
data.boxesVertexFuture.append(BoxFuture);
if (true &&(g_outer_uu>=0 || g_outer_lu>=0 || g_outer_ul>=0 || g_outer_ll>=0)){
unsafeBox=true;
data.boxesUnsafeOuterG.append(currentBox);
double myUnsafeFutureBox[j][2][2];
for (int k = 0; k < j; ++k) {
myMaxPos= qMax(qMax(qMax(x_k_uu[0][j],x_k_ll[0][j]),x_k_lu[0][j]),x_k_ul[0][j]);
myMinPos= qMin(qMin(qMin(x_k_uu[0][j],x_k_ll[0][j]),x_k_lu[0][j]),x_k_ul[0][j]);
myMaxVel= qMax(qMax(qMax(x_k_uu[1][j],x_k_ll[1][j]),x_k_lu[1][j]),x_k_ul[1][j]);
myMinVel= qMin(qMin(qMin(x_k_uu[1][j],x_k_ll[1][j]),x_k_lu[1][j]),x_k_ul[1][j]);
myUnsafeFutureBox[j][0][0]=myMinPos;
myUnsafeFutureBox[j][0][1]=myMaxPos;
myUnsafeFutureBox[j][1][0]=myMinVel;
myUnsafeFutureBox[j][1][1]=myMaxVel;
IntervalVector BoxUnsafeFuture(data.numVarG, myUnsafeFutureBox[k]);
data.boxesUnsafeOuterGFuture.append(BoxUnsafeFuture);
// cout<<"Unsafe vertex box "<<BoxUnsafeFuture<<endl;
}
break;
}
if (j>20 && g_inner_uu<0 && g_inner_lu<0 && g_inner_ul<0 && g_inner_ll<0){
safeBox=true;
//data.boxesUnsafeOuterG.append(currentBox);
double mySafeFutureBox[j][2][2];
for (int k = 0; k < j; ++k) {
myMaxPos= qMax(qMax(qMax(x_k_uu[0][j],x_k_ll[0][j]),x_k_lu[0][j]),x_k_ul[0][j]);
myMinPos= qMin(qMin(qMin(x_k_uu[0][j],x_k_ll[0][j]),x_k_lu[0][j]),x_k_ul[0][j]);
myMaxVel= qMax(qMax(qMax(x_k_uu[1][j],x_k_ll[1][j]),x_k_lu[1][j]),x_k_ul[1][j]);
myMinVel= qMin(qMin(qMin(x_k_uu[1][j],x_k_ll[1][j]),x_k_lu[1][j]),x_k_ul[1][j]);
mySafeFutureBox[j][0][0]=myMinPos;
mySafeFutureBox[j][0][1]=myMaxPos;
mySafeFutureBox[j][1][0]=myMinVel;
mySafeFutureBox[j][1][1]=myMaxVel;
IntervalVector BoxSafeFuture(data.numVarG, mySafeFutureBox[k]);
data.boxesGuaranteedIntegrationUnsafe.append(BoxSafeFuture);
// cout<<"Safe vertex box "<<BoxSafeFuture<<endl;
}
break;
}
}
if (unsafeBox==false && safeBox==true){
data.boxesUncertain.append(currentBox);
}
}
}
for (int i = 0; i < data.boxesUncertain.size(); ++i) { //process uncertain boxes
IntervalVector uncertainBox=data.boxesUncertain.at(i);
Variable y(data.numVarF);
// Initial conditions
IntervalVector yinit(data.numVarF);
for (int j = 0; j < data.numVarF; ++j) {
yinit[j] = uncertainBox[j];
cout<<uncertainBox[j]<<endl;
}
Function ydot = Function (y,Return (y[1],-1*sin(y[0])-0.15*y[1])); // Ivp contruction (initial time is 0.0)
QTime t1;
t1.start();
ivp_ode problem = ivp_ode (ydot, 0.0 , yinit);
// Simulation construction and run
int prevSize=data.boxesUncertainFuture.size();
simulation simu = simulation (&problem,data.dt*data.numFuturePos, __METH__, __PREC__); //uses Runge-kutta4 method
data.boxesUncertainFuture.append(simu.run_simulation()); //modified ibex_simulation.h to make it return a list with all the solutions, not just the last one
int afterSize=data.boxesUncertainFuture.size();
for (int k = 0; k < (afterSize-prevSize); ++k) {
data.boxesUncertainFutureIndex.append(i);
}
double timeSiviaCalculations1=t1.elapsed()/1000.0;
double timeSiviaCalculationsTotal=tSivia.elapsed()/1000.0;
cout<<endl<<"Unsafe # "<<i<<" , Box time = "<<timeSiviaCalculations1<<" , Total elapsed time = "<<timeSiviaCalculationsTotal<<endl;
}
for (int j = 0; j < data.boxesUncertainFuture.size(); ++j) {
IntervalVector myBox=data.boxesUncertainFuture.at(j);
bool testInsideOuterG = true;
Function g_outer("g_outer.txt");
IntervalVector gg=g_outer.eval_vector(myBox);
for(int j = 0; j<gg.size(); j++){
testInsideOuterG = (testInsideOuterG && (gg[j].ub()<0));
}
if (testInsideOuterG==false){
data.boxesUnsafeOuterGFuture.append(myBox);
data.boxesUnsafeOuterG.append(data.boxesUncertain.at(data.boxesUncertainFutureIndex.at(j)));
}
else{
data.boxesSafeOuterGFuture.append(myBox);
}
}
}
}
BoolInterval PdcHansenFeasibility::test(const IntervalVector& box) {
int n=f.nb_var();
int m=f.image_dim();
IntervalVector mid=box.mid();
/* Determine the "most influencing" variable thanks to
* the pivoting of Gauss elimination */
// ==============================================================
Matrix A=f.jacobian(mid).mid();
Matrix LU(m,n);
int *pr = new int[m];
int *pc = new int[n]; // the interesting output: the variables permutation
BoolInterval res=MAYBE;
try {
real_LU(A,LU,pr,pc);
} catch(SingularMatrixException&) {
// means in particular that we could not extract an
// invertible m*m submatrix
delete [] pr;
delete [] pc;
return MAYBE;
}
// ==============================================================
// PartialFnc pf(f,pc,m,mid);
BitSet _vars=BitSet::empty(n);
for (int i=0; i<m; i++) _vars.add(pc[i]);
VarSet vars(f.nb_var(),_vars);
IntervalVector box2(box);
// fix parameters to their midpoint
vars.set_param_box(box2, vars.param_box(box).mid());
IntervalVector savebox(box2);
if (inflating) {
if (inflating_newton(f,vars,box2)) {
_solution = box2;
res = YES;
} else {
_solution.set_empty();
}
}
else {
// ****** TODO **********
newton(f,vars,box2);
if (box2.is_empty()) {
_solution.set_empty();
} else if (box2.is_strict_subset(savebox)) {
_solution = box2;
res = YES;
}
}
delete [] pr;
delete [] pc;
return res;
}
void Optimizer::optimize(const IntervalVector& init_box) {
buffer.flush();
Cell* root=new Cell(IntervalVector(n+1));
write_ext_box(init_box,root->box);
// add data required by the bisector
bsc.add_backtrackable(*root);
// add data required by optimizer + Fritz John contractor
root->add<EntailedCtr>();
//root->add<Multipliers>();
entailed=&root->get<EntailedCtr>();
entailed->init_root(user_sys,sys);
loup_changed=false;
loup_point=init_box.mid();
time=0;
Timer::start();
handle_cell(*root,init_box);
try {
while (!buffer.empty()) {
loup_changed=false;
if (trace >= 2) cout << ((CellBuffer&) buffer) << endl;
Cell* c=buffer.top();
// cout << " box before bisection " << c->box << endl;
try {
pair<IntervalVector,IntervalVector> boxes=bsc.bisect(*c);
pair<Cell*,Cell*> new_cells=c->bisect(boxes.first,boxes.second);
delete buffer.pop();
handle_cell(*new_cells.first, init_box);
handle_cell(*new_cells.second, init_box);
if (uplo_of_epsboxes == NEG_INFINITY) {
cout << " possible infinite minimum " << endl;
break;
}
if (loup_changed ) {
// In case of a new upper bound (loup_changed == true), all the boxes
// with a lower bound greater than (loup - goal_prec) are removed and deleted.
// Note: if contraction was before bisection, we could have the problem
// that the current cell is removed by contract_heap. See comments in
// older version of the code (before revision 284).
double ymax= compute_ymax();
buffer.contract_heap(ymax);
if (ymax <=NEG_INFINITY) {
if (trace) cout << " infinite value for the minimum " << endl;
break;
}
if (trace) cout << setprecision(12) << "ymax=" << ymax << " uplo= " << uplo<< endl;
}
update_uplo();
time_limit_check();
}
catch (NoBisectableVariableException& ) {
bool bb=false;
for (int i=0;(!bb)&&( i<(c->box).size()); i++) {
if (i!=ext_sys.goal_var()) // skip goal variable
bb=bb||(c->box)[i].is_unbounded();
}
if (!bb) {
// rem4: this case can append if the interval [1.79769e+308,inf] is in c.box.
// It is only numerical degenerated case
update_uplo_of_epsboxes ((c->box)[ext_sys.goal_var()].lb());
}
delete buffer.pop();
}
}
}
catch (TimeOutException& ) {
return;
}
Timer::stop();
time+= Timer::VIRTUAL_TIMELAPSE();
}
