visibility.cpp

/*
 * vim: ts=4 sw=4 et tw=0 wm=0
 *
 * libavoid - Fast, Incremental, Object-avoiding Line Router
 * Copyright (C) 2004-2006  Michael Wybrow <mjwybrow@users.sourceforge.net>
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
*/

#include <algorithm>
#include <cfloat>

#include "libavoid/shape.h"
#include "libavoid/debug.h"
#include "libavoid/visibility.h"
#include "libavoid/vertices.h"
#include "libavoid/graph.h"
#include "libavoid/geometry.h"
#include "libavoid/router.h"

#include <math.h>

#ifdef LINEDEBUG
  #include "SDL_gfxPrimitives.h"
#endif

namespace Avoid {


void shapeVis(ShapeRef *shape)
{
    Router *router = shape->router();

    if ( !(router->InvisibilityGrph) )
    {
        // Clear shape from graph.
        shape->removeFromGraph();
    }

    VertInf *shapeBegin = shape->firstVert();
    VertInf *shapeEnd = shape->lastVert()->lstNext;

    VertInf *pointsBegin = NULL;
    if (router->IncludeEndpoints)
    {
        pointsBegin = router->vertices.connsBegin();
    }
    else
    {
        pointsBegin = router->vertices.shapesBegin();
    }

    for (VertInf *curr = shapeBegin; curr != shapeEnd; curr = curr->lstNext)
    {
        bool knownNew = true;

        db_printf("-- CONSIDERING --\n");
        curr->id.db_print();

        db_printf("\tFirst Half:\n");
        for (VertInf *j = pointsBegin ; j != curr; j = j->lstNext)
        {
            EdgeInf::checkEdgeVisibility(curr, j, knownNew);
        }

        db_printf("\tSecond Half:\n");
        VertInf *pointsEnd = router->vertices.end();
        for (VertInf *k = shapeEnd; k != pointsEnd; k = k->lstNext)
        {
            EdgeInf::checkEdgeVisibility(curr, k, knownNew);
        }
    }
}


void shapeVisSweep(ShapeRef *shape)
{
    Router *router = shape->router();

    if ( !(router->InvisibilityGrph) )
    {
        // Clear shape from graph.
        shape->removeFromGraph();
    }

    VertInf *startIter = shape->firstVert();
    VertInf *endIter = shape->lastVert()->lstNext;

    for (VertInf *i = startIter; i != endIter; i = i->lstNext)
    {
        vertexSweep(i);
    }
}


void vertexVisibility(VertInf *point, VertInf *partner, bool knownNew,
        const bool gen_contains)
{
    Router *router = point->_router;
    const VertID& pID = point->id;

    // Make sure we're only doing ptVis for endpoints.
    assert(!(pID.isShape));

    if ( !(router->InvisibilityGrph) )
    {
        point->removeFromGraph();
    }

    if (gen_contains && !(pID.isShape))
    {
        router->generateContains(point);
    }

    if (router->UseLeesAlgorithm)
    {
        vertexSweep(point);
    }
    else
    {
        VertInf *shapesEnd = router->vertices.end();
        for (VertInf *k = router->vertices.shapesBegin(); k != shapesEnd;
                k = k->lstNext)
        {
            EdgeInf::checkEdgeVisibility(point, k, knownNew);
        }
        if (router->IncludeEndpoints && partner)
        {
            EdgeInf::checkEdgeVisibility(point, partner, knownNew);
        }
    }
}


//============================================================================
//  SWEEP CODE
//

static VertInf *centerInf;
static Point centerPoint;
static VertID centerID;
static double centerAngle;


class PointPair
{
    public:
        PointPair(VertInf *inf)
            : vInf(inf)
        {
            double x = vInf->point.x - centerPoint.x;
            double y = vInf->point.y - centerPoint.y;

            angle = pos_to_angle(x, y);
        }
        bool operator==(const PointPair& rhs) const
        {
            if (vInf->id == rhs.vInf->id)
            {
                return true;
            }
            return false;
        }
        static double pos_to_angle(double x, double y)
        {
            double ang = atan(y / x);
            ang = (ang * 180) / M_PI;
            if (x < 0)
            {
                ang += 180;
            }
            else if (y < 0)
            {
                ang += 360;
            }
            return ang;
        }

