cell.c

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

/* Brian O'Connor
 *
 * cell.c: Read in the environment and decompose it into a set of
 * convex polygonal cells.
 */

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

#define INIT_DIST 0.001 /* not zero, but smaller than any real distance */

void ExtendRays(WorldInfo *wInfo, EdgeInfo *edges, EdgeInfo *rays);
void MakeEdges(WorldInfo *wInfo, EdgeInfo *eInfo);
void MakeCellEdges(WorldInfo *wInfo, EdgeInfo *rays, CellInfo *cInfo);
void ExtendOneRay(Point *p1, Point *p2, EdgeInfo *e, EdgeInfo *rays,
		Obstacle *o1, Obstacle *o2, Obstacle *border);
void AddRay(Point *p1, Point *p2, EdgeInfo *rays);
void DumpRayInfo(EdgeInfo *rays);
void DumpCellInfo(CellInfo *cells);
//int NotEdgeTest(Obstacle *ob, Point *p1, Point *p2);
void InsertPoint(Point *p, double dist, Point **ptArr, int *nPts);	
void RemoveDuplicates(Point **ptArr, int *nPts);
int NextClockwiseEdge(CellInfo *cellInfo, Point *p, int i);
void SortEdges(CellEdge **edges, int nEdges);
void MakeCells(WorldInfo *wInfo, CellInfo *cells);
int IsCellObstacle(WorldInfo *wInfo, Cell *cell, int *bBorder);
Point *CellCenter(Cell *cell);
void CollapseRays(EdgeInfo *rays);
void RemoveRay(EdgeInfo *rays, int i);
int NotEdgeTest(Point *p1, Point *p2, Obstacle *ob, Line *l1a, Line *l1b, 
		Line *l2a, Line *l2b, int bBorder);
Line *FindEdgeB(EdgeInfo *edges, Point *p);



void BuildDecomposition(char *file, PInfo *pInfo) 
{
  EdgeInfo *rays;
  int i;
  
  pInfo->wInfo = new WorldInfo;
  pInfo->edgeInfo = new EdgeInfo;
  rays = new EdgeInfo;
  
  /*		DotProductTest(); */ 
  ReadInputFile(file, pInfo->wInfo);
  MakeEdges(pInfo->wInfo, pInfo->edgeInfo);
  ExtendRays(pInfo->wInfo, pInfo->edgeInfo, rays);
  //DumpRayInfo(rays);
  CollapseRays(rays);
  //DumpRayInfo(rays);
  MakeCellEdges(pInfo->wInfo, rays, pInfo->cellInfo);
  //DumpCellInfo(pInfo->cellInfo); 
  SortEdges(pInfo->cellInfo->edges, pInfo->cellInfo->nEdges);
  //DumpCellInfo(pInfo->cellInfo); 
  MakeCells(pInfo->wInfo, pInfo->cellInfo); 
  /*for(i = 0; i < pInfo->cellInfo->nCells; i++) {
    printf("Num edges: %d\n", pInfo->cellInfo->cells[i]->nEdges);
  }*/

  delete rays;
}

/* for multiple pursuers: build a new decomposition given the old one
 * which will also tell us what cell to start the new decomposition
 * at. */
void RebuildDecomposition(PInfo *pInfo1, PInfo *pInfo)
{
  EdgeInfo *rays;
  int i;
  
  pInfo->wInfo = new WorldInfo;
  pInfo->edgeInfo = new EdgeInfo;
  rays = new EdgeInfo;

  RebuildInput(pInfo1, pInfo->wInfo);
  MakeEdges(pInfo->wInfo, pInfo->edgeInfo);
  ExtendRays(pInfo->wInfo, pInfo->edgeInfo, rays);
  //DumpRayInfo(rays);
  CollapseRays(rays);
  //DumpRayInfo(rays);
  MakeCellEdges(pInfo->wInfo, rays, pInfo->cellInfo);
  //DumpCellInfo(pInfo->cellInfo); 
  SortEdges(pInfo->cellInfo->edges, pInfo->cellInfo->nEdges);
  //DumpCellInfo(pInfo->cellInfo); 
  MakeCells(pInfo->wInfo, pInfo->cellInfo); 
  /*for(i = 0; i < pInfo->cellInfo->nCells; i++) {
    printf("Num edges: %d\n", pInfo->cellInfo->cells[i]->nEdges);
  }*/

  delete rays;

}

