void TestIbex::check(const Interval& y_actual, const Interval& y_expected) {
//cout << "TestIbex::check: " << y_expected << " (expected) " << y_actual << " (actual)"<< endl;
if (y_expected.is_empty()) { TEST_ASSERT(y_actual.is_empty()); return; }
TEST_ASSERT(!y_actual.is_empty());
TEST_ASSERT(!isnan(y_actual.lb()));
TEST_ASSERT(!isnan(y_actual.ub()));
TEST_ASSERT_DELTA(y_actual.lb(),y_expected.lb(),ERROR);
TEST_ASSERT_DELTA(y_actual.ub(),y_expected.ub(),ERROR);
}
bool TestIbex::almost_eq(const Interval& y_actual, const Interval& y_expected, double err) {
if (y_actual.is_empty() && y_expected.is_empty()) return true;
if (y_actual.lb()==NEG_INFINITY)
if (y_expected.lb()!=NEG_INFINITY) return false;
else;
else if (fabs(y_actual.lb()-y_expected.lb())> err) return false;
if (y_actual.ub()==POS_INFINITY)
if (y_expected.ub()!=POS_INFINITY) return false;
else;
else if (fabs(y_actual.ub()-y_expected.ub())> err) return false;
return true;
}
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();
}
}
double Interval::delta(const Interval& x) const {
if (is_empty()) return 0;
if (x.is_empty()) return diam();
// ** warning **
// checking if *this or x is infinite by
// testing if the lower/upper bounds are -oo/+oo
// is not enough because diam() may return +oo even
// with finite bounds (e.g, very large intervals like [-DBL_MAX,DBL_MAX]).
// ("almost-unboundedness")
volatile double d=diam();
volatile double dx=x.diam();
// furthermore, if these variables are not declared volatile
// conditions like d==POS_INFINITY are evaluated
// to FALSE for intervals like [-DBL_MAX,DBL_MAX] (with -O3 option)
// while the returned expression (d-dx) evaluates to +oo (instead of 0).
if (d==POS_INFINITY) {
//cout << "d=" << d << " dx=" << dx << endl;
if (dx==POS_INFINITY) {
double left=(x.lb()==NEG_INFINITY? 0 : x.lb()-lb());
double right=(x.ub()==POS_INFINITY? 0 : ub()-x.ub());
//cout << "left=" << left << " right=" << right << endl;
return left+right;
} else
return POS_INFINITY;
}
else return d-dx;
}
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();
}
}
int Interval::diff(const Interval& y, Interval& c1, Interval& c2) const {
y.complementary(c1,c2);
c1 &= *this;
int res=2;
if (c1.is_degenerated()) { c1=Interval::EMPTY_SET; res--; }
c2 &= *this;
if (c2.is_degenerated()) { c2=Interval::EMPTY_SET; res--; }
if (c1.is_empty()) {
c1=c2;
c2=Interval::EMPTY_SET;
}
return res;
}
// launch Hansen test
bool Optimizer::update_real_loup() {
IntervalVector epsbox(loup_point);
// ====================================================
// solution #1: we inflate the loup-point and
// call Hansen test in contracting mode.
// TODO: replace default_equ_eps by something else!
// epsbox.inflate(default_equ_eps);
// PdcHansenFeasibility pdc(equs->f, false);
// ====================================================
// ====================================================
// solution #2: we call Hansen test in inflating mode.
PdcHansenFeasibility pdc(equs->f, true);
// ====================================================
// TODO: maybe we should check first if the epsbox is inner...
// otherwise the probability to get a feasible point is
// perhaps too small?
// TODO: HansenFeasibility uses midpoint
// but maybe we should use random
if (pdc.test(epsbox)==YES) {
Interval resI = sys.goal->eval(pdc.solution());
if (!resI.is_empty()) {
double res=resI.ub();
if (res<loup) {
//TODO : in is_inner, we check again all equalities,
// it's useless in this case!
if (is_inner(pdc.solution())) {
loup = res;
loup_box = pdc.solution();
cout << setprecision (12) << " *real* loup update " << loup << " loup box: " << loup_box << endl;
return true;
}
}
}
}
//===========================================================
return false;
}
Interval Tube::operator[](const ibex::Interval& intv_t) const
{
// Write access is not allowed for this operator:
// a further call to computeTree() is needed when values change,
// this call cannot be garanteed with a direct access to m_intv_y
// For write access: use setY()
if(intv_t.lb() == intv_t.ub())
return (*this)[intv_t.lb()];
Interval intersection = m_intv_t & intv_t;
if(intersection.is_empty())
return Interval::EMPTY_SET;
else if(isSlice() || intv_t == m_intv_t || intv_t.is_unbounded() || intv_t.is_superset(m_intv_t))
{
if(m_tree_computation_needed)
computeTree();
return m_intv_y;
}
else
{
Interval inter_firstsubtube = m_first_subtube->getT() & intersection;
Interval inter_secondsubtube = m_second_subtube->getT() & intersection;
if(inter_firstsubtube == inter_secondsubtube)
return (*m_first_subtube)[inter_firstsubtube.lb()] & (*m_second_subtube)[inter_secondsubtube.lb()];
else if(inter_firstsubtube.lb() == inter_firstsubtube.ub()
&& inter_secondsubtube.lb() != inter_secondsubtube.ub())
return (*m_second_subtube)[inter_secondsubtube];
else if(inter_firstsubtube.lb() != inter_firstsubtube.ub()
&& inter_secondsubtube.lb() == inter_secondsubtube.ub())
return (*m_first_subtube)[inter_firstsubtube];
else
return (*m_first_subtube)[inter_firstsubtube] | (*m_second_subtube)[inter_secondsubtube];
}
}
const pair<Interval,Interval> Tube::getEnclosedBounds(const Interval& intv_t) const
{
if(intv_t.lb() == intv_t.ub())
return make_pair(Interval((*this)[intv_t.lb()].lb()), Interval((*this)[intv_t.lb()].ub()));
Interval intersection = m_intv_t & intv_t;
if(intersection.is_empty())
return make_pair(Interval::EMPTY_SET, Interval::EMPTY_SET);
else if(isSlice() || intv_t == m_intv_t || intv_t.is_unbounded() || intv_t.is_superset(m_intv_t))
{
if(m_tree_computation_needed)
computeTree();
return m_enclosed_bounds; // pre-computed
}
else
{
Interval inter_firstsubtube = m_first_subtube->getT() & intersection;
Interval inter_secondsubtube = m_second_subtube->getT() & intersection;
if(inter_firstsubtube.lb() == inter_firstsubtube.ub() && inter_secondsubtube.lb() == inter_secondsubtube.ub())
return make_pair((*m_first_subtube)[inter_firstsubtube.lb()] & (*m_second_subtube)[inter_secondsubtube.lb()],
(*m_first_subtube)[inter_firstsubtube.ub()] & (*m_second_subtube)[inter_secondsubtube.ub()]);
else if(inter_firstsubtube.is_empty() || inter_firstsubtube.lb() == inter_firstsubtube.ub())
return m_second_subtube->getEnclosedBounds(inter_secondsubtube);
else if(inter_secondsubtube.is_empty() || inter_secondsubtube.lb() == inter_secondsubtube.ub())
return m_first_subtube->getEnclosedBounds(inter_firstsubtube);
else
{
pair<Interval,Interval> ui_past = m_first_subtube->getEnclosedBounds(inter_firstsubtube);
pair<Interval,Interval> ui_future = m_second_subtube->getEnclosedBounds(inter_secondsubtube);
return make_pair(ui_past.first | ui_future.first, ui_past.second | ui_future.second);
}
}
}
Affine2Main<AF_iAF>::Affine2Main(int n, int m, const Interval& itv) :
_n (n),
_elt (NULL,0.0)
{
assert((n>=0) && (m>=0) && (m<=n));
if (!(itv.is_unbounded()||itv.is_empty())) {
_elt._val =new Interval[n + 1];
_elt._val[0] = itv.mid();
for (int i = 1; i <= n; i++){
_elt._val[i] = 0.0;
}
if (m == 0) {
_elt._err = itv.rad();
} else {
_elt._val[m] = itv.rad();
}
} else {
*this = itv;
}
}
Affine2Main<AF_iAF>::Affine2Main(const Interval & itv):
_n (0),
_elt (NULL,0.0) {
if (itv.is_empty()) {
_n = -1;
_elt._err = itv;
} else if (itv.ub()>= POS_INFINITY && itv.lb()<= NEG_INFINITY ) {
_n = -2;
_elt._err = itv;
} else if (itv.ub()>= POS_INFINITY ) {
_n = -3;
_elt._err = itv;
} else if (itv.lb()<= NEG_INFINITY ) {
_n = -4;
_elt._err = itv;
} else {
_n = 0;
_elt._val = new Interval[1];
_elt._val[0] = itv.mid();
_elt._err = itv.rad();
}
}
void interval_test() {
Interval e = Interval::everything();
Interval n = Interval::nothing();
Expr x = Variable::make(Int(32), "x");
Interval xp{x, Interval::pos_inf};
Interval xn{Interval::neg_inf, x};
Interval xx{x, x};
internal_assert(e.is_everything());
internal_assert(!e.has_upper_bound());
internal_assert(!e.has_lower_bound());
internal_assert(!e.is_empty());
internal_assert(!e.is_bounded());
internal_assert(!e.is_single_point());
internal_assert(!n.is_everything());
internal_assert(!n.has_upper_bound());
internal_assert(!n.has_lower_bound());
internal_assert(n.is_empty());
internal_assert(!n.is_bounded());
internal_assert(!n.is_single_point());
internal_assert(!xp.is_everything());
internal_assert(!xp.has_upper_bound());
internal_assert(xp.has_lower_bound());
internal_assert(!xp.is_empty());
internal_assert(!xp.is_bounded());
internal_assert(!xp.is_single_point());
internal_assert(!xn.is_everything());
internal_assert(xn.has_upper_bound());
internal_assert(!xn.has_lower_bound());
internal_assert(!xn.is_empty());
internal_assert(!xn.is_bounded());
internal_assert(!xn.is_single_point());
internal_assert(!xx.is_everything());
internal_assert(xx.has_upper_bound());
internal_assert(xx.has_lower_bound());
internal_assert(!xx.is_empty());
internal_assert(xx.is_bounded());
internal_assert(xx.is_single_point());
check(Interval::make_union(xp, xn), e, __LINE__);
check(Interval::make_union(e, xn), e, __LINE__);
check(Interval::make_union(xn, e), e, __LINE__);
check(Interval::make_union(xn, n), xn, __LINE__);
check(Interval::make_union(n, xp), xp, __LINE__);
check(Interval::make_union(xp, xp), xp, __LINE__);
check(Interval::make_intersection(xp, xn), Interval::single_point(x), __LINE__);
check(Interval::make_intersection(e, xn), xn, __LINE__);
check(Interval::make_intersection(xn, e), xn, __LINE__);
check(Interval::make_intersection(xn, n), n, __LINE__);
check(Interval::make_intersection(n, xp), n, __LINE__);
check(Interval::make_intersection(xp, xp), xp, __LINE__);
check(Interval::make_union({3, Interval::pos_inf}, {5, Interval::pos_inf}), {3, Interval::pos_inf}, __LINE__);
check(Interval::make_intersection({3, Interval::pos_inf}, {5, Interval::pos_inf}), {5, Interval::pos_inf}, __LINE__);
check(Interval::make_union({Interval::neg_inf, 3}, {Interval::neg_inf, 5}), {Interval::neg_inf, 5}, __LINE__);
check(Interval::make_intersection({Interval::neg_inf, 3}, {Interval::neg_inf, 5}), {Interval::neg_inf, 3}, __LINE__);
check(Interval::make_union({3, 4}, {9, 10}), {3, 10}, __LINE__);
check(Interval::make_intersection({3, 4}, {9, 10}), {9, 4}, __LINE__);
check(Interval::make_union({3, 9}, {4, 10}), {3, 10}, __LINE__);
check(Interval::make_intersection({3, 9}, {4, 10}), {4, 9}, __LINE__);
std::cout << "Interval test passed" << std::endl;
}
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();
}