vispoly.c

source code to compute the visibility polygon of a point in a polygon.

/* Brian O'Connor
 *
 * vispoly.C
 *
 * Functions for creating a visibility polygon 
 */

#include <stdio.h>
#include <assert.h>
#include "input.h"
#include "line.h"
#include "cell.h"
#include "pursuer.h"
#include "vispoly.h"

void MakeGapEdges(EdgeInfo *eInfo, Point *p, EdgeInfo *vEdges);
void DumpGapEdges(EdgeInfo *vEdges);
void CalcPoly(EdgeInfo *edges, EdgeInfo *vEdges, VPoly *vpoly);
VPEdge *GapToObEdge(EdgeInfo *edgeInfo, EdgeInfo *vEdges, VPEdge *vpe,
		    VPEdge *lastOb, int bFirst);
VPEdge *ObToGapEdge(VPEdge *vpe, EdgeInfo *vEdges);
VPEdge *ObToObEdge(VPEdge *vpe, EdgeInfo *edgeInfo, EdgeInfo *vEdges);
int ClosestVisEdge(Line *edge, EdgeInfo *vEdges, Point *inGap, Point
		   *badGap, int i1, int i2);
void OrderPoly(VPoly *vpoly);

/* given the edges composing the obstacles & a point, return the
 * visibility polygon for this point. */
int MakeVisPoly(EdgeInfo *eInfo, Point *p, VPoly *vpoly)
{
  EdgeInfo *vEdges;
  
  vEdges = new EdgeInfo;
  MakeGapEdges(eInfo, p, vEdges);
//  DumpGapEdges(vEdges);
  CalcPoly(eInfo, vEdges, vpoly);
  if( vpoly->nEdges < 3 ) {
    return(FALSE);
  }
  OrderPoly(vpoly);
  return(TRUE);
}

void MakeGapEdges(EdgeInfo *eInfo, Point *p, EdgeInfo *vEdges)
{
  int i;
  vEdges->nEdges = 0;
  vEdges->edges = new (Line *)[eInfo->nEdges];
  
  for( i = 0; i < eInfo->nEdges; i++ ) {
    ExtendGapEdge(p, eInfo->edges[i]->A, eInfo, vEdges, 
		  eInfo->edges[i]->ob);
  }
}

/* very similar to ExtendOneRay in cell.C; will either:
 * 1) nothing if the path between p1 & p2 is not clear;
 * 2) a ray between p1 and p2 if p2 is blocked;
 * 3) a ray from p2 to another edge along the p1,p2 line if p2 is clear.
 * These edges can then be used as gap edges or for testing if a vertex
 * is visible when constructing a visibility polygon.
 */
void ExtendGapEdge(Point *p1, Point *p2, EdgeInfo *e, EdgeInfo *vEdges,
	Obstacle *o2)
{
  Line *l, *l2;
  int i;
  int p2vis;
  int visible = TRUE;
  double maxDist, currDist;
  Point *maxPt, *currPt;
  
  /* initialize fields */
  maxDist = INFINITY;
  p2vis = TRUE;
  
  l = PointsToLine(p1, p2);
  l->ob = NULL;
  
  if( !OpenEdgeTest(o2, p1, p2) ) {
    p2vis = FALSE;
  }

  maxPt = NULL;
  currPt = NewPoint();

  for( i = 0; i < e->nEdges; i++ ) {
    l2 = e->edges[i];
    if( (l2->A == p1 && l2->B == p2) ||
	(l2->B == p1 && l2->A == p2) ) continue;
    
    LineIntersectionDistance(l, l2, currPt, &currDist);
    if( !PointOnSegment(l2, currPt) ) currDist = INFINITY;
    if( currDist == INFINITY ) continue;
    if( FloatCompare(currDist, 0.0) > 0 &&
	FloatCompare(currDist, l->length) < 0 ) {
      visible = FALSE;
      return;
    } else if( FloatCompare(currDist, l->length) > 0 &&
	       FloatCompare(currDist, maxDist) < 0 ) {
      if( l2->ob != o2 ||
	  !InsideTest(l2, currPt, p2)) {
	maxDist = currDist;
	if( maxPt != NULL ) delete maxPt;
	maxPt = currPt;
	currPt = NewPoint();
      } else {
	/* we are closed off at this end & shouldn't extend: broken test? */
	/*if( maxPt != NULL ) delete maxPt;
	maxPt = p2;
	maxDist = -INFINITY; */
      }
    }
  }
  
  /* make rays out of the data we've found */
  if( maxPt == NULL ) maxPt = p2;
  
  if( visible && p2vis && !PointEqual(maxPt, p2) ) {
    vEdges->edges[vEdges->nEdges++] = new VPEdge(p2, maxPt);
    ((VPEdge *)vEdges->edges[vEdges->nEdges - 1])->bGap = TRUE;
  } else if( visible ) {
    vEdges->edges[vEdges->nEdges++] = new VPEdge(p1, p2);
    ((VPEdge *)vEdges->edges[vEdges->nEdges - 1])->bGap = FALSE;
  } else {
    /* delete resources */
  }
  
  /* cleanup */
  if( currPt != maxPt ) delete currPt;
}

