cell.c

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

  
  lastSide = 0;
  for( i = 0; i < ob->nPts; i++ ) {
    l = PointsToLine(&ob->pts[i], &ob->pts[(i + 1) % ob->nPts] );
    currSide = l->a * pMid.w + l->b * pMid.x + l->c * pMid.y;
    if( (PointEqual(l->A, p1) && PointEqual(l->B, p2) ) ||
	(PointEqual(l->B, p1) && PointEqual(l->A, p2) ) ) {
      delete l;
      return(TRUE);
    }
    delete l;
    if( FloatEqual(currSide, 0.0) ) {
      /* close enough to be on an edge */
      return(TRUE);
    } else if( FloatCompare(lastSide, 0.0) <= 0 &&
	       FloatCompare(currSide, 0.0) < 0 ) {
      lastSide = -1;
    } else if( FloatCompare(lastSide, 0.0) >= 0 &&
	       FloatCompare(currSide, 0.0) > 0 ) {
      lastSide = 1;
      /*} else if( currSide != 0 ) {
	return(TRUE); */	
    } else {
      return(TRUE); /* even if currSide = 0 */
    }
  }

  /* if we get this far, we are inside the obstacle. */
  return(FALSE);
}

int IsPointAdjacent(Obstacle *ob, Point *p1, Point *p2)
{
  int i;
  for(i = 0; i < ob->nPts; i++ ) {
    if( &ob->pts[i] == p1 )
      if( &(ob->pts[(i + 1) % ob->nPts]) == p2 )
	return(TRUE);
    if( &ob->pts[i] == p2 )
      if( &(ob->pts[(i + 1) % ob->nPts]) == p1 )
	return(TRUE);
  }
  return(FALSE);
}


/* convert rays into cell Edges by finding every possible intersection 
   and separating each edge at each intersection.  Also we introduce
   two edges for each original edge (one in each direction) because each
   edge will be used as a border for 2 cells (1 clockwise, 1 
   counterclockwise.) */
void MakeCellEdges(WorldInfo *wInfo, EdgeInfo *rays, CellInfo *cInfo)
{
  int i,j;
  
  double dist;
  Point *currPt;
  CellEdge *currEdge;
  CellEdge *currEdgeDual;
  int nPts; Point **ptArr;

  nPts = 0;
  ptArr = new (Point *)[MAX_INTERSECTIONS];

  cInfo->nEdges = 0;
  cInfo->edges = new (CellEdge *)[4 * MAX_INTERSECTIONS * MAX_OBS];

  currPt = NewPoint();
	
  for( i = 0; i < rays->nEdges; i++ ) {
    nPts = 0;
    for( j = 0; j < rays->nEdges; j++ ) {
      if( i == j ) continue;
      LineIntersectionDistance(rays->edges[i], 
			       rays->edges[j], currPt, &dist);
      if( !PointOnSegment(rays->edges[i], currPt))
	continue;
      InsertPoint(currPt, dist, ptArr, &nPts);	
      currPt = NewPoint();
    }
    /* now make edges out of point list */
    
    RemoveDuplicates(ptArr, &nPts);
    for( j = 0; j < nPts - 1; j++ ) {
      currEdge = new CellEdge(ptArr[j], ptArr[j+1]);	
      currEdgeDual = new CellEdge(ptArr[j+1],
				  ptArr[j]);
      currEdge->dual = currEdgeDual;
      currEdgeDual->dual = currEdge;
      currEdge->ob = currEdgeDual->ob = NULL;
      currEdge->cell = currEdge->dualCell = NULL;
      currEdgeDual->cell = currEdgeDual->dualCell = NULL;
      cInfo->edges[cInfo->nEdges++] = currEdgeDual;
      
      cInfo->edges[cInfo->nEdges++] = currEdge;
    }
  }
  delete ptArr;
}

void InsertPoint(Point *p, double dist, Point **ptArr, int *nPts)	
{
  int i;

  p->w = dist; /* save storage space */
  for(i = *nPts; i > 0; i-- ) {
    if( FloatCompare(p->w, ptArr[i - 1]->w) > 0 ) {
      ptArr[i] = p;
      (*nPts)++;
      return;
    }
    ptArr[i] = ptArr[i - 1];
  }
  ptArr[0] = p;
  (*nPts)++;
}

void RemoveDuplicates(Point **ptArr, int *nPts)
{
  int i, j;
  for(i = 0; i < (*nPts); i++) {
    while( i + 1 < (*nPts) && FloatEqual(ptArr[i]->w, 
					 ptArr[i + 1]->w) ) {
      for( j = i + 1; j < (*nPts) - 1; j++ ) {
	ptArr[j] = ptArr[j + 1];
      }
      (*nPts)--;
    }
  }
  for( i = 0; i < (*nPts); i++ ) {
    ptArr[i]->w = 1;
  }
}

