void contract(IntervalVector& box_x) {
// Create a CtcExist contractor on-the-fly
// and set the splitting precision on y
// to one tenth of the maximal diameter of x.
// The box_x is then contracted
CtcExist(c,vars,box_y,box_x.max_diam()/10).contract(box_x);
}
void CtcPolytopeHull::contract(IntervalVector& box) {
if (!(limit_diam_box.contains(box.max_diam()))) return;
// is it necessary? YES (BNE) Soplex can give false infeasible results with large numbers
// cout << " box before LR " << box << endl;
try {
// Update the bounds the variables
mylinearsolver->initBoundVar(box);
//returns the number of constraints in the linearized system
int cont = lr.linearization(box,mylinearsolver);
if(cont<1) return;
optimizer(box);
// mylinearsolver->writeFile("LP.lp");
// system ("cat LP.lp");
// cout << " box after LR " << box << endl;
mylinearsolver->cleanConst();
}
catch(EmptyBoxException&) {
box.set_empty(); // empty the box before exiting in case of EmptyBoxException
mylinearsolver->cleanConst();
throw EmptyBoxException();
}
}
void CtcNewton::contract(IntervalVector& box) {
if (!(box.max_diam()<=ceil)) return;
else newton(f,box,prec,gauss_seidel_ratio);
if (box.is_empty()) {
set_flag(FIXPOINT);
}
}
void CtcNewton::contract(IntervalVector& box, ContractContext& context) {
if (!(box.max_diam()<=ceil)) return;
else {
if (!vars)
newton(f,box,prec,gauss_seidel_ratio);
else
newton(f,*vars,box,prec,gauss_seidel_ratio);
}
if (box.is_empty()) {
context.output_flags.add(FIXPOINT);
}
}
Sivia::Sivia(repere& R,struct sivia_struct *my_struct) : R(R) {
my_struct->areain = 0;
my_struct->areap = 0;
my_struct->isinside=0;
Variable xvar,yvar,zvar,tvar;
int n = my_struct->nb_beacon;
double *x=my_struct->x; // vecteur des abcisses des donnees
double *y=my_struct->y; // vecteur des ordonnees des donnees
double *z=my_struct->z;
double *r=new double[n]; // vecteur des rayons
double xr=my_struct->robot_position[0],yr=my_struct->robot_position[1],zr=my_struct->robot_position[2];
for (int i=0;i<n;i++) {
// r[i]= sqrt(pow(xr-x[i],2)+pow(yr-y[i],2)+pow(zr-z[i],2));
r[i]= sqrt(pow(xr-x[i],2)+pow(yr-y[i],2)+pow(zr-z[i],2));
if (my_struct->outliers[i]!=0)
r[i] *= (1+my_struct->outliers[i]*my_struct->erroutlier/100);
}
vector<Function*> f;
double th1[n];
double th2[n];
for (int i=0;i<n;i++){
th1[i] = my_struct->theta_sonar[i];
th2[i] = th1[i] + 20;
}
for(int i=0;i<n;i++) {
// f.push_back(new Function(xvar,yvar,zvar,sqrt(sqr(xvar-x[i])+sqr(yvar-y[i])+sqr(zvar-z[i]))));
f.push_back(new Function(xvar,yvar,zvar,sqrt(sqr(xvar-Interval(x[i]-my_struct->beacon_interval*r[i]/100,x[i]+my_struct->beacon_interval*r[i]/100))
+sqr(yvar-Interval(y[i]-my_struct->beacon_interval*r[i]/100,y[i]+my_struct->beacon_interval*r[i]/100))
+sqr(zvar-Interval(z[i]-my_struct->beacon_interval*r[i]/100,z[i]+my_struct->beacon_interval*r[i]/100)))));
}
vector<Ctc*> vec_out;
vector<Ctc*> vec_in;
for(int i=0;i<n;i++) {
vec_out.push_back(new CtcIn(*(f[i]),(r[i]+Interval(-1,1)*my_struct->err[i])));
vec_in.push_back(new CtcNotIn(*(f[i]),(r[i]+Interval(-1,1)*my_struct->err[i])));
}
double re=0.5;
int maxq = my_struct->nb_beacon; //nb of contractors
int ctcq = maxq - my_struct->q + 1; //nb for q-relaxed function of Ibex
int ninbox = 0;
CtcQInter insidetmp(vec_in,ctcq);
CtcQInter outsidetmp(vec_out,my_struct->q);
CtcFixPoint inside(insidetmp);
CtcFixPoint outside(outsidetmp);
IntervalVector box = my_struct->box.back();
// IntervalVector box(3);box[0]=box[1]=Interval(-25,25);box[2]=Interval(0,2);
IntervalVector viinside(3);
//vin.resize(4);
LargestFirst lf;
stack<IntervalVector> s;
s.push(box);
my_struct->in_perhaps=0;
while (!s.empty()) {
IntervalVector box=s.top();
s.pop();
contract_and_draw(inside,box,viinside,1,my_struct,ninbox,Qt::magenta,Qt::red);
if (box.is_empty()) { continue; }
contract_and_draw(outside,box,viinside,0,my_struct,ninbox,Qt::darkBlue,Qt::cyan);
if (box.is_empty()) { continue; }
if (box.max_diam()<my_struct->epsilon_sivia) {
//R.DrawBox(box[0].lb(),box[0].ub(),box[1].lb(),box[1].ub(),QPen(Qt::yellow),QBrush(Qt::white));
my_struct->areap += box[0].diam()*box[1].diam()*box[2].diam();
my_struct->in_perhaps=1;
} else {
pair<IntervalVector,IntervalVector> boxes=lf.bisect(box);
s.push(boxes.first);
s.push(boxes.second);
}
}
double tx[ninbox],ty[ninbox],tz[ninbox];
//cout<<"next"<<ninbox<<endl;
for(int i=0;i<ninbox;i++){
IntervalVector cur = (my_struct->vin.back());
my_struct->vin_prev.push_back(cur);
//cout<<cur<<endl;
Interval xcur=cur[0];
Interval ycur=cur[1];
Interval zcur=cur[2];
tx[i]=xcur.mid();
ty[i]=ycur.mid();
tz[i]=zcur.mid();
my_struct->vin.pop_back();
}
double xin=0,yin=0,zin=0;
for(int i=0;i<ninbox;i++){
xin += tx[i];
yin += ty[i];
zin += tz[i];
}
xin/=double(ninbox);
yin/=double(ninbox);
zin/=double(ninbox);
my_struct->robot_position_found[0] = xin;
my_struct->robot_position_found[1] = yin;
my_struct->robot_position_found[2] = zin;
for(int i=0;i<n;i++)
R.DrawEllipse(x[i],y[i],re,QPen(Qt::black),QBrush(Qt::NoBrush));
my_struct->vin.clear();
vec_out.clear();
vec_in.clear();
f.clear();
}
