graph_model.cpp

Go to the documentation of this file.

#include <graph_model.h>
#include <iostream>
#include <frechet/reachability/graph_model.h>

using namespace frechet;
using namespace reach;
using namespace data;

IndexRange::IndexRange()
: IndexRange(HORIZONTAL, 0,0)
{}

IndexRange::IndexRange(Orientation anori, int l, int u)
: ori(anori), lower(l),upper(u)
{ }

bool IndexRange::empty() const { return upper <= lower; }
void IndexRange::clear() { lower=upper=0; }

int IndexRange::len() const {
    Q_ASSERT(upper>=lower);
    return upper-lower;
}

bool IndexRange::contains(int i) const {
    return i>=lower && i < upper;
}

bool IndexRange::contains(const IndexRange& that) const
{
    Q_ASSERT(ori==that.ori);
    return (that.lower >= this->lower) && (that.upper <= this->upper);
}

IndexRange IndexRange::operator& (const IndexRange& b) const
{
    Q_ASSERT(ori==b.ori);
    return IndexRange(ori,
            std::max(lower,b.lower),
            std::min(upper,b.upper));
}

IndexRange& IndexRange::operator&= (const IndexRange& b)
{
    Q_ASSERT(ori==b.ori);
    lower = std::max(lower,b.lower);
    upper = std::min(upper,b.upper);
    return *this;
}

bool IndexRange::intersects(const IndexRange& b) const
{
    return (ori==b.ori)
            && (upper > b.lower)
            && (lower < b.upper);
}

IndexRange IndexRange::operator+ (const IndexRange& b) const
{
    Q_ASSERT(this->ori==b.ori);
    Q_ASSERT(this->upper==b.lower || b.upper==this->lower);
    return IndexRange(ori,
            std::min(lower,b.lower),
            std::max(upper,b.upper));
}

bool IndexRange::operator==(const IndexRange& that) const
{
    return ori==that.ori
            && lower==that.lower
            && upper==that.upper;
}

bool IndexRange::operator!=(const IndexRange& that) const
{
    return ! operator==(that);
}

IndexRange IndexRange::operator+ (int offset) const
{
    return IndexRange(ori,lower+offset,upper+offset);
}

IndexRange IndexRange::operator- (int offset) const
{
    return IndexRange(ori,lower-offset,upper-offset);
}

IndexRange& IndexRange::operator+= (const IndexRange& b) {
    Q_ASSERT(this->ori==b.ori);
    Q_ASSERT(this->upper==b.lower || b.upper==this->lower);
    lower = std::min(lower,b.lower);
    upper = std::max(upper,b.upper);
    return *this;
}

IndexRange &IndexRange::operator-=(int offset)
{
    lower -= offset;
    upper -= offset;
    return *this;
}

Bias reach::operator| (Bias a, Bias b) {
    return (Bias)((uint8_t)a | (uint8_t)b);
}

Bias& reach::operator|= (Bias& a, Bias b) {
    a = (a|b);
    return a;
}

GraphModelAxis::GraphModelAxis() : map(),_max_index(0),_dim(0) { }

void GraphModelAxis::insert1(double x, Bias bias)
{
    Map::iterator i = map.find(x);
    if (i==map.end()) {
        map.insert(std::make_pair(x,BoundsIndex(-1,bias)));
    }
    else {
        i->second.bias |= bias;
    }
}

void GraphModelAxis::insert2(double x, Bias bias)
{
    insert1(x,bias);
    insert1(x+_dim,bias);
}

void GraphModelAxis::remove(double x)
{
    map.erase(x);
}

const BoundsIndex& GraphModelAxis::lookup(double x) const
{
    //Q_ASSERT(x >= 0.0 && x <= _dim);
    Map::const_iterator i = map.find(x);
    Q_ASSERT(i != map.end());
    return i->second;
}

const BoundsIndex* GraphModelAxis::lookupLowerEqual(double x) const
{
    Map::const_iterator i = map.lower_bound(x); //  *i >= x
    if (i==map.end())
        return nullptr;
    Q_ASSERT(i != map.end());
    Q_ASSERT(i->first >= x);
    if (i->first==x)
        return &i->second;
    --i;
    if (i==map.end())
        return nullptr;
    Q_ASSERT(i->first < x);
    return &i->second;
}

const BoundsIndex* GraphModelAxis::lookupLarger(double x) const
{
    Map::const_iterator i = map.upper_bound(x); //  *i > x
    if (i==map.end())
        return nullptr;
    Q_ASSERT(i != map.end());
    Q_ASSERT(i->first > x);
    return &i->second;
}