void SortEdges(CellEdge **edges, int nEdges)
{
  int minElem;
  CellEdge *temp;
  int i, j;
  for(i = 0; i < nEdges; i++) {
    VerifyLine(edges[i]);
    minElem = i;
    for( j = i; j < nEdges; j++) {
      if( FloatCompare(edges[j]->A->x,
		       edges[minElem]->A->x) > 0 ) continue;
      if( FloatEqual(edges[j]->A->x,
		     edges[minElem]->A->x) &&
	  FloatCompare(edges[j]->A->y,
		       edges[minElem]->A->y) > 0 ) continue;
      minElem = j;
    }	
    temp = edges[i];
    edges[i] = edges[minElem];
    edges[minElem] = temp;
  }
}

void RemoveEdge(CellInfo *cells, int i)
{
  int j;
  assert( i < cells->nEdges );
  for(j = i; j < cells->nEdges - 1; j++) {
    cells->edges[j] = cells->edges[j + 1]; 
  }
  cells->nEdges--;
}

void MakeCells(WorldInfo *wInfo, CellInfo *cellInfo)
{
  int i, j;
  int iList[CELL_INTS];
  Cell *c;
  int bRemoved,bBorder;
  
  cellInfo->cells = new (Cell *)[CELL_INTS];
  cellInfo->nCells = 0;
  bRemoved = FALSE;

  while(cellInfo->nEdges > 0) {
    i = 0;
    cellInfo->cells[cellInfo->nCells++] = new Cell;
    c = cellInfo->cells[cellInfo->nCells - 1];
    
    c->edges = new (CellEdge *)[CELL_INTS];
    c->nEdges = 1;
    c->edges[0] = cellInfo->edges[0];
    iList[0] = i /* = 0 */;
    
    while(TRUE) {
      assert(c->nEdges < CELL_INTS);
      /*printf("Edge: (%f, %f), (%f, %f)\n",
	cellInfo->edges[i]->A->x,
	cellInfo->edges[i]->A->y,
	cellInfo->edges[i]->B->x,
	cellInfo->edges[i]->B->y);*/ 
      i = NextClockwiseEdge(cellInfo,
			    cellInfo->edges[i]->B, i);
      if( i == 0 ) break;
      iList[c->nEdges++] = i;
    }
    for( i = 0; i < c->nEdges; i++ ) {
      assert(iList[i] < cellInfo->nEdges);
      c->edges[i] = cellInfo->edges[iList[i]];
      c->edges[i]->cell = c;
      c->edges[i]->dual->dualCell = c;
      RemoveEdge(cellInfo, iList[i]);
      for( j = i; j < c->nEdges; j++ ) {
	if( iList[j] > iList[i] ) {
	  iList[j]--;
	}
      }
    }
    /* if the cell we just made is actually the border, omit it. */
    /* this will cause problems if we ever get a world that can
       be solved with only 1 cell, but that isn't a very useful
       case so ignoring it is OK. */
    if(IsCellObstacle(wInfo, c, &bBorder) ) {
      if( bBorder && !bRemoved ) {
        bRemoved = TRUE;
      } else if ( bBorder && bRemoved ) {
        c->pt = CellCenter(c);
        continue;
      }
      cellInfo->nCells--; /* delete this cell */
      for( i = 0; i < c->nEdges; i++ ) {
	c->edges[i]->dual->dualCell = NULL;
	c->edges[i] = NULL;
      }
      delete c->edges;
      delete c;
    } else {
      c->pt = CellCenter(c);
    }
  }
}

int NextClockwiseEdge(CellInfo *cellInfo, Point *p, int i)
{
  int j, k;
  CellEdge *e;
  int bestEdge;
  float angle, bestAngle = -INFINITY;
  
  for( j = 0; j < cellInfo->nEdges; j++ ) {
    e = cellInfo->edges[j];
    if( PointEqual(e->A, p) ) break;
  }
	
  if( j >= cellInfo->nEdges ) {
    printf("NextClockwiseEdge: no compatible edge found.\n");
    assert(0);
  }

  /* now iterate over all edges with this endpoint & find one
     w. lowest dot product. */
  for( k = j; k < cellInfo->nEdges && 
	 PointEqual(e->A, p);k++ ) {
    if( i == k ) continue;
    e = cellInfo->edges[k];
    if( !PointEqual(e->A, p) ) break;
    angle = LineAngle(cellInfo->edges[i], e);
    if( angle > bestAngle && angle < 180) {
      bestAngle = angle;
      bestEdge = k;
    }
  }

  if( bestAngle == -INFINITY || bestAngle >= 180 ||
      bestAngle <= -180 ) {
    printf("NextClockwiseEdge: no compatible edge found. \n");
    assert(0);
  }
  return( bestEdge );
}

