freespace.cpp

Go to the documentation of this file.

#include <freespace.h>
#include <numeric.h>
#include <tbb/tbb.h>

using namespace frechet;
using namespace data;
using namespace numeric;
using namespace fs;

const QRectF FreeSpace::UNIT_RECT(0,0,1,1);
const double FreeSpace::DET_MINIMUM = 1e-12;

FreeSpace::FreeSpace(const Curve &ap, const Curve &aq)
    :   P(ap), Q(aq),
        n(P.length()), m(Q.length()),       
        cells(n,m),
        _components(n,m)
{ }


FreeSpace::FreeSpace(const FreeSpace& that)
        :   P(that.P), Q(that.Q),
            n(that.P.length()), m(that.Q.length()),
            cells(that.cells),
            _components(that._components)
{ }

void FreeSpace::calculateFreeSpace(double epsilon)
{
    //  inner cells
    tbb::static_partitioner sp;
    tbb::parallel_for(0, n-1, [&] (int i) 
    {
        const Point& p1 = P[i];
        const Point& p2 = P[i+1];

        for(int j=0; j < (m-1); ++j)
        {
            //  Calculate L,B
            const Point& q1 = Q[j];
            const Point& q2 = Q[j+1];

            Cell& cl = cell(i,j);
            calculateFreeSpaceSegment<accurate_float>(
                    p1, q1,q2,
                    epsilon,
                    cl.L);

            calculateFreeSpaceSegment<accurate_float>(
                    q1, p1,p2,
                    epsilon,
                    cl.B);
        }
    }, sp);

    //  rightmost column
    if (n > 0)
    {
        int i = n-1;
        const Point& p1 = P[i];
        for(int j=0; j < (m-1); ++j)
        {
            //  Calculate L
            Cell& cl = cell(i,j);
            calculateFreeSpaceSegment<accurate_float>(
                    p1, Q[j],Q[j+1],
                    epsilon,
                    cl.L);
        }
    }

    //  topmost row
    if (m > 0)
    {
        int j = m-1;
        const Point& q1 = Q[j];
        for(int i=0; i < (n-1); ++i)
        {
            //  Calculate B
            Cell& cl = cell(i,j);
            calculateFreeSpaceSegment<accurate_float>(
                    q1, P[i],P[i+1],
                    epsilon,
                    cl.B);
        }
    }

    //
    //  Fix border cases
    //

    tbb::parallel_for (0, n - 1, [&] (int i)
    {
        for (int j = 0; j <= (m - 1); ++j) {
            Cell& cl = cell(i, j);
            fixBorderCase(
                (j < (m-1)) ? &cl.L : nullptr, 
                (i < (n-1)) ? &cl.B : nullptr,
                (j>0) ? &cell(i, j - 1).L : nullptr,
                (i>0) ? &cell(i - 1, j).B : nullptr);
        }
    }, sp);
}

void FreeSpace::calculateBounds(double epsilon)
{
    int i,j;
    for(i=0; i < (n-1); ++i)
    {
        for(j=0; j < (m-1); ++j)
        {
            Cell& cl = cell(i,j);
            const Interval& L = cl.L;
            const Interval& B = cell(i,j).B;
            const Interval& R = cell(i+1,j).L;  //  neighbor to the right
            const Interval& T = cell(i,j+1).B;  //  neighbor at top

            Interval& H = cl.bounds.H;
            Interval& V = cl.bounds.V;

            if (L)
                H.setLower(0.0);      //  easy: left boundary = 0.0
            else
                H.setLower(min(B.lower(),T.lower()));  //  check minimum of intersections (possibly undefined)

            if (R)
                H.setUpper(1.0);      //  easy: right boundary = 1.0
            else
                H.setUpper(max(B.upper(),T.upper()));  //  check maximum of intersections (possibly undefined)

            if (B)
                V.setLower(0.0);  //  easy: bottom boundary = 0.0
            else
                V.setLower(min(L.lower(),R.lower()));

            if (T)
                V.setUpper(1.0);
            else
                V.setUpper(max(L.upper(),R.upper()));

            //  do we have to calculate the ellipse boundaries ??
            if (H != Interval::UNIT)
            {
                double lambda[2];
                calculateEllipseBoundaries<accurate_float>(
                        P[i],P[i+1],  Q[j],Q[j+1],
                        epsilon, lambda);

                H.setLower(min(H.lower(),lambda[0]));
                H.setUpper(max(H.upper(),lambda[1]));
            }

            if (V != Interval::UNIT)
            {
                double lambda[2];
                calculateEllipseBoundaries<accurate_float>(
                        Q[j],Q[j+1], P[i],P[i+1],
                        epsilon, lambda);

                V.setLower(min(V.lower(),lambda[0]));
                V.setUpper(max(V.upper(),lambda[1]));
            }
        }
    }
}

