pgraph.c

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

/* Brian O'Connor
 * pgraph.C
 * implementation of graph for pursuer/evader problem 
 */

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

int GapsToStates(int n)
{
  int i, result;
  result = 1;
  for( i = 0; i < n; i++ ) {
    result *= 2;
  }
  return(result);
}

PGAdjInfo::PGAdjInfo(Cell *newStart, Cell *newEnd)
{
  int i,j, bDone;

  start = newStart;
  end = newEnd;

  bDone = FALSE;
  path = new Line(newStart->pt, newEnd->pt);
  for( i = 0; !bDone && i < newStart->nEdges; i++ ) {
    for( j = 0; !bDone && j < newEnd->nEdges; j++ ) {
      if( newStart->edges[i] == newEnd->edges[j]->dual ) {
	bDone = TRUE;
	border = newStart->edges[i];
	newStart->edges[i]->adjInfo = this;
	assert(border->dualCell == newEnd);
	assert(newEnd->edges[j]->dualCell == newStart);
      }
    }
  }
  assert(bDone);

}

void PGAdjInfo::BuildTransitions(void)
{
/* calculate bottom_bmap & top_bmap (gap edges in start that intersect
 * the dividing edge above & below the connecting line). 
 * Then iterate over the gaps in the new state with the following logic:
 * 1) check if intersects dividing edge, and if so make the transition
 *    bottom_bmap or top_bmap, respectively.
 * 2) check if there is an intersection with any other gap edge from
      start.  If one is found, assert that there is only one found, 
      and also assert that the one found is not in top or bottom_bmap.
      Then set this transition to be a 1 for this gap and 0 elsewhere.
 * 3) all other transitions should be the zero transition.
 */

  int i, j, startEdgeNum, edgeNum;
  Bitmap bottomBmap, topBmap;
  Point p;
  double side;

  bottomBmap = ZeroBitmap();
  topBmap = ZeroBitmap();
  edgeNum = 0;
  for(i = 0; i < start->vPoly->nEdges; i++) {
    if( start->vPoly->edges[i]->bGap ) {
      if(LineSegIntersection(border, start->vPoly->edges[i], &p) ) {   
        /* find side of path that p lies on */
	  side = p.w * path->a + p.x * path->b + p.y * path->c;
          if(FloatCompare(side, 0) < 0 ) {
            bottomBmap = bottomBmap | ( 1 << edgeNum );
	  } else {
            assert( !FloatEqual(side, 0.0) );
	    topBmap = topBmap | ( 1 << edgeNum );
          }
      }
      edgeNum++;
    }
  }

  transitions = new Bitmap[BITMAP_SIZE];
  for( i = 0; i < BITMAP_SIZE; i++ ) {
    transitions[i] = ZeroBitmap(); 
  }

  edgeNum = 0;
  for( i = 0; i < end->vPoly->nEdges; i++ ) {
    if( end->vPoly->edges[i]->bGap ) {
      if(LineSegIntersection(border, end->vPoly->edges[i], &p) ) {   
        /* find side of path that p lies on */
	  side = p.w * path->a + p.x * path->b + p.y * path->c;
          if(FloatCompare(side, 0) < 0 ) {
            transitions[edgeNum] = bottomBmap;
	  } else {
            assert( !FloatEqual(side, 0.0) );
	    transitions[edgeNum] = topBmap;
          }
      } else {
	/* search for a start edge w. common endpt */
        startEdgeNum = 0;
        for(j = 0; j < start->vPoly->nEdges; j++ ) {
          if( start->vPoly->edges[j]->bGap ) {
	    /*            if( SegmentIntersection(start->vPoly->edges[j],
			end->vPoly->edges[i], &p)) { */
	    if(PointEqual(end->vPoly->edges[i]->A,
			  start->vPoly->edges[j]->A) ||
	       PointEqual(end->vPoly->edges[i]->A,
			  start->vPoly->edges[j]->B) ||
	       PointEqual(end->vPoly->edges[i]->B,
			  start->vPoly->edges[j]->A) ||
	       PointEqual(end->vPoly->edges[i]->B,
			  start->vPoly->edges[j]->B) ) { 
	      if(transitions[edgeNum] != (Bitmap)0 ) {
		printf("duplicate cross\n");
		assert(0);
	      }
              transitions[edgeNum] = (1 << startEdgeNum);
            }
            startEdgeNum++;
          }
        }
      }
    edgeNum++;
    }
  }
}