/* returns TRUE if every edge in this cell corresponds to an edge in
   either the border or an obstacle. */
int IsCellObstacle(WorldInfo *wInfo, Cell *cell, int *bBorder)
{
  int i,j,k, bOK, bSame;
  Point *p1, *p2;
  Obstacle *border = wInfo->border;
  *bBorder = FALSE;

  /* a hack to catch implausible obstacles */
  if( cell->nEdges > 20 ) {
    /*printf("Deleting illegal cell\n"); */
    return(TRUE); 
  }

  for( i = 0; i < border->nPts; i++) {
    bOK = FALSE;
    for( j = 0; !bOK && j < cell->nEdges; j++ ) {
      if( PointEqual(cell->edges[j]->A,
		     &border->pts[i]) ) {
        *bBorder = TRUE;
	bOK = TRUE;
      }
    }
    if( !bOK ) break;
  }	
  if(bOK) return TRUE;

  /* now check obstacles */
  for( k = 0; k < wInfo->nObs; k++ ) {
    border = wInfo->obs[k];
    for( i = 0; i < border->nPts; i++ ) {
      bOK = FALSE;
      for( j = 0; !bOK && j < cell->nEdges; j++ ) {
	if( PointEqual(cell->edges[j]->A,
		       &border->pts[i]) ) {
	  bOK = TRUE;
	}
      }
      if( !bOK ) break;
    }
    if( bOK ) return TRUE;
  }

  return(FALSE);
}

/* use vertex removal to reduce cell to a line or triangle; then interpolate 
 * to find center. */
Point *CellCenter(Cell *cell)
{
  int i,j;
  Point *p;
  double x, y;
  
  x = y = 0;
  for( i = 0; i < cell->nEdges; i++ ) {
    x += cell->edges[i]->A->x;
    y += cell->edges[i]->A->y;
  }
  p = NewPoint();
  p->x = x / cell->nEdges;
  p->y = y / cell->nEdges;
  return(p);
}

/* function that removes useless rays that will appear whenever we have
 * singularities in our environment. Does a N^2 pass through the rays
 * and collapses any two rays that are colinear and overlapping into
 * a single ray.  The new ray replaces the first of the two in the array
 * and the second is deleted. 
 */

void CollapseRays(EdgeInfo *rays)
{
  int i,j;
  Point *pmin, *pmax;
  for( i = 0; i < rays->nEdges; i++ ) {
    for( j = 0; j < rays->nEdges; j++ ) {
      if( i == j ) continue;
      if( LinesOverlap(rays->edges[i], rays->edges[j], &pmin, &pmax) ) {
	/*1) make new ray from pmin to pmax
	  2) delete old rays i & j
	  3) insert new ray from pmin to pmax. */
	if( i < j ) {
	  delete rays->edges[j];
	  rays->edges[j] = new Line(pmin, pmax);
	  RemoveRay(rays, i);
	} else {
	  delete rays->edges[i];
	  rays->edges[i] = new Line(pmin, pmax);
	  RemoveRay(rays, j);
	}
      }
    }
  }
}

void RemoveRay(EdgeInfo *rays, int i)
{
  int j;
  assert( i < rays->nEdges );
  delete rays->edges[i];
  for(j = i; j < rays->nEdges - 1; j++) {
    rays->edges[j] = rays->edges[j + 1]; 
  }
  rays->nEdges--;
}

void RemoveCell(CellInfo *cells, int i)
{
  int j, k;
  assert( i < cells->nCells );
  for( k = 0; k < cells->cells[i]->nEdges; k++ ) {
    // invalidate dual information.
    cells->cells[i]->edges[k]->dual->dual = NULL;
    cells->cells[i]->edges[k]->dual->dualCell = NULL;
  }
  for(j = i; j < cells->nCells - 1; j++) {
    cells->cells[j] = cells->cells[j + 1]; 
  }
  cells->nCells--;
}

EdgeInfo::~EdgeInfo() {
  int i;
  for(i = 0; i < nEdges; i++) {
    delete edges[i];
  }
  delete edges;
}

/* returns the line which has p as it's endpoint B. */
Line *FindEdgeB(EdgeInfo *edges, Point *p)
{
  int i;
  for( i = 0; i < edges->nEdges; i++ ) {
    if(PointEqual(edges->edges[i]->B, p) ) {
      return(edges->edges[i]);
    }
  }
  assert(0); // should always find something
  return(NULL);
}