line.c

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

/* Brian O'Connor
 * 
 * line.c: functions for working with lines 
 * IMPORTANT: all Points are assumed to be homogenous in this module. 
 */

#include <assert.h>
#include <stdio.h>
#include <math.h>
#include "input.h"
#include "line.h"

/* create a new line from 2 pts */
/* remember that this module uses Lines as if they are in sorted order,
 * with the line of direction travelling from a to b. */

/* C++ version */
Line::Line(Point *p1, Point *p2)
{
	A = p1;
	B = p2;
	/* ob is set by caller */

	assert(FloatEqual(p1->w, 1.0) && FloatEqual(p2->w, 1.0));
	a = p1->x * p2->y - p1->y * p2->x;
	b = p1->y - p2->y;
	c = p2->x - p1->x;

	length = LineDistance(p1, p2);
	assert( FloatCompare(length, 0.0) > 0 );
}

/* C version */
Line *PointsToLine(Point *p1, Point *p2)
{
	Line *newLine;

	newLine = new Line;

	newLine->A = p1;
	newLine->B = p2;
	/* newLine->ob is set by calling module */

	newLine->a = p1->x * p2->y - p1->y * p2->x;
	newLine->b = p1->y - p2->y;
	newLine->c = p2->x - p1->x;

	newLine->length = LineDistance(p1, p2);
	assert( FloatCompare(newLine->length, 0.0) > 0 );

	return(newLine);
}

/* return TRUE if segments intersect w. intersection point returned in
	result */
int SegmentIntersection(Line *l1, Line *l2, Point *result)
{
	LineIntersection(l1, l2, result);
	if( FloatEqual(result->w, 0.0) ) {
	  /* special case: l1, l2 are parallel */
	  if( PointEqual(l1->A, l2->A) || PointEqual(l1->A, l2->B) ) {
	    *result = *l1->A;
	    return(TRUE);
	  } else if( PointEqual(l1->B, l2->A) || PointEqual(l1->B, l2->B)) {
	    *result = *l1->B;
            return(TRUE);
	  } else {
	    return(FALSE);
	  }
	}
	return( PointOnSegment(l1, result) && PointOnSegment(l2, result) ); 
	
}

int LineSegIntersection(Line *l, Line *s, Point *result)
{
        LineIntersection(l, s, result);
        return( PointOnSegment(s, result));
}

/* return Point where the given lines intersect in result */ 
void LineIntersection(Line *l1, Line *l2, Point *result)
{
	result->w = l1->b * l2->c - l1->c * l2->b;
	result->x = l2->a * l1->c - l1->a * l2->c;
	result->y = l1->a * l2->b - l1->b * l2->a;

/* convert to site notation if intersection is at a finite point */
	if( !FloatEqual(result->w, 0.0) ) {
		result->x = result->x / result->w;
		result->y = result->y / result->w;
		result->w = 1.0;
	} 
}

/* return Point where the given lines intersect in result, and 
   return the distance from l1->A endpoint to result (positive 
   direction is direction from A to B).  */
void LineIntersectionDistance(Line *l1, Line *l2, Point *result, double *dist)
{
	double d, d2, dbase;

	LineIntersection(l1, l2, result);
	/* calculate dist:
	d = distance from l1->A to result, and if
	result is closer to l1->A than to l1->B, dist = -d. */

	d = LineDistance(l1->A, result);
	d2 = LineDistance(l1->B, result);
	dbase = LineDistance(l1->A, l1->B);


	/* if: between A & B, return positive distance from A
 	 *     beyond B, return positive distance from A
         *     behind A, return negative distance from A */
	if( FloatCompare(d,d2) < 0 && FloatCompare(d2, d + dbase) >= 0 ) {
		*dist = -d;
	}
	else *dist = d;
}

double LineDistance(Point *p1, Point *p2)
{
	if( FloatEqual(p1->w, 0.0) || FloatEqual(p2->w, 0.0) ) {
		return INFINITY;
	}
	return( sqrt( (p2->x - p1->x) * (p2->x - p1->x) +
		      (p2->y - p1->y) * (p2->y - p1->y) ) );
}