/* only meant to be called after end has been added to node list! */
void PGAdjInfo::BuildArcs(PursuerGraph *pg)
{
  int i;
  PGNode *node;
   
  for( i = 0; i < GapsToStates(start->nGapEdges) ; i++ ) {
    node = pg->FindNode(start, i);
    assert( node != NULL );
    assert(0); // for safety
    BuildArc(node, end->nGapEdges, pg);
  }
}

void PGAdjInfo::BuildArc(PGNode *nodeStart, int nTransitions, 
			  PursuerGraph *pg)
{
  Bitmap resultState;
  Bitmap currBit;
  int i; 
  PGNode *p;

  resultState = ZeroBitmap();
  for( i = nTransitions - 1; i >= 0; i-- ) {
    resultState = resultState << 1;
    if( transitions[i] & nodeStart->NodeState() ) {
      resultState = resultState | 1;
    }
  }
  if( (p = pg->FindNode(end, resultState)) == NULL ) {
    p = pg->AddNode(end, resultState);
  }
  nodeStart->AddTransition(p, this);
}


PGArc::PGArc(PGNode *newStart, PGNode *newEnd, PGAdjInfo *newAdjInfo)
{
  start = newStart;
  end = newEnd;
  adjInfo = newAdjInfo;
}



PGNode::PGNode(Cell *cell, Bitmap newState, int newNumStates)
{
  int i;

  c = cell;
  nStates = newNumStates;
  state = newState;

  /*  printf("New node centered at (%f, %f)\n", cell->pt->x, cell->pt->y);
  PrintBitmap(state); */

  nArcs = 0;
  arcs = new (PGArc *)[CountPossibleStates(newNumStates, cell)];
}

void PGNode::AddTransition(PGNode *dest, PGAdjInfo *adjInfo)
{
  arcs[nArcs++] = new PGArc(this, dest, adjInfo);
  assert(adjInfo->Start() == c);
  assert(adjInfo->End() == dest->c);
}

int PGNode::CountPossibleStates(int newNumStates, Cell *cell)
{
  int i;
  int factor = 1;
  int nEdges = 0;

  for( i = 0; i < newNumStates; i++ ) {
    factor *= 2;
  }

  for(i = 0; i < cell->nEdges; i++) {
    if( cell->edges[i]->dualCell != NULL ) {
      nEdges++;
    }
  }

  if( factor == 1 ) {
    /* special case: no gap edges (we're done) */
    return(1);
  }

  return( factor * nEdges + 1 );
}

PursuerGraph::PursuerGraph(CellInfo *cellInfo, int startCell)
{
  int i,j;
  int factor;

  start = cellInfo->cells[startCell];
  nNodes = 0;
  for( i = 0; i < cellInfo->nCells; i++ ) {
    factor = 1;
    for( j = 0; j < cellInfo->cells[i]->vPoly->nEdges; j++ ) {
      if( cellInfo->cells[i]->vPoly->edges[j]->bGap ) factor *= 2;
    }
    nNodes += factor;
  }
  nodes = new (PGNode *)[nNodes + 1];

  adjInfoArr = new (PGAdjInfo *)[cellInfo->nCells * cellInfo->nCells * 2];
  nAdj = 0;

  nNodes = 0;
  best = NULL;
  //BuildNodeArr(cellInfo);
  //BuildAdjInfoArr(cellInfo);
  

}

PGNode *PursuerGraph::AddNode(Cell *c, Bitmap state)
{
  int j, nGaps, nCombos;
  VPoly *vpoly;

  nGaps = 0;
  vpoly= c->vPoly;
  for( j = 0; j < vpoly->nEdges; j++ ) {
    if( vpoly->edges[j]->bGap ) nGaps++;
  }
  c->nGapEdges = nGaps;
  nCombos = GapsToStates(nGaps);
  
  if( nCombos == 1 ) {
    /* special case: no gap edges, create one state w. map = 0000000. */
    nodes[nNodes++] = new PGNode(c, (Bitmap)0, 0);
  } else {
    nodes[nNodes++] = new PGNode(c, state, nGaps);
  }
  nodes[nNodes - 1]->Reset();
  return nodes[nNodes - 1];
}

