void MapLocalizer::contractSegment(Interval& x, Interval& y, Wall& wall) {
IntervalVector tmp(6);
tmp[0] = x;
tmp[1] = y;
tmp[2] = Interval(wall[0]);
tmp[3] = Interval(wall[1]);
tmp[4] = Interval(wall[2]);
tmp[5] = Interval(wall[3]);
this->ctcSegment.contract(tmp);
x &= tmp[0];
y &= tmp[1];
if (x.is_empty() || y.is_empty()) {
x.set_empty();
y.set_empty();
}
}
void MapLocalizer::contractOneMeasurment(Interval&x, Interval&y, Interval& rho, Interval& theta, Wall& wall) {
Interval ax = rho * cos(theta);
Interval ay = rho * sin(theta);
Interval qx = x + ax;
Interval qy = y + ay;
contractSegment(qx, qy, wall);
bwd_add(qx, x, ax);
bwd_add(qy, y, ay);
if (x.is_empty() || y.is_empty()) {
x.set_empty();
y.set_empty();
}
}
bool Interval::div2_inter(const Interval& num, const Interval& div, Interval& out2) {
// warning: we may have &num==this
Interval out1,_out2;
div2(num,div,out1,_out2);
out1 &= *this;
if (out1.is_empty()) {
*this &= _out2;
out2.set_empty();
return !this->is_empty();
} else {
out2 = *this & _out2;
*this = out1;
return true;
}
}
void div2(const Interval& num, const Interval& div, Interval& out1, Interval& out2) {
if (num.is_empty() || div.is_empty()) {
out1.set_empty();
out2.set_empty();
return;
}
const double& a(num.lb());
const double& b(num.ub());
const double& c(div.lb());
const double& d(div.ub());
// notice : we do not consider 0/0=0 but 0/0=emptyset
if (c==0 && d==0) {
out1.set_empty();
out2.set_empty();
return;
}
if (a==0 && b==0) {
out1 = num;
out2.set_empty();
return;
}
if (c>0 || d<0) {
out1 = num/div;
out2.set_empty();
return;
}
if (b<=0 && d==0) {
if (c==NEG_INFINITY)
out1 = Interval::POS_REALS;
else
out1 = Interval(INF_DIV(b,c), POS_INFINITY);
out2.set_empty();
return;
}
if (b<=0 && c<0 && d>0) {
if (b==0 || (c==NEG_INFINITY && d==POS_INFINITY)) {
out1 = Interval::ALL_REALS;
out2.set_empty();
return;
} else {
out1 = Interval(NEG_INFINITY, d==POS_INFINITY? 0 : SUP_DIV(b,d));
out2 = Interval(c==NEG_INFINITY? 0 : INF_DIV(b,c), POS_INFINITY);
return;
}
}
if (b<=0 && c==0) {
if (d==POS_INFINITY)
out1 = Interval::NEG_REALS;
else
out1 = Interval(NEG_INFINITY, SUP_DIV(b,d));
out2.set_empty();
return;
}
if (a>=0 && d==0) {
if (c==NEG_INFINITY)
out1 = Interval::NEG_REALS;
else
out1 = Interval(NEG_INFINITY, SUP_DIV(a,c));
out2.set_empty();
return;
}
if (a>=0 && c<0 && d>0) {
if (a==0 || (c==NEG_INFINITY && d==POS_INFINITY)) {
out1 = Interval::ALL_REALS;
out2.set_empty();
return;
} else {
out1 = Interval(NEG_INFINITY, c==NEG_INFINITY? 0 : SUP_DIV(a,c));
out2 = Interval(d==POS_INFINITY? 0 : INF_DIV(a,d), POS_INFINITY);
return;
}
}
if (a>=0 && c==0) {
if (d==POS_INFINITY)
out1 = Interval::POS_REALS;
else
out1 = Interval(INF_DIV(a,d), POS_INFINITY);
out2.set_empty();
return;
}
out1 = Interval::ALL_REALS;
out2.set_empty();
}