/* test if p is inbetween l->A, l->B.  Will also return FALSE if p is
 * not on the line at all. */
int PointOnSegment(Line *l, Point *p)
{
	double d, dA, dB;
	double dot;

	dot = l->a * p->w + l->b * p->x + l->c * p->y;
	if( !FloatEqual(dot, 0.0) ) return(FALSE);

	d = (l->B->x - l->A->x) * (l->B->x - l->A->x) +
	    (l->B->y - l->A->y) * (l->B->y - l->A->y);
	dA = (p->x - l->A->x) * (p->x - l->A->x) +
	     (p->y - l->A->y) * (p->y - l->A->y);
	dB = (p->x - l->B->x) * (p->x - l->B->x) +
	     (p->y - l->B->y) * (p->y - l->B->y);

	return( FloatCompare(dA, d) <= 0 && FloatCompare(dB, d) <= 0 );
}


/* assumes that l2->A is a point on the line l1. */
double LineAngle(Line *l1, Line *l2)
{
  double result, scaledResult, angle;
  double sideTest, side;
  result = l1->b * l2->b + l1->c * l2->c; /* ignore offsets from origin */
  scaledResult = result / l1->length;
  scaledResult /= l2->length;

  VerifyLine(l1);
  VerifyLine(l2);

  if( scaledResult >= 1 && scaledResult <= 1.001 ) scaledResult = 1.0;
  if( scaledResult <= -1 && scaledResult >= -1.001 ) scaledResult = -1.0;
  assert( scaledResult <= 1 && scaledResult >= -1 );
  angle = acos(scaledResult);
  angle *= 180 / 3.14159265;

  sideTest = l1->a * l2->B->w + l1->b * l2->B->x + l1->c * l2->B->y;
  if( sideTest < 0 ) side = 1;
  else side = -1;

  angle *= side;

  /*  printf("Line 1: (%f, %f),(%f,%f)  \nLine 2: (%f, %f)i,(%f,%f)\n",
	 l1->A->x, l1->A->y, l1->B->x, l1->B->y, 
	 l2->A->x, l2->A->y, l2->B->x, l2->B->y);
  printf("dot product: %f      scaled product: %f      angle: %f\n",
	 result, scaledResult, angle); */

  return(angle);
}


void VerifyLine(Line *l)
{
/* 	l dot A = 0 and l dot B = 0 */

	double eq1, eq2;
	eq1 = l->a * l->A->w + l->b * l->A->x + l->c * l->A->y;
	eq2 = l->a * l->B->w + l->b * l->B->x + l->c * l->B->y;

	assert( FloatEqual(eq1, 0.0) && FloatEqual(eq2, 0.0) );
}

Point *NewPoint(void)
{
	Point *p;
	p = new Point;
	p->w = 1;
	return(p);
}

/* do dot products of a bunch of lines from all quadrants with l & see
/* what we get. */

extern void DotProductTest(void)
{
  Line *lines[10];
  Line *l;
  int i;
  
  Point p[10];

  for(i = 0; i < 10; i++) {
    p[i].w = 1;
  }

  p[0].x = p[0].y = 0;
  p[1].x = 0; p[1].y = -1;
  p[2].x = 1; p[2].y = -3;
  p[3].x = 2; p[3].y = 0;
  p[4].x = 2; p[4].y = 2;
  p[5].x = -1; p[5].y = 3;
  p[6].x = -1; p[6].y = 0;
  p[7].x = -2; p[7].y = -1;

  l = PointsToLine(&p[1], &p[0]);
  lines[0] = PointsToLine(&p[0], &p[1]);
  lines[1] = PointsToLine(&p[0], &p[2]);
  lines[2] = PointsToLine(&p[0], &p[3]);
  lines[3] = PointsToLine(&p[0], &p[4]);
  lines[4] = PointsToLine(&p[0], &p[5]);
  lines[5] = PointsToLine(&p[0], &p[6]);
  lines[6] = PointsToLine(&p[0], &p[7]);

  LineAngle(l, l);
  LineAngle(l, lines[0]);
  LineAngle(l, lines[1]);
  LineAngle(l, lines[2]);
  LineAngle(l, lines[3]);
  LineAngle(l, lines[4]);
  LineAngle(l, lines[5]);
  LineAngle(l, lines[6]);
}

int FloatCompare(double m, double n)
{
	double f1, f2;
	f1 = m;
	f2 = n;
	if( m > n - PRECISION && m < n + PRECISION ) return(0);
	if( m < n ) return(-1);
	assert(m > n );
	return(1);
}

int FloatEqual(double m, double n)
{
	double f1, f2;
	f1 = m; f2 = n;

	if( m > n - PRECISION && m < n + PRECISION ) return(1);
	return(0);
}

int PointCompare(Point *p1, Point *p2)
{
	if( FloatEqual(p1->x, p2->x) ) {
		if( FloatEqual(p1->y, p2->y) ) {
			return(0);
		}
	}
	if( FloatCompare(p1->x, p2->x) < 0 ) return -1;
	if( FloatCompare(p1->x, p2->x) > 0 ) return 1;
	return( FloatCompare(p1->y, p2->y) );
}

int PointEqual(Point *p1, Point *p2)
{
	return( PointCompare(p1, p2) == 0 );
}

int LinesEqual(Line *l1, Line *l2)
{
  double b1, c1, b2, c2;
  b1 = l1->b / l1->a;
  c1 = l1->c / l1->a;
  b2 = l2->b / l2->a;
  c2 = l2->c / l2->a;
  return( FloatEqual(b1, b2) && FloatEqual(c1, c2) );
}

int LinesOverlap(Line *l1, Line *l2, Point **pmin, Point **pmax)
{
  int bOverlap = FALSE;
  int i;
  double d[7];
  double maxDist;
  int maxPair;
  *pmin = NULL;
  *pmax = NULL;

  if( !LinesEqual(l1, l2) ) return FALSE;

/* 1) are normalized lines equivalent?
 * 2) does the interval l1->A, l1->B overlap l2->A, l2->B 
 */
  if( PointOnSegment(l1, l2->A) ) {
    bOverlap = TRUE;
  } else if( PointOnSegment(l1, l2->B) ) {
    bOverlap = TRUE;
  } else if( PointOnSegment(l2, l1->A) ) {
    bOverlap = TRUE;
  } else if( PointOnSegment(l2, l1->B) ) {
    bOverlap = TRUE;
  }

  if( bOverlap ) {
  /* find 2 pts that are furthest apart */
    d[1] = LineDistance(l1->A, l1->B);
    d[2] = LineDistance(l2->A, l2->B);
    d[3] = LineDistance(l1->A, l2->A);
    d[4] = LineDistance(l1->A, l2->B);
    d[5] = LineDistance(l1->B, l2->A);
    d[6] = LineDistance(l1->B, l2->B);

    maxDist = -INFINITY;
    for( i = 1; i <= 6; i++ ) {
      if(FloatCompare(d[i], maxDist) > 0 ) {
        maxDist = d[i];
        maxPair = i;
      }
    }

    switch(maxPair) {
    case 1: 
      *pmin = l1->A;
      *pmax = l1->B;
      break;
    case 2: 
      *pmin = l2->A;
      *pmax = l2->B;
      break;
    case 3: 
      *pmin = l1->A;
      *pmax = l2->A;
      break;
    case 4: 
      *pmin = l1->A;
      *pmax = l2->B;
      break;
    case 5: 
      *pmin = l1->B;
      *pmax = l2->A;
      break;
    case 6: 
      *pmin = l1->B;
      *pmax = l2->B;
      break;
    default: assert(0); 
    }
  }
  return(bOverlap);
}