visibility.cpp revision 5ccaf9e36e8931186a458f3ab7b57eb4a09d4630
/*
* vim: ts=4 sw=4 et tw=0 wm=0
*
* libavoid - Fast, Incremental, Object-avoiding Line Router
* Copyright (C) 2004-2005 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 <cfloat>
#include "libavoid/shape.h"
#include "libavoid/debug.h"
#include "libavoid/visibility.h"
#include "libavoid/graph.h"
#include <math.h>
#ifdef LINEDEBUG
#include "SDL_gfxPrimitives.h"
#endif
namespace Avoid {
bool UseAStarSearch = true;
bool IgnoreRegions = true;
bool SelectiveReroute = true;
bool IncludeEndpoints = true;
bool UseLeesAlgorithm = false;
bool InvisibilityGrph = true;
bool PartialFeedback = false;
bool PartialTime = false;
void computeCompleteVis(void)
{
VertInf *beginVert = vertices.shapesBegin();
VertInf *endVert = vertices.end();
for (VertInf *i = beginVert; i != endVert; i = i->lstNext)
{
db_printf("-- CONSIDERING --\n");
i->id.db_print();
for (VertInf *j = i->lstPrev ; j != NULL; j = j->lstPrev)
{
bool knownNew = true;
EdgeInf::checkEdgeVisibility(i, j, knownNew);
}
}
}
void shapeVis(ShapeRef *shape)
{
if (!InvisibilityGrph)
{
// Clear shape from graph.
shape->removeFromGraph();
}
VertInf *shapeBegin = shape->firstVert();
VertInf *shapeEnd = shape->lastVert()->lstNext;
VertInf *pointsBegin = NULL;
if (IncludeEndpoints)
{
pointsBegin = vertices.connsBegin();
}
else
{
pointsBegin = 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 = vertices.end();
for (VertInf *k = shapeEnd; k != pointsEnd; k = k->lstNext)
{
EdgeInf::checkEdgeVisibility(curr, k, knownNew);
}
}
}
void shapeVisSweep(ShapeRef *shape)
{
if (!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)
{
const VertID& pID = point->id;
// Make sure we're only doing ptVis for endpoints.
assert(!(pID.isShape));
if (!InvisibilityGrph)
{
point->removeFromGraph();
}
if (gen_contains && !(pID.isShape))
{
generateContains(point);
}
if (UseLeesAlgorithm)
{
vertexSweep(point);
}
else
{
VertInf *shapesEnd = vertices.end();
for (VertInf *k = vertices.shapesBegin(); k != shapesEnd;
k = k->lstNext)
{
EdgeInf::checkEdgeVisibility(point, k, knownNew);
}
if (IncludeEndpoints && partner)
{
EdgeInf::checkEdgeVisibility(point, partner, knownNew);
}
}
}
//============================================================================
// SWEEP CODE
//
static VertInf *centerInf;
static Point centerPoint;
static VertID centerID;
static double centerAngle;
#ifdef LINEDEBUG
SDL_Surface *avoid_screen = NULL;
#endif
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
{
return true;
}
}
else
{
return true;
}
}
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)
{
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 = vertices.connsBegin();
VertInf *endVert = 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 (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 = contains[centerID];
// And edge to T that intersect the initial ray.
VertInf *last = vertices.end();
for (VertInf *k = 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(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 (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(centerInf->shPrev->point, centerPoint,
centerInf->shNext->point, currInf->point);
}
if (currInf->id.isShape)
{
cone2 = inValidRegion(currInf->shPrev->point, currInf->point,
currInf->shNext->point, centerPoint);
}
if (!cone1 || !cone2)
{
lastInf = NULL;
if (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 (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)
{
ePtr = e.find(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)
{
ePtr = e.find(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
}
}
}