void DumpGapEdges(EdgeInfo *vEdges)
{
	int i;
	VPEdge *e;
	for( i = 0; i < vEdges->nEdges; i++ ) {
		e = (VPEdge *)vEdges->edges[i];	
		if( e->bGap ) {
			printf("Gap Edge: (%f, %f), (%f, %f)\n",
				e->A->x, e->A->y, e->B->x, e->B->y );
		} else {
			printf("Visibility edge: (%f, %f), (%f, %f)\n",
				e->A->x, e->A->y, e->B->x, e->B->y );
		}
	}
	printf("\n");
}

/* algorithm: 
 * select next edge by:
 *  gap edge: take cell edge that the opposite endpoint connects to
 *  ob  edge: look for unmarked cell edges & take any; otherwise
 *	take adjacent edge we haven't visited & which has an 
 *	unmarked visibility edge.
 * terminate when we hit an edge w. all vedges out marked.
 */
void CalcPoly(EdgeInfo *edges, EdgeInfo *vEdges, VPoly *vpoly)
{

  int i,j, bPrevGap;
  VPEdge *vpe, *result;
  vpoly->edges = new (VPEdge *)[CALCPOLY_PTS];
  
  i = 0;
  vpoly->nEdges = 0;
  if( ((VPEdge *)vEdges->edges[0])->bGap) {
    vpoly->edges[i++] = (VPEdge *)vEdges->edges[0];
  }
  bPrevGap = FALSE;
  ((VPEdge *)vEdges->edges[0])->bMark = TRUE;

  while(TRUE) {
    vpoly->nEdges = i;
    assert( i < CALCPOLY_PTS );
    if( i == 0 ) {
      /* special case: first vEdge we used was not a gap edge */
      vpoly->edges[i++] = GapToObEdge(edges, vEdges,
				      (VPEdge *) vEdges->edges[0],NULL,
				      TRUE);
      assert(vpoly->edges[i - 1] != NULL);
    } else if( vpoly->edges[i-1]->bGap ) {
      /* case 1: last edge was a gap edge; search edgelist for a match. */
      vpoly->edges[i++] = GapToObEdge(edges, vEdges, vpoly->edges[i-1],
				      i>1?vpoly->edges[i-2]:NULL, FALSE);
      assert(vpoly->edges[i -1] != NULL);
      bPrevGap = TRUE;
    } else {
      /* case 2: last edge was an obstacle edge;
	 first look for an unmarked gap edge out, then look for
	 an unmarked visibility edge indicating which edge to visit next.
	 */
      assert(i > 0 );
      vpe = (VPEdge *)vpoly->edges[i - 1];
      if(!bPrevGap) result = ObToGapEdge(vpe, vEdges);
      else result = NULL;
      if( result == NULL ) result = ObToObEdge(vpe, edges, vEdges);
      if(bPrevGap && result == NULL) result = ObToGapEdge(vpe, vEdges); 
      if( result == NULL ) break; /* finished polygon */
      bPrevGap = FALSE;
      vpoly->edges[i++] = result;
    }
  }

  vpoly->nEdges = i;
}