        VertInf    *vInf;
        double     angle;
};


typedef std::list<PointPair > VertList;


class EdgePair
{
    public:
        EdgePair(VertInf *v1, VertInf *v2, double d, double a)
            : vInf1(v1), vInf2(v2), initdist(d), initangle(a)
        {
            currdist  = initdist;
            currangle = initangle;
        }
        bool operator<(const EdgePair& rhs) const
        {
            if (initdist == rhs.initdist)
            {
                // TODO: This is a bit of a hack, should be
                //       set by the call to the constructor.
                return dist(centerPoint, vInf2->point) <
                        dist(centerPoint, rhs.vInf2->point);
            }
            return (initdist < rhs.initdist);
        }
        bool operator==(const EdgePair& rhs) const
        {
            if (((vInf1->id == rhs.vInf1->id) &&
                        (vInf2->id == rhs.vInf2->id)) ||
                ((vInf1->id == rhs.vInf2->id) &&
                        (vInf2->id == rhs.vInf1->id)))
            {
                return true;
            }
            return false;
        }
        bool operator!=(const EdgePair& rhs) const
        {
            if (((vInf1->id == rhs.vInf1->id) &&
                        (vInf2->id == rhs.vInf2->id)) ||
                ((vInf1->id == rhs.vInf2->id) &&
                        (vInf2->id == rhs.vInf1->id)))
            {
                return false;
            }
            return true;
        }
        void SetObsAng(double a)
        {
            obsAngle = fmod(initangle - (a - 180), 360);

            //db_printf("SetObsAng: %.2f  (from init %.2f, a %.2f)\n",
            //      obsAngle, initangle, a);
        }