void MakeEdges(WorldInfo *wInfo, EdgeInfo *eInfo)
{
  int nEdges, currEdge;
  int i,j;
  
  /* count the number of edges we will need */
  nEdges = wInfo->border->nPts;
  for( i = 0; i < wInfo->nObs; i++ ) {
    nEdges += wInfo->obs[i]->nPts;
  }
  eInfo->edges = new Line *[nEdges];
  eInfo->nEdges = nEdges;
  
  /* now read in the edges */
  currEdge = 0;
  for( i = 0; i < wInfo->border->nPts; i++ ) {
    eInfo->edges[currEdge] = PointsToLine(&wInfo->border->pts[i],
		  (&wInfo->border->pts[(i + 1)% wInfo->border->nPts]) );
    eInfo->edges[currEdge]->ob = wInfo->border;
    currEdge++;
  }
  eInfo->firstEdge = 0;
  
  for( i = 0; i < wInfo->nObs; i++ ) {
    for(j = 0; j < wInfo->obs[i]->nPts; j++ ) {
      eInfo->edges[currEdge] = PointsToLine(&wInfo->obs[i]->pts[j],
		    &wInfo->obs[i]->pts[(j + 1) % wInfo->obs[i]->nPts] );
      eInfo->edges[currEdge]->ob = wInfo->obs[i];
      currEdge++;
    }
  }
  assert( currEdge <= eInfo->nEdges );
}

/* Given a list of edges, extend them using the algorithm on the web
 * into the segments that will partition the cells of our decomposition.
 */
void ExtendRays(WorldInfo *wInfo, EdgeInfo *edges, EdgeInfo *rays)
{
  
  int i,j, bBorder;
  int sameOb;
  Obstacle *ob;
  
  i = j = 0;
  /* allocate for maximum O(N^2) edges */
  rays->nEdges = 0;
  rays->firstEdge = 0;
  rays->edges = new(Line *)[(edges->nEdges - i) * (edges->nEdges - i)];
  
  /* loop over all vertex pairs */
  for( i = edges->firstEdge; i < edges->nEdges; i++ ) {
    for( j = i + 1; j < edges->nEdges; j++ ) {
      sameOb = (edges->edges[i]->ob == edges->edges[j]->ob);
      ob = edges->edges[i]->ob;
      if( ob == wInfo->border ) bBorder = 1; // flag for NotEdgeTest
      else bBorder = 0;
      /*if(!sameOb || NotEdgeTest(edges->edges[j]->ob, 
				edges->edges[i]->A, edges->edges[j]->A) )
				{*/
        if( !sameOb || NotEdgeTest(edges->edges[i]->A,
				   edges->edges[j]->A,
                           ob,
			   edges->edges[i],
			   FindEdgeB(edges, edges->edges[i]->A),
			   edges->edges[j],
			   FindEdgeB(edges, edges->edges[j]->A), bBorder )
	  ) {

	ExtendOneRay(edges->edges[i]->A, edges->edges[j]->A, edges, rays,
		     edges->edges[i]->ob, edges->edges[j]->ob, wInfo->border);
	if( rays->nEdges > 25 ) {
	  //printf("In testcase\n");
	}
      }
    }
  }
}

/* add one or two new rays to the list, with new endpoints 
 * equal to the furthest extent of the ray.
 * sameOb == TRUE if p1 and p2 are part of the same obstacle
 */