PGNode *PursuerGraph::FindNode(Cell *c, Bitmap state)
{
  int i;
  for( i = 0; i < nNodes; i++ ) {
    if( nodes[i]->NodeCell() == c && nodes[i]->NodeState() == state ) {
      return(nodes[i]);
    }
  }
  return(NULL);
}

int PursuerGraph::FindAdjInfo(Cell *c1, Cell *c2)
{
  int i;
  for( i = 0; i < nAdj; i++ ) {
    if( adjInfoArr[i]->Start() == c1 &&
	adjInfoArr[i]->End() == c2 ) return i;
  }
  return -1;
}

void PursuerGraph::BuildAdjInfoArr(CellInfo *cellInfo)
{
  int i,j;
  Cell *c;

  for( i = 0; i < cellInfo->nCells; i++ ) {
    c = cellInfo->cells[i];
    for( j = 0; j < c->nEdges; j++ ) {
      if(nAdj == 115 ) {
	printf("Debugging case.\n");
      }
      if( c->edges[j]->dualCell != NULL ) {
	//adjInfoArr[nAdj] = new PGAdjInfo(c, c->edges[j]->dualCell);
	//adjInfoArr[nAdj]->BuildTransitions();
	//  adjInfoArr[nAdj]->BuildArcs(this);
        nAdj++;
      }
    }
  }
}

void PursuerGraph::BuildNodeArr(CellInfo *cellInfo)
{
  int i,j;
  int nGaps, nCombos;
  VPoly *vpoly;

  for( i = 0; i < cellInfo->nCells; i++ ) {
    nGaps = 0;
    vpoly= cellInfo->cells[i]->vPoly;
    for( j = 0; j < vpoly->nEdges; j++ ) {
      if( vpoly->edges[j]->bGap ) nGaps++;
    }
    cellInfo->cells[i]->nGapEdges = nGaps;
    nCombos = GapsToStates(nGaps);

    if( nCombos == 1 ) {
      /* special case: no gap edges, create one state w. map = 0000000. */
      nodes[nNodes++] = new PGNode(cellInfo->cells[i], (Bitmap)0, 0);
    } else {
      for( j = nCombos - 1; j >= 0; j-- ) {
	nodes[nNodes++] = new PGNode(cellInfo->cells[i], (Bitmap)j, nGaps);
      }
    }
  }
}

void PursuerGraph::Print(void)
{
  int i,j;
  for( i = 0; i < nAdj; i++ ) {
    printf("Cell (%f, %f) to (%f, %f) :\n", 
      adjInfoArr[i]->Start()->pt->x, adjInfoArr[i]->Start()->pt->y,
      adjInfoArr[i]->End()->pt->x, adjInfoArr[i]->End()->pt->y);
    printf("border : (%f, %f) to (%f, %f)\n",
      adjInfoArr[i]->Border()->A->x, adjInfoArr[i]->Border()->A->y,
      adjInfoArr[i]->Border()->B->x, adjInfoArr[i]->Border()->B->y);
    printf("\ngap edges of start cell:\n");
    for(j = 0; j < adjInfoArr[i]->Start()->vPoly->nEdges ; j++) {
      if( adjInfoArr[i]->Start()->vPoly->edges[j]->bGap ) {
        printf("(%f, %f), (%f, %f)\n", 
          adjInfoArr[i]->Start()->vPoly->edges[j]->A->x,
          adjInfoArr[i]->Start()->vPoly->edges[j]->A->y,
          adjInfoArr[i]->Start()->vPoly->edges[j]->B->x,
          adjInfoArr[i]->Start()->vPoly->edges[j]->B->y);
      }

    }
    printf("\ngap edges of end cell:\n");
    for(j = 0; j < adjInfoArr[i]->End()->vPoly->nEdges; j++) {
      if( adjInfoArr[i]->End()->vPoly->edges[j]->bGap ) {
        printf("(%f, %f), (%f, %f)\n", 
          adjInfoArr[i]->End()->vPoly->edges[j]->A->x,
          adjInfoArr[i]->End()->vPoly->edges[j]->A->y,
          adjInfoArr[i]->End()->vPoly->edges[j]->B->x,
          adjInfoArr[i]->End()->vPoly->edges[j]->B->y);
      }
    }
    printf("\ntransition table:\n");
    for(j = 0; j < adjInfoArr[i]->End()->nGapEdges; j++) {
      PrintBitmap(adjInfoArr[i]->TransitionEntry(j));
    }
    printf("\n---\n");
  }
}