bool FreeSpace::cellEmptyBounds(int i, int j) const {
    const Cell& cl = cell(i,j);
    //  easy to calculate but assumes that bounds have been calculate !!
    //  (only k-Frechet does it!)
    return !cl.bounds.H.valid()
            && !cl.bounds.V.valid();
}

bool FreeSpace::cellEmptyIntervals(int i, int j) const {
    const Cell& cl = cell(i,j);
    return ! cell(i,j).L
           && ! cell(i,j).B
           && ! cell(i+1,j).L
           && ! cell(i,j+1).B;
}

bool FreeSpace::cellFull(int i, int j) const {
    //  is this cell completely free?
    const Cell& cl = cell(i,j);
    return cl.L.contains(Interval::UNIT)
            && cl.B.contains(Interval::UNIT)
            && cell(i+1,j).L.contains(Interval::UNIT)
            && cell(i,j+1).B.contains(Interval::UNIT);
}


double FreeSpace::determinant(int i, int j) const {
    Point s1 = P[i];
    Point s2 = P[i+1];

    Point t1 = Q[j];
    Point t2 = Q[j+1];

    return (s1.x()-s2.x())*(t1.y()-t2.y()) - (s1.y()-s2.y())*(t1.x()-t2.x());
}


QRectF FreeSpace::segmentBounds(int i, int j)
{
    const Cell& cl = cell(i,j);
    return cl.bounds.boundingRect();
}

std::list<Interval> FreeSpace::collectFreeVerticalIntervals(int i) const
{
    std::list<Interval> result;
    const Interval& L = cell(i,0).L;
    bool inside = (L.lower()==0.0);
    if (inside) result.push_back(L);

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

        if (!L) {
            inside=false;
        }
        else if (L.lower() > 0.0) {
            result.push_back(L + (double)j);
            inside = (L.upper() >= 1.0) && (L2.lower()==0.0);
        }
        else if (inside) {
            Q_ASSERT(L.lower()==0.0);
            result.back().upper() = L.upper() + j;
            inside = (L.upper() >= 1.0) && (L2.lower()==0.0);
        }
    }
    return result;
}

void FreeSpace::fixBorderCase(Interval* N, Interval* E, Interval* S, Interval* W)
{

    bool fix = (N && (N->lower()==0.0))
            || (E && (E->lower()==0.0))
            || (S && (S->upper()==1.0))
            || (W && (W->upper()==1.0));

    if (fix) {
        if (N) {
            N->lower()=0.0;
            if (std::isnan(N->upper())) N->upper()=0.0;
        }
        if (E) {
            E->lower()=0.0;
            if (std::isnan(E->upper())) E->upper()=0.0;
        }
        if (S) {
            S->upper()=1.0;
            if (std::isnan(S->lower())) S->lower()=1.0;
        }
        if (W && (std::isnan(W->upper()) || (W->upper()!=1.0))) {
            W->upper()=1.0;
            if (std::isnan(W->lower())) W->lower()=1.0;
        }
    }
}

template<typename Float>
void FreeSpace::calculateEllipseBoundaries(
       const Point& p1, const Point& p2,
       const Point& q1, const Point& q2,
       double epsilon,
       double lambda[2])
{
    Float A,B,D;

    Float p1x=p1.x();
    Float p1y=p1.y();
    Float p2x=p2.x();
    Float p2y=p2.y();
    Float q1x=q1.x();
    Float q1y=q1.y();
    Float q2x=q2.x();
    Float q2y=q2.y();

    lambda[0]=lambda[1] = NAN;

    A = q2x*(p1y-q1y) + q1x*(q2y-p1y) + p1x*(q1y-q2y);
    B = euclidean_distance_sq<Float>(q1,q2);
    D = (q2y-q1y)*(p2x-p1x) - (p2y-p1y)*(q2x-q1x);

    if (D==0.0) return;

    //  lambda_1/2
    Float lambda0 = (A - epsilon*sqrt(B)) / D;
    Float lambda1 = (A + epsilon*sqrt(B)) / D;

    if (lambda0 > lambda1)
        std::swap(lambda0,lambda1);

    //  clip to unit rect
    if ((lambda0<0.0) || (lambda0>1.0))
        lambda0 = NAN;
    if ((lambda1<0.0) || (lambda1>1.0))
        lambda1 = NAN;

    if (std::isnan(lambda0) && std::isnan(lambda1)) return;

    //  mu_1/2
    Float E8 = (p2x-p1x)*(q2x-q1x) + (p2y-p1y)*(q2y-q1y);
    Float E9 = (q2x-q1x)*(p1x-q1x) + (q2y-q1y)*(p1y-q1y);

    Float mu0 = (lambda0*E8+E9)/B;
    Float mu1 = (lambda1*E8+E9)/B;

    //  clip to unit rect
    if ((mu0>=0.0) && (mu0<=1.0))
        lambda[0] = (double)lambda0;

    if ((mu1>=0.0) && (mu1<=1.0))
        lambda[1] = (double)lambda1;
}