void ExtendOneRay(Point *p1, Point *p2, EdgeInfo *e, EdgeInfo *rays,
	Obstacle *o1, Obstacle *o2, Obstacle *border)
{
  Line *l, *l2;
  int i, sameOb;
  int p1vis, p2vis;
  int visible = TRUE;
  double minDist, maxDist, currDist;
  /* because the points we create are saved in the rays structure,
   * we need to dynamically allocate memory for them each time
   * we create a new ray. */
  Point* minPt, *maxPt, *currPt;
  
  /* initialize fields */
  sameOb = ( (o1 == o2) && IsPointAdjacent(o1, p1, p2));
  minDist = -INFINITY;
  maxDist = INFINITY;
  p1vis = p2vis = TRUE;
  
  l = PointsToLine(p1, p2);
  l->ob = NULL;

  if( p1->x == 175 && p1->y == 100 && p2->x == 275 && p2->y == 200 ) {
    //printf("In testcase\n");
  }
  
  /* new: perform OpenEdgeTest (could move up 1 function too) */
  if(!sameOb && 
     (!OpenEdgeTest(o2, p1, p2) || !OpenEdgeTest(o1, p2, p1)) ) {
       
    p1vis = FALSE; 
    p2vis = FALSE;
    return;
  }

  minPt = NULL; maxPt = NULL;
  currPt = NewPoint();
  
  /*printf(" (%f, %f) to (%f, %f)\n", p1->x, p1->y, p2->x, p2->y); */
  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, 0.0) < 0 &&
	       FloatCompare(currDist, minDist) > 0 ) {
      /* if minPt/maxPt is on same ob as p1, p2 then make sure
       * we didn't extend the ray through an obstacle. */
      if(/*!sameOb ||*/ /*l2->ob == border || */
	 l2->ob != o1 ||
	 !InsideTest(l2, p1, currPt)) {
	minDist = currDist;
	if( minPt != NULL ) delete minPt;
	minPt = currPt;
	currPt = NewPoint();
      } else {
	if( minPt != NULL ) delete minPt;
	minPt = p1;
	minDist = currDist;
	//minDist = INFINITY; 
	/* possible bug: to fix eagle test, needed to remove suppression
	   code (setting dists to INFINITY).  This may cause bugs in
	   other examples however. Same change made below in max code.*/
      }
    } else if( FloatCompare(currDist, l->length) > 0 &&
	       FloatCompare(currDist, maxDist) < 0 ) {
      if(/*!sameOb ||*/ /* l2->ob == border || */
	 l2->ob != o2 ||
	 !InsideTest(l2, currPt, p2)) {
	maxDist = currDist;
	if( maxPt != NULL ) delete maxPt;
	maxPt = currPt;
	currPt = NewPoint();
      } else {
	if( maxPt != NULL ) delete maxPt;
	maxPt = p2;
	maxDist = currDist;
	//	maxDist = -INFINITY;
      }
    }
  }
  
  /* make rays out of the data we've found */
  if( minPt == NULL ) minPt = p1;
  if( maxPt == NULL ) maxPt = p2;
  
  /* this assert isn't always true, but it is for convex obstacles */
  /* ray rules:
   * 1) same obstacle: final = (minPt, maxPt)
   * 2) diff. obstacle: final = (minPt, p1) and (p2, maxPt) */
  if( sameOb && visible ) {
    AddRay(minPt, maxPt, rays);
  } else if ( visible ) {
    /* right now, p1vis & p2vis will have the same value, but this
       may need to change later. */
    if( p1vis && !PointEqual(minPt, p1) ) {
      AddRay(minPt, p1, rays);
    }
    if( p2vis && !PointEqual(maxPt, p2) ) {
      AddRay(p2, maxPt, rays);
    }
  }
  
  /* cleanup */
  if( currPt != minPt && currPt != maxPt ) delete currPt;
}

void AddRay(Point *p1, Point *p2, EdgeInfo *rays)
{
  /*printf("Adding edge %d\n", rays->nEdges); */
  rays->edges[rays->nEdges++] = PointsToLine(p1, p2);
}

void DumpRayInfo(EdgeInfo *rays)
{
  int i;
  for( i = 0; i < rays->nEdges; i++ ) {
    printf("Ray %d: (%f, %f) to (%f, %f)\n", i,
	   rays->edges[i]->A->x, rays->edges[i]->A->y,
	   rays->edges[i]->B->x, rays->edges[i]->B->y);
  }
}

void DumpCellInfo(CellInfo *cells)
{
  int i;
  for( i = 0; i < cells->nEdges; i++ ) {
    printf("Cell Edge %d: (%f, %f) to (%f, %f)\n", i,
	   cells->edges[i]->A->x, cells->edges[i]->A->y,
	   cells->edges[i]->B->x, cells->edges[i]->B->y);
  }
}

