rebuild.c

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

/* rebuild.C 
 * Brian O'Connor
 *
 * Functions to build a new environment given the original one.
 * The new environment will be the old environment minus the area 
 * that the original pursuer was able to search in that environment.
 */

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

Line *FindDirtyBorder(PInfo *pinfo);
Line *FindDirtyEdge(PInfo *pinfo, Point *p);
int GapEdgeIntersection(PInfo *pinfo, Line *last, Line **next);
Line *FindNextEdge(PInfo *pinfo, Line *last, Point *p);
Line *DirtyEdgeSegment(PInfo *pinfo, Line *edge, Point *p);
Line *DirtyCorner(PInfo *pinfo, int hit1, int hit2, Point *p);
void  AddUniquePt(Obstacle *border, Line *next);
int  TestClosure(Line *next, Obstacle *border);

/* starting from a dirty gap edge in the old environment, traverse the 
 * edges until we make a loop in the new environment. */

int RebuildInput(PInfo *pinfo, WorldInfo *wInfo)
{
  Line *start;
  Line *next;
  Line *last;
  int bGap;

  wInfo->nObs = 0;
  wInfo->robot = NULL;
  wInfo->obs = new (Obstacle *);
  wInfo->obs[0] = NULL;
  wInfo->border = new Obstacle;
  wInfo->border->pts = new Point[100];
  wInfo->border->nPts = 0;

  next = NULL;
  bGap = TRUE;
  start = FindDirtyBorder(pinfo);
  last = start;

  wInfo->border->pts[wInfo->border->nPts++] = *last->B;
  wInfo->border->pts[wInfo->border->nPts++] = *last->A;
  while( next != start ) {

    if( bGap ) {
      next = FindDirtyEdge(pinfo, last->A);
      if( TestClosure(next, wInfo->border) ) {
	return(TRUE);
      }
      AddUniquePt(wInfo->border, next);
      bGap = FALSE;
    } else {
      if( !GapEdgeIntersection(pinfo, last, &next) ) {
	next = FindNextEdge(pinfo, last,
			    &wInfo->border->pts[wInfo->border->nPts - 1]);
	if( TestClosure(next, wInfo->border) ) {
	  return(TRUE);
	}
	AddUniquePt(wInfo->border, next);
	bGap = FALSE;
      } else {
	// change our last endpont and set the new endpoint.
	if( PointOnSegment(last, next->A)) {
	  wInfo->border->pts[wInfo->border->nPts-1] = *next->A;
	  wInfo->border->pts[wInfo->border->nPts++] = *next->B;
	} else {
	  assert( PointOnSegment(last, next->B) );
	  wInfo->border->pts[wInfo->border->nPts-1] = *next->B;
	  wInfo->border->pts[wInfo->border->nPts++] = *next->A;	  
	}
	bGap = TRUE;
      }
    }
    
    last = next;
  }
  
  return(TRUE);
}

/* iterate through the 'best' vpoly we found in this pinfo and select a 
 * dirty gap edge; this will be the starting edge for the new environment.
 */

Line *FindDirtyBorder(PInfo *pinfo)
{
  VPoly *vpoly;
  int i, gapNum;

  gapNum = 0;
  vpoly = pinfo->pg->Best()->NodeCell()->vPoly;

  // clear marks since we need to use them again
  for( i = 0; i < vpoly->nEdges; i++ ) {
    if( vpoly->edges[i]->bGap ) {
      vpoly->edges[i]->bMark = FALSE;
    }
  }


  for( i = 0; i < vpoly->nEdges; i++ ) {
    if( vpoly->edges[i]->bGap ) {
      if( pinfo->pg->Best()->NodeState() & ( 1 << gapNum ) ) {
	vpoly->edges[i]->bMark = TRUE;
	return( vpoly->edges[i] );
      }
      gapNum++;
    }
  }

  assert(0); // there should always be 1 dirty gap edge in the vpoly

}


/* p will be a pt on a gap edge; find the edge it is a part of and return
   the portion of that edge that was not covered by the vispoly we 
   have in pinfo.
   */