/* search uses Dijkstra's algorithm, enhanced by efficiency so that nodes
   aren't added to the search list until they have already been "seen"
   by another node. */
PGNode *PursuerGraph::Search()
{
  PGNode *n;
  SortedList *nList;
  CellEdge *e;
  int i, j;
  nList = InitSearch();

  while( !nList->IsEmpty() ) {
    n = nList->RemoveFront();
    if( FloatEqual(n->Dist(), INFINITY) ) {
      return(NULL);
    } else if( n->NodeState() == ZeroBitmap() ) {
      return(n);
    }

    
    for( i = 0, j = 0; i < n->NodeCell()->nEdges; i++ ) {
      if( n->NodeCell()->edges[i]->dualCell != NULL ) {
	e = n->NodeCell()->edges[i];
	// build adj info if necessary and add arcs.
	if( e->adjInfo == NULL ) {
	  // build adj info
	  adjInfoArr[nAdj] = new PGAdjInfo(n->NodeCell(), e->dualCell);
	  adjInfoArr[nAdj]->BuildTransitions();
	  e->adjInfo = adjInfoArr[nAdj];
	  nAdj++;
	}
	e->adjInfo->BuildArc(n, e->dualCell->nGapEdges, this);

	if( IsBest(n) ) {
	  best = n;
	}

	if( Relax(n, n->Dest(j), nList) ) {
	  return(n->Dest(j));
	}
	j++;
      }
    }      
  }
  return(NULL); // ran out of nodes, so exit
}

int NodeCmpFn(PGNode *n1, PGNode *n2)
{
  return(FloatCompare(n1->Dist(), n2->Dist()));
}


SortedList *PursuerGraph::InitSearch(void)
{
  SortedList *nList;
  int i,j, nCombos, nGaps;
  PGNode *startNode;
  VPoly *vpoly;

  /* create starting node */
  // figure out state of starting node

  nGaps = 0;
  vpoly= start->vPoly;
  for( j = 0; j < vpoly->nEdges; j++ ) {
    if( vpoly->edges[j]->bGap ) nGaps++;
  }
  nCombos = GapsToStates(nGaps);
  startNode = AddNode(start, nCombos - 1);
  startNode->SetDist(0);
  best = startNode;
  bestNum = nGaps;
  
  nList = new SortedList(NodeCmpFn);
  nList->Insert(startNode);

  #if 0 // old stuff
  /* find startNode given start cell */
  for( i = 0; i < nNodes; i++ ) {
    if( nodes[i]->NodeCell() == start ) {
      startNode = nodes[i];
      //assert(startNode->NodeState() != ZeroBitmap() );
      break;
    }
  }

  for( i = 0; i < nNodes; i++ ) {
    nodes[i]->Reset();
    if( nodes[i] == startNode ) {
      nodes[i]->SetDist(0);
    }
    nList->Insert(nodes[i]);
  }
  #endif

  return(nList);
}

int PursuerGraph::Relax(PGNode *u, PGNode *v, SortedList *list)
{
  if( v->Dist() > u->Dist() + LineDistance(u->NodePoint(),v->NodePoint()) ) {
    v->SetDist(u->Dist() + LineDistance(u->NodePoint(),v->NodePoint()));
    v->SetPre(u);
    list->Remove(v);
    list->Insert(v); /* put node in the right position */
  }
  if( v->NodeState() == ZeroBitmap() ) {
    return(TRUE);
  }
  return(FALSE);
}


/* 'best' node is defined as the node with the fewest gap edges and the 
 * least total distance.  If we can't find a solution we will continue
 * from this node with a second pursuer. */
int PursuerGraph::IsBest(PGNode *n)
{
  int i;
  int ngaps;
  for( i = 0; i < BITMAP_SIZE; i++ ) {
    if( n->NodeState() & (1 << i)) {
      ngaps++;
    }
  }
  return( ngaps < bestNum || ( ngaps == bestNum && 
	       FloatCompare(n->Dist(), best->Dist()) < 0) );
}