VPEdge *GapToObEdge(EdgeInfo *edgeInfo, EdgeInfo *vEdges, VPEdge *vpe,
		    VPEdge *lastOb, int bFirst)
{
  Line *edge;
  VPEdge *visEdge;
  Point *p;
  Point *p2;
  int i,j, firstVisEdge, nextGapEdge, nextVisEdge;
  double minDist;
  
  /* find an unused ob edge that we are connected to */
  for( i = 0; i < edgeInfo->nEdges; i++ ) {
    edge = NULL;
    p = NULL;
    p2 = NULL;
    firstVisEdge = -1;
    nextGapEdge = -1;
    nextVisEdge = -1;
    minDist = INFINITY;
    
    if( PointOnSegment(edgeInfo->edges[i], vpe->B) && 
	(lastOb == NULL || !PointOnSegment(lastOb, vpe->B)) ) {
      edge = edgeInfo->edges[i];
      p = vpe->B;
    } else if( PointOnSegment(edgeInfo->edges[i], vpe->A) &&
	       (lastOb == NULL || !PointOnSegment(lastOb, vpe->A)) ) {
      edge = edgeInfo->edges[i];
      p = vpe->A;
    } else continue;
    
    if( bFirst ) { // entering edge was really a vis edge
      if( PointEqual(p, edge->A) ) {
	return( new VPEdge(p, edge->B) );
      } else if( PointEqual(p, edge->B)) {
	return( new VPEdge(p, edge->A) );
      } else assert(0);
    }
    
    /*    for( j = 0; j < vEdges->nEdges; j++ ) {
      visEdge = (VPEdge *)vEdges->edges[j];
      if( PointOnSegment(edge, visEdge->A) ) {
	p2 = visEdge->A;
      } else if( PointOnSegment(edge, visEdge->B) ) {
	p2 = visEdge->B;
      } else continue;
      if( firstVisEdge == -1 && !visEdge->bGap && !visEdge->bMark ) {
	firstVisEdge = j;
      } else if( !visEdge->bGap && !visEdge->bMark ) {
	assert(nextVisEdge == -1);
	nextVisEdge = j;
      } else if( visEdge->bGap && !visEdge->bMark ) {
	
	assert(nextGapEdge == -1);
	nextGapEdge = j;
      }
    }*/
    
    for( j = 0; j < vEdges->nEdges; j++ ) {
      visEdge = (VPEdge *)vEdges->edges[j];
      if( !visEdge->bMark ) {
	if( PointOnSegment(edge, visEdge->A) ) {
	  p2 = visEdge->A;
	} else if( PointOnSegment(edge, visEdge->B) ) {
	  p2 = visEdge->B;
	} else continue;
	if( FloatCompare(LineDistance(p, p2), minDist) < 0 ) {
	  firstVisEdge = j;
	  minDist = LineDistance(p, p2);
	}
      }
    }
      
    if( firstVisEdge != -1 ) break;
  } /* end for loop, get a new value for edge */ 

  /* can't continue; close off the polygon */
  if( firstVisEdge == -1) {
    if( PointOnSegment( lastOb, vpe->A) ) p = vpe->B;
    else if( PointOnSegment( lastOb, vpe->B) ) p = vpe->A;
    else assert(0);
    return( new VPEdge(p,vEdges->edges[0]->B) ); 
  }
  assert( p != NULL );

  assert(firstVisEdge != -1);
  /* now convert firstVisEdge into an endpoint on an obstacle edge */
  if( PointOnSegment(edge, vEdges->edges[firstVisEdge]->A) ) {
    p2 = vEdges->edges[firstVisEdge]->A;
  } else if( PointOnSegment(edge, vEdges->edges[firstVisEdge]->B) ) {
    p2 = vEdges->edges[firstVisEdge]->B;
  } else assert(0);

  return( new VPEdge(p, p2) );
}