Line *FindDirtyEdge(PInfo *pinfo, Point *p)
{
  EdgeInfo *e;
  int i;
  int nHits, hits[2];

  e = pinfo->edgeInfo;
  nHits = 0;
  for( i = 0; i < e->nEdges; i++ ) {
    if( PointOnSegment(e->edges[i], p)) {
      hits[nHits++] = i;
    }
  }
  assert( nHits > 0 );
  if( nHits == 1 ) {
    return( DirtyEdgeSegment(pinfo, e->edges[hits[0]], p));
  } else {
    assert( nHits == 2 );
    return( DirtyCorner(pinfo, hits[0], hits[1], p) );
  }
}

/* split edge at p; one part was in the old vpoly and we want to return
   the other part. */

Line *DirtyEdgeSegment(PInfo *pinfo, Line *edge, Point *p)
{
  int i;
  VPoly *vpoly;
  Point *p1, *p2;
  p1 = new Point;
  p2 = new Point;

  vpoly = pinfo->pg->Best()->NodeCell()->vPoly;
  for( i = 0; i < vpoly->nEdges; i++ ) {
    if( LinesOverlap(edge, vpoly->edges[i], &p1, &p2) ) {
      if( PointEqual(vpoly->edges[i]->A, p) ) {
	p1 = vpoly->edges[i]->B;
      } else if( PointEqual(vpoly->edges[i]->B, p)) {
	p1 = vpoly->edges[i]->A; 
      } else continue;

      if( LineDistance(p, edge->A) > LineDistance(p1, edge->A) ) {
	// means vpoly is moving away from A
	return new Line(p, edge->B);
      } else {
	return new Line(p, edge->A);
      }
    }
  }

  assert(0); // should have been an overlap somewhere
  return(NULL);

}

/* p is the corner of 2 edges; one of these edges will be in vpoly and
   we want to return the other edge. */
Line *DirtyCorner(PInfo *pinfo, int hit1, int hit2, Point *p)
{
  VPoly *vpoly;
  int i;
  Point *p1, *p2;
  p1 = new Point;
  p2 = new Point;

  vpoly = pinfo->pg->Best()->NodeCell()->vPoly;
  for( i = 0; i < vpoly->nEdges; i++ ) {
    if( LinesOverlap(pinfo->edgeInfo->edges[hit1], vpoly->edges[i], &p1,
		     &p2)) {
      // hit1 is in the vpoly, so return hit2.
      return( pinfo->edgeInfo->edges[hit2] );
    } else if( LinesOverlap(pinfo->edgeInfo->edges[hit2], vpoly->edges[i],
			    &p1, &p2)) {
      // hit 2 is in the vpoly, so return hit1.
      return( pinfo->edgeInfo->edges[hit1] );
    }
  }

  assert(0); // should have found a hit

}

// check every gap edge in vpoly and see if one intersects last.
int GapEdgeIntersection(PInfo *pinfo, Line *last, Line **next)
{

  VPoly *vpoly;
  int i;

  vpoly = pinfo->pg->Best()->NodeCell()->vPoly;
  for( i = 0; i < vpoly->nEdges; i++ ) {
    if( vpoly->edges[i]->bGap && !vpoly->edges[i]->bMark &&
	(PointOnSegment(last, vpoly->edges[i]->A) ||
	PointOnSegment(last, vpoly->edges[i]->B)) ) {
      // we found a gap edge intersecting this edge.
      vpoly->edges[i]->bMark = TRUE;
      *next = vpoly->edges[i];
      return(TRUE);
    }
  }

  return(FALSE);
}

/* given an edge from the original decomposition return the next edge
   in A to B direction. */
Line *FindNextEdge(PInfo *pinfo, Line *last, Point *p)
{
  EdgeInfo *e;
  int i;

  e = pinfo->edgeInfo;
  for( i = 0; i < e->nEdges; i++ ) {
    if( PointOnSegment(e->edges[i], p) && e->edges[i] != last) {
      return( e->edges[i] );
    }
  }

  // should always find a match
  assert(0);

}

/* one point on next should match the last point in the border; so add the
   other point. */
void  AddUniquePt(Obstacle *border, Line *next)
{
  Point *p;
  p = &border->pts[border->nPts - 1];
  if( PointEqual(p, next->A) ) {
    border->pts[border->nPts++] = *next->B;
  } else {
    assert( PointEqual(p, next->B) );
    border->pts[border->nPts++] = *next->A;
  }
}

/* if the line we just added was the last in the border, we need to add
 * the last point and exit. */
int  TestClosure(Line *next, Obstacle *border)
{
  if( PointOnSegment(next, &border->pts[0]) ) {
    return(TRUE);
  }
  return(FALSE);
}