        VertInf *vInf1;
        VertInf *vInf2;
        double  initdist;
        double  initangle;
        double  currdist;
        double  currangle;
        double  obsAngle;
};

typedef std::set<EdgePair> EdgeSet;


static bool ppCompare(PointPair& pp1, PointPair& pp2)
{
    if (pp1.angle == pp2.angle)
    {
        // If the points are colinear, then order them in increasing
        // distance from the point we are sweeping around.
        return dist(centerPoint, pp1.vInf->point) <
                dist(centerPoint, pp2.vInf->point);
    }
    return pp1.angle < pp2.angle;
}


#define AHEAD    1
#define BEHIND  -1

class isBoundingShape
{
    public:
        // constructor remembers the value provided
        isBoundingShape(ShapeSet& set)
        : ss(set)
        { }
        // the following is an overloading of the function call operator
        bool operator () (const PointPair& pp)
        {
            if (pp.vInf->id.isShape &&
                    (ss.find(pp.vInf->id.objID) != ss.end()))
            {
                return true;
            }
            return false;
        }
    private:
        ShapeSet& ss;
};


static bool sweepVisible(EdgeSet& T, VertInf *currInf, VertInf *lastInf,
        bool lastVisible, double lastAngle, int *blocker)
{

    if (!lastInf || (lastAngle != centerAngle))
    {
        // Nothing before it on the current ray
        EdgeSet::iterator closestIt = T.begin();
        if (closestIt != T.end())
        {

            Point &e1 = (*closestIt).vInf1->point;
            Point &e2 = (*closestIt).vInf2->point;

            if (segmentIntersect(centerInf->point, currInf->point, e1, e2))
            {
                *blocker = (*closestIt).vInf1->id.objID;
                return false;
            }
        }
    }
    else
    {
        // There was another point before this on the ray (lastInf)
        if (!lastVisible)
        {
            *blocker = -1;
            return false;
        }
        else
        {
            // Check if there is an edge in T that blocks the ray
            // between lastInf and currInf.
            EdgeSet::iterator tfin = T.end();
            for (EdgeSet::iterator l = T.begin(); l != tfin; ++l)
            {
                Point &e1 = (*l).vInf1->point;
                Point &e2 = (*l).vInf2->point;

                if (segmentIntersect(lastInf->point, currInf->point, e1, e2))
                {
                    *blocker = (*l).vInf1->id.objID;
                    return false;
                }
            }
        }
    }
    return true;
}


void vertexSweep(VertInf *vert)
{
    Router *router = vert->_router;
    VertID& pID = vert->id;
    Point& pPoint = vert->point;

    centerInf = vert;
    centerID = pID;
    centerPoint = pPoint;
    Point centerPt = pPoint;
    centerAngle = -1;

    // List of shape (and maybe endpt) vertices, except p
    // Sort list, around
    VertList v;

    // Initialise the vertex list
    VertInf *beginVert = router->vertices.connsBegin();
    VertInf *endVert = router->vertices.end();
    for (VertInf *inf = beginVert; inf != endVert; inf = inf->lstNext)
    {
        if (inf->id == centerID)
        {
            // Don't include the center point
            continue;
        }

        if (inf->id.isShape)
        {
            // Add shape vertex
            v.push_back(inf);
        }
        else
        {
            if (router->IncludeEndpoints)
            {
                if (centerID.isShape)
                {
                    // Add endpoint vertex
                    v.push_back(inf);
                }
                else
                {
                    // Center is an endpoint, so only include the other
                    // endpoint from the matching connector.
                    VertID partnerID = VertID(centerID.objID, false,
                            (centerID.vn == 1) ? 2 : 1);
                    if (inf->id == partnerID)
                    {
                        v.push_back(inf);
                    }
                }
            }
        }
    }
    // TODO: This should be done with a sorted data type and insertion sort.
    v.sort(ppCompare);

    EdgeSet e;
    ShapeSet& ss = router->contains[centerID];

    // And edge to T that intersect the initial ray.
    VertInf *last = router->vertices.end();
    for (VertInf *k = router->vertices.shapesBegin(); k != last; )
    {
        VertID kID = k->id;
        if (!(centerID.isShape) && (ss.find(kID.objID) != ss.end()))
        {
            unsigned int shapeID = kID.objID;
            db_printf("Center is inside shape %u so ignore shape edges.\n",
                    shapeID);
            // One of the endpoints is inside this shape so ignore it.
            while ((k != last) && (k->id.objID == shapeID))
            {
                // And skip the other vertices from this shape.
                k = k->lstNext;
            }
            continue;
        }

        VertInf *kPrev = k->shPrev;
        if ((centerInf == k) || (centerInf == kPrev))
        {
            k = k->lstNext;
            continue;
        }

        Point xaxis(DBL_MAX, centerInf->point.y);

        if (segmentIntersect(centerInf->point, xaxis, kPrev->point, k->point))
        {
            double distance;
            if (vecDir(centerInf->point, xaxis, kPrev->point) == BEHIND)
            {
                distance = dist(centerInf->point, kPrev->point);
            }
            else
            {
                distance = dist(centerInf->point, k->point);
            }

            EdgePair intPair = EdgePair(k, kPrev, distance, 0.0);
            e.insert(intPair).first;
        }
        k = k->lstNext;
    }

    // Start the actual sweep.
    db_printf("SWEEP: "); centerID.db_print(); db_printf("\n");

    VertInf *lastInf     = NULL;
    double   lastAngle   = 0;
    bool     lastVisible = false;
    int      lastBlocker = 0;

    isBoundingShape isBounding(router->contains[centerID]);
    VertList::iterator vfst = v.begin();
    VertList::iterator vfin = v.end();
    for (VertList::iterator t = vfst; t != vfin; ++t)
    {
        VertInf *currInf = (*t).vInf;
        VertID& currID = currInf->id;
        Point&  currPt = currInf->point;
        centerAngle = (*t).angle;

#ifdef LINEDEBUG
        Sint16 ppx = (int) centerPt.x;
        Sint16 ppy = (int) centerPt.y;

        Sint16 cx = (int) currPt.x;
        Sint16 cy = (int) currPt.y;
#endif

        double currDist = dist(centerPt, currPt);
        db_printf("Dist: %.1f.\n", currDist);

        EdgeInf *edge = EdgeInf::existingEdge(centerInf, currInf);
        if (edge == NULL)
        {
            edge = new EdgeInf(centerInf, currInf);
        }
        // Ignore vertices from bounding shapes, if sweeping round an endpoint.
        if (!(centerID.isShape) && isBounding(*t))
        {
            if (router->InvisibilityGrph)
            {
                // if p and t can't see each other, add blank edge
                db_printf("\tSkipping visibility edge... \n\t\t");
                edge->addBlocker(currInf->id.objID);
                edge->db_print();
            }
            continue;
        }


        bool cone1 = true, cone2 = true;
        if (centerID.isShape)
        {
            cone1 = inValidRegion(router->IgnoreRegions,
                    centerInf->shPrev->point, centerPoint,
                    centerInf->shNext->point, currInf->point);
        }
        if (currInf->id.isShape)
        {
            cone2 = inValidRegion(router->IgnoreRegions,
                    currInf->shPrev->point, currInf->point,
                    currInf->shNext->point, centerPoint);
        }

        if (!cone1 || !cone2)
        {
            lastInf = NULL;
            if (router->InvisibilityGrph)
            {
                db_printf("\tSetting invisibility edge... \n\t\t");
                edge->addBlocker(0);
                edge->db_print();
            }
        }
        else
        {
            int blocker = 0;
            // Check visibility.
            bool currVisible = sweepVisible(e, currInf,
                    lastInf, lastVisible, lastAngle, &blocker);
            if (blocker == -1)
            {
                blocker = lastBlocker;
            }
            if (currVisible)
            {
#ifdef LINEDEBUG
                lineRGBA(avoid_screen, ppx, ppy, cx, cy, 255, 0, 0, 32);
#endif
                db_printf("\tSetting visibility edge... \n\t\t");
                edge->setDist(currDist);
                edge->db_print();
            }
            else if (router->InvisibilityGrph)
            {
                db_printf("\tSetting invisibility edge... \n\t\t");
                edge->addBlocker(blocker);
                edge->db_print();
            }

            lastVisible = currVisible;
            lastInf     = currInf;
            lastAngle   = centerAngle;
            lastBlocker = blocker;
        }

        if (currID.isShape)
        {
            // This is a shape edge
            Point& prevPt = currInf->shPrev->point;
            Point& nextPt = currInf->shNext->point;

            int prevDir = vecDir(centerPt, currPt, prevPt);
            EdgePair prevPair = EdgePair(currInf, currInf->shPrev,
                    currDist, centerAngle);

            EdgeSet::iterator ePtr;
            if (prevDir == BEHIND)
            {
                // XXX: Strangely e.find does not return the correct results.
                // ePtr = e.find(prevPair);
                ePtr = std::find(e.begin(), e.end(), prevPair);
                if (ePtr != e.end())
                {
                    e.erase(ePtr);
                }
            }
            else if ((prevDir == AHEAD) && (currInf->shPrev != centerInf))
            {
                double x = prevPt.x - currPt.x;
                double y = prevPt.y - currPt.y;
                double angle = PointPair::pos_to_angle(x, y);
                prevPair.SetObsAng(angle);

                ePtr = e.insert(prevPair).first;
            }


            int nextDir = vecDir(centerPt, currPt, nextPt);
            EdgePair nextPair = EdgePair(currInf, currInf->shNext,
                    currDist, centerAngle);

            if (nextDir == BEHIND)
            {
                // XXX: Strangely e.find does not return the correct results.
                // ePtr = e.find(nextPair);
                ePtr = std::find(e.begin(), e.end(), nextPair);
                if (ePtr != e.end())
                {
                    e.erase(ePtr);
                }
            }
            else if ((nextDir == AHEAD) && (currInf->shNext != centerInf))
            {
                double x = nextPt.x - currPt.x;
                double y = nextPt.y - currPt.y;
                double angle = PointPair::pos_to_angle(x, y);
                nextPair.SetObsAng(angle);

                ePtr = e.insert(nextPair).first;
            }
        }

#ifdef LINEDEBUG
        SDL_Flip(avoid_screen);
#endif
    }
}


}