#define DISPLACEMENT 1.1

int OpenEdgeTest(Obstacle *ob, Point *p1, Point *p2)
{
  /* if (p1, p2) is extended by delta in p2's direction, is this point
   * inside obstacle ob (the obstacle p2 is part of) */
  
  float delta = DISPLACEMENT;
  double dot1, dot2;
  int i;
  Point newPt;
  Line *l;
  newPt.x = p1->x + (p2->x - p1->x) * delta;
  newPt.y = p1->y + (p2->y - p1->y) * delta;
  newPt.w = 1;
  
  /* loop over edges in the obstacle, ignoring edges p2 is not part of.
     if the dot product sign differs for the two edges p2 is on, then
     the line connecting p1 to p2 is open in that direction.  Otherwise,
     the line connecting these points is closed.
     
     Right now I don't make a distinction between being on the 
     positive side or the negative side of an obstacle because I want
     to avoid dealing with directed edges.  This may have to change.
     */
  
  for( i = 0; i < ob->nPts; i++ ) {
    if( &ob->pts[i] == p2 ) break;
  }
  assert( i < ob->nPts );
  l = PointsToLine(&ob->pts[i], &ob->pts[( (i + 1) % ob->nPts)]);
  dot1 = l->a * newPt.w + l->b * newPt.x + l->c * newPt.y;
  delete l;
  
  l = PointsToLine(&ob->pts[( (i - 1 + ob->nPts) % ob->nPts)],
		   &ob->pts[i]);
  dot2 = l->a * newPt.w + l->b * newPt.x + l->c * newPt.y;
  delete l;
	
  /* what to do if a dot product = 0 (ie colinear?) */
  if( (FloatCompare(dot1, 0.0) > 0 && FloatCompare(dot2, 0.0) > 0) ||
      (FloatCompare(dot1, 0.0) < 0 && FloatCompare(dot2, 0.0) < 0) ) {
    return(FALSE); /* ray is blocked */
  } else return(TRUE); /* ray is clear */
}

/* like NotEdgeTest but works for concave obstacles */
int InsideTest(Line *l, Point *p1, Point *p2)
{
  Point newPt;
  newPt.x = (p1->x + p2->x) / 2;
  newPt.y = (p1->y + p2->y) / 2;
  newPt.w = 1;

  if(FloatCompare(l->a * newPt.w + l->b * newPt.x + l->c * newPt.y,
		  0.0) > 0 ) {
    return(FALSE);
  } else {
    return(TRUE);
  }
}

/* Majorly overhauled this function: 
 * now it checks the corner near each point and sees if the other point is
 * 'inside' in relation to this corner, meaning the the dot product is
 * positive at this corner for both points.
 */

int NotEdgeTest(Point *p1, Point *p2, Obstacle *ob, Line *l1a, Line *l1b, 
		Line *l2a, Line *l2b, int bBorder)
{
  double lastSide, currSide;
  int t1, t2, t3, t4;
  double d1, d2, d3, d4;
  int i;
  Line *l;
  Point pMid;

  if( bBorder ) { // border, use inside testing 
    d1 = l1a->a * p2->w + l1a->b * p2->x + l1a->c * p2->y;  
    d2 = l1b->a * p2->w + l1b->b * p2->x + l1b->c * p2->y;  
    d3 = l2a->a * p1->w + l2a->b * p1->x + l2a->c * p1->y;  
    d4 = l2b->a * p1->w + l2b->b * p1->x + l2b->c * p1->y;  

    t1 = FloatCompare(d1,0); 
    t2 = FloatCompare(d2,0);  
    t3 = FloatCompare(d3,0); 
    t4 = FloatCompare(d4,0); 

    if( ((t1 >= 0) && (t2 >= 0)) && ((t3 >= 0) && (t4 >= 0)) ) {
      return(TRUE);
    }
  }

  assert(ob != NULL);

  pMid.x = (p1->x + p2->x) / 2;
  pMid.y = (p1->y + p2->y) / 2;
  pMid.w = 1;
  assert( FloatEqual(p1->w,1) && FloatEqual(p2->w,1) );