void GraphModelAxis::indexify(int start) {
    _max_index = start;
    for(Map::iterator i=map.begin(); i != map.end(); ++i)
    {
        i->second.index = _max_index++;
        if (i->second.bias==LOWER_UPPER) _max_index++;
        //  Single-Value Intervals [x,x] need to be indexed separately.
        //  Reserve an index for them.
    }
}

void GraphModelAxis::createReverseMap(std::vector<Interval>& result) const
{
    if (map.empty()) return;

    Map::const_iterator i=map.begin();
    Map::const_iterator j=i;
    ++j;

    result.reserve(map.size());
    for( ; j != map.end(); ++i,++j)
    {
        Q_ASSERT(i->second.index==result.size());
        if (i->second.bias==LOWER_UPPER)
            result.push_back(Interval(i->first,i->first));
        if (j != map.end())
            result.push_back(Interval(i->first,j->first));
    }
    Q_ASSERT(i != map.end());
    if (i->second.bias==LOWER_UPPER)
        result.push_back(Interval(i->first,i->first));

    result.push_back(Interval(result.back().upper(),_dim));
}


int GraphModel::dim1(Orientation ori) const {
    switch(ori) {
    case HORIZONTAL:    //Q_ASSERT((axis[HORIZONTAL]._dim % 2) == 0);
                        return axis[HORIZONTAL]._dim;
    case VERTICAL:      return axis[VERTICAL]._dim;
    }
    Q_ASSERT(false); // don't come here
    return -1;
}

int GraphModel::dim2(Orientation ori) const {
    switch(ori) {
    case HORIZONTAL:    return 2*axis[HORIZONTAL]._dim;
    case VERTICAL:      return axis[VERTICAL]._dim;
    }
    Q_ASSERT(false); // don't come here
    return -1;
}


int GraphModel::count2(Orientation ori) const
{
    switch(ori) {
    case HORIZONTAL:    return axis[HORIZONTAL]._max_index;
    case VERTICAL:      return axis[VERTICAL]._max_index;
    }
    Q_ASSERT(false); // don't come here
    return -1;
}

int GraphModel::count2() const
{
    return count2(HORIZONTAL) + count2(VERTICAL);
}
/*
BoundsIndex GraphModel::lookup(Orientation ori, double x) const
{
    Q_ASSERT(x >= 0.0);
    Q_ASSERT(x <= dim2(ori));
    return axis[ori].lookup(x);
}
*/
IndexRange GraphModel::map(
        Orientation ori,
        const Interval& ival,
        bool upperBoundInclusive) const
{
    //  Lookup lower and upper bounds
    BoundsIndex low = axis[ori].lookup(ival.lower());
    if (ival.lower()==ival.upper()) {
        //  Single-point interval; it must be present in the map
        Q_ASSERT(low.bias==LOWER_UPPER);
        if (upperBoundInclusive)
            return IndexRange(ori, low.index,low.index+1);
        else
            return IndexRange(ori, low.index,low.index);
    }
    //  else
    //Q_ASSERT(ival.upper() <= 2*axis[ori]._dim);

    if (ori==HORIZONTAL && ival.upper() == 2*axis[ori]._dim) {
        return IndexRange(ori, low.index, count2(ori));
    }
    else {
        BoundsIndex upp = axis[ori].lookup(ival.upper());
        if ((upp.bias==LOWER_UPPER) && upperBoundInclusive)
            return IndexRange(ori, low.index,upp.index+1);
        else
            return IndexRange(ori, low.index,upp.index);
    }
}

IndexRange
GraphModel::mapClosest(
        Orientation ori,
        const Interval &ival) const
{
    const BoundsIndex* low = axis[ori].lookupLowerEqual(ival.lower());
    const BoundsIndex* upp = axis[ori].lookupLarger(ival.upper());

    if (upp && upp->bias==LOWER_UPPER)
        return IndexRange(ori, low ? low->index:0, upp->index+1);
    else if (upp)
        return IndexRange(ori, low ? low->index:0, upp->index);
    else
        return IndexRange(ori, low ? low->index:0, count2(ori));
}

IndexRange GraphModel::map(Pointer b) const
{
    Q_ASSERT(b);
    return map(b->ori, *b, true);
}

int GraphModel::map_lower(Pointer p) const
{
    return map_lower(p->ori, p->lower());
}

int GraphModel::map_upper(Pointer p) const
{
    return map_upper(p->ori, p->upper(), true);
}

int GraphModel::map_lower(Orientation ori, double x) const
{
    BoundsIndex low = axis[ori].lookup(x);
    return low.index;
}

