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();
    }
}
Esempio n. 3
0
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;
	}
}
Esempio n. 4
0
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();
}