VPEdge *ObToGapEdge(VPEdge *vpe, EdgeInfo *vEdges)
{
  int i;
  VPEdge *vpeNew;
  Point *p1, *p2;
  double dist, minDist;
  Point *minp1, *minp2;
  VPEdge *minvpe;
 
  minDist = INFINITY;
  for(i = 0; i < vEdges->nEdges; i++) {
    vpeNew = (VPEdge *)vEdges->edges[i];
    if( PointOnSegment(vpe, vpeNew->A) ) {
      dist = LineDistance(vpeNew->A, vpe->A);
      p1 = vpeNew->A;
      p2 = vpeNew->B;
    } else if( PointOnSegment(vpe, vpeNew->B) ) {
      dist = LineDistance(vpeNew->B, vpe->A);
      p1 = vpeNew->B;
      p2 = vpeNew->A;
    } else continue;

    if(FloatCompare(dist, minDist) < 0 && vpeNew->bGap && !vpeNew->bMark ) {
      minvpe = vpeNew;
      minp1 = p1;
      minp2 = p2;
      minDist = dist;
    }
  }

  if( minDist < INFINITY ) {
    /* endpt of ob edge = start pt of gap edge */
    vpe->B = minp1;
    minvpe->A = minp1;
    minvpe->B = minp2;
    minvpe->bMark = TRUE;
    return(minvpe);
  }

  return(NULL); /* no unmarked gap edge leaving this edge */
}

VPEdge *ObToObEdge(VPEdge *obEdge, EdgeInfo *edgeInfo, EdgeInfo *vEdges)
{
  int i;
  VPEdge *vpe;
  Point *p;
  Point *p2;

  p = NULL;
  for(i = 0; p == NULL && i < vEdges->nEdges; i++ ) {
    vpe = (VPEdge *)vEdges->edges[i];
    if( !vpe->bGap && !vpe->bMark ) {
      if( PointEqual(vpe->A, obEdge->A) ) { p = vpe->A; p2 = obEdge->B; }
      else if( PointEqual(vpe->A, obEdge->B) ) { p = vpe->A; p2 = obEdge->A; }
      else if( PointEqual(vpe->B, obEdge->A) ) { p = vpe->B; p2 = obEdge->B; }
      else if( PointEqual(vpe->B, obEdge->B) ) { p = vpe->B; p2 = obEdge->A; }
    }
  }
  if( p == NULL) return(NULL); /* no vis edges connected to adjacent edges */
  /* mark vpe so that we don't try to use it again in the future */
  vpe->bMark = TRUE;

  /* now find the other edge that contains point p; the PointOnSegment
     check verifies that we don't select a new edge == obEdge. */
  for( i = 0; i < edgeInfo->nEdges; i++ ) {
    if( (PointEqual(p, edgeInfo->edges[i]->A) &&
	 !PointOnSegment(edgeInfo->edges[i], p2) )) {
      return( new VPEdge(p, edgeInfo->edges[i]->B));
    } else if( PointEqual(p, edgeInfo->edges[i]->B) &&
	 !PointOnSegment(edgeInfo->edges[i], p2) ) {
      return( new VPEdge(p, edgeInfo->edges[i]->A) );
    }
  }
  assert(0); /* should always find a matching edge */
}

int ClosestVisEdge(Line *edge, EdgeInfo *vEdges, Point *inGap, Point
		   *badGap, int i1, int i2)
{
  Point *p1, *p2;
  double d1in, d2in, d1bad, d2bad;

  assert( i1 != i2 );
  p1 = vEdges->edges[i1]->B;
  p2 = vEdges->edges[i2]->B;
 
  d1in = LineDistance(inGap, p1);
  d2in = LineDistance(inGap, p2);
  d1bad = LineDistance(badGap, p1);
  d2bad = LineDistance(badGap, p2);

  /* select pt that is closer to p1 than to p2 */
  if( d1in <= d1bad) {
    assert( d2in >= d2bad );
    return(i1);
  } else {
    assert( d2in <= d2bad );
    return(i2);
  }
}

/* flip order of edges around so that they all line up. */
void OrderPoly(VPoly *vpoly)
{
	int i, bGap, bMark;
	Point *p1, *p2;
	assert(vpoly->nEdges > 1); // check for bad vpoly
	if( !PointEqual(vpoly->edges[0]->B, vpoly->edges[1]->A) ) {
		p1 = vpoly->edges[0]->A;
		p2 = vpoly->edges[0]->B;
                bGap = ( (VPEdge *)vpoly->edges[0])->bGap;
                bMark = ( (VPEdge *)vpoly->edges[0])->bMark;
		assert( PointEqual(vpoly->edges[1]->A, p1) );
		delete vpoly->edges[0];
		vpoly->edges[0] = new VPEdge(p2, p1);
		vpoly->edges[0]->bGap = bGap;
		vpoly->edges[0]->bMark = bMark;
	}
}