int GraphModel::map_upper(Orientation ori, double x, bool inclusive) const
{
    if (ori==HORIZONTAL && x==2*axis[ori]._dim)
        return count2(HORIZONTAL);

    BoundsIndex upp = axis[ori].lookup(x);
    if (inclusive && upp.bias==LOWER_UPPER)
        return upp.index+1;
    else
        return upp.index;
}

IndexRange GraphModel::mergedHMask(const IndexRange& ma, const IndexRange& mb) const
{
    //int h1 = graphModel()->count1(HORIZONTAL);
    Q_ASSERT(ma.ori==HORIZONTAL && mb.ori==HORIZONTAL);
    //Q_ASSERT((ma.upper==mb.lower) || (ma.upper==mb.lower+h1));

    if (ma.upper==mb.lower)
        return ma + mb;
    else {
        IndexRange mb2 = this->shiftedHorizontally(mb);
        return ma + mb2;
    }
}

void GraphModel::init(const FreeSpace::ptr fs)
{
    initHorizontal(fs);
    initVertical(fs);

    //  GraphModel represents a DOUBLE Free-Space
    //  The Horizontal axis is be duplicated (@see lookup).

    axis[HORIZONTAL].indexify(0);
    axis[VERTICAL].indexify(0);

    //hcount1 = map_lower(HORIZONTAL,dim1(HORIZONTAL));

 //   Q_ASSERT(count2(HORIZONTAL)%2==0);
//    Q_ASSERT(map_lower(HORIZONTAL,dim1(HORIZONTAL))==count1(HORIZONTAL));

    axis[HORIZONTAL].createReverseMap(reversed[HORIZONTAL]);
    axis[VERTICAL].createReverseMap(reversed[VERTICAL]);
}

bool GraphModel::empty() const {
    return count2(HORIZONTAL)==0 || count2(VERTICAL)==0;
}

void GraphModel::initVertical(const FreeSpace::ptr fs)
{
    GraphModelAxis& ax = axis[VERTICAL];
    ax._dim = fs->m-1;
    for(int i=0; i < fs->n; ++i)
    {
        const Interval& L = fs->cell(i,0).L;
        bool inside = (L.lower()==0.0);
        if (inside) {
            ax.insert1(0.0, LOWER);
        }

        for(int j=0; j < fs->m-1; ++j)
        {
            const Interval& L = fs->cell(i,j).L;
            const Interval& L2 = fs->cell(i,j+1).L;

            if (inside) {
                //  integer boundaries
                ax.insert1((double)j,LOWER_UPPER);
            }

            if (!L) {
                inside=false;
            }
            else if (L.lower() > 0.0) {
                ax.insert1(L.lower() + (double)j,
                          (L.lower()==L.upper()) ? LOWER_UPPER:LOWER);
                inside = (L.upper() >= 1.0) && (L2.lower()==0.0);
                if (!inside) ax.insert1(L.upper() + (double)j,UPPER);
            }
            else if (inside) {
                Q_ASSERT(L.lower()==0.0);
                inside = (L.upper() >= 1.0) && (L2.lower()==0.0);
                if (!inside) ax.insert1(L.upper() + (double)j,UPPER);
            }
        }

        if (inside) {
            ax.insert1((double)(fs->m-1),UPPER);
        }
    }
}

void GraphModel::initHorizontal(const FreeSpace::ptr fs)
{
    GraphModelAxis& ax = axis[HORIZONTAL];
    ax._dim = fs->n-1;
    for(int j=0; j < fs->m; ++j)
    {
        const Interval& B = fs->cell(0,j).B;
        bool inside = (B.lower()==0.0);
        if (inside) {
            ax.insert2(0.0, LOWER);
        }

        for(int i=0; i < fs->n-1; ++i)
        {
            const Interval& B = fs->cell(i,j).B;

            if (true) {
                //  integer boundaries
                ax.insert2((double)i,LOWER_UPPER);
            }

            if (!B) {
                inside=false;
            }
            else if (B.lower() > 0.0) {
                ax.insert2(B.lower() + (double)i,
                          (B.lower()==B.upper()) ? LOWER_UPPER:LOWER);
                inside = (B.upper() >= 1.0) && (i+1 < fs->n) && (fs->cell(i+1,j).B.lower()==0.0);
                if (!inside) ax.insert2(B.upper() + (double)i,UPPER);
            }
            else if (inside) {
                Q_ASSERT(B.lower()==0.0);
                inside = (B.upper() >= 1.0) && (i+1 < fs->n) && (fs->cell(i+1,j).B.lower()==0.0);
                if (!inside) ax.insert2(B.upper() + (double)i,UPPER);
            }
        }
        ax.remove((double)2*ax._dim);
        //ax.insert2((double)(fs->n-1),LOWER_UPPER);
    }
}