/*
 * $RCSfile$ $Revision$ $State$
 */
/****************************************************************************
*   File: rtmp_geom.c                                                       *
*                                                                           *
*       Copyright 1993 by Loral Advanced Distributed Simulation, Inc.       *
*                                                                           *
*               Loral Advanced Distributed Simulation, Inc.                 *
*               10 Moulton Street                                           *
*               Cambridge, MA 02238                                         *
*               617-873-1850                                                *
*                                                                           *
*       This software was developed by Loral under U. S. Government contracts*
*       and may be reproduced by or for the U. S. Government pursuant to    *
*       the copyright license under the clause at DFARS 252.227-7013        *
*       (OCT 1988).                                                         *
*                                                                           *
*       Contents:                                                           *
*       Created: Tue Nov  9 1993                                            *
*       Author: oded                                                        *
*       Remarks:                                                            *
*                                                                           *
****************************************************************************/

#include "librtmp_local.h"
#include <stdext.h>
#include <string.h>

void routemap_my_strcpy(a,b)
int8 *a;
int8 *b;
{
  int32 i;
  
  for (i=0;i<=strlen(b);i++)
    a[i]=b[i];
}


/* return 1 if a or b are within epsilon of 0, or if both a and b have the
   same sign
*/
int32 routemap_equal_sign(a,b,epsilon)
     float32 a,b,epsilon;
{
  if ((abs(a)<epsilon) || (abs(b)<epsilon))
    return (1);
  if ((a<0.0) && (b<0.0))
    return (1);
  if ((a>0.0) && (b>0.0))
    return (1);
  else
    return (0);
}

float32 routemap_dot_product(a,b)
     point *a,*b;
{
  return ((a->x*b->x) + (a->y*b->y));
}

/*
  returns euclidean distance between the points (x1,y1) and (x2,y2)
*/
float32 routemap_distance(x1,y1,x2,y2)
     float32 x1,y1,x2,y2;
{
  return (fsqrt(((x1-x2)*(x1-x2)) + ((y1-y2)*(y1-y2))));
}

/*
  returns squared euclidean distance between the points a and b
*/
float32 routemap_distance_squared(a,b)
     point *a,*b;
{
  return (((a->x-b->x)*(a->x-b->x)) + ((a->y-b->y)*(a->y-b->y)));
}

/*
  this routine uses some grungy looking algebra to figure out if the line
  segment ab intersect line segment cd
*/

#if 0


int32 routemap_line_intersection(a,b,c,d)
     point *a,*b,*c,*d;
{
  point i;
  float32 ab_dx = (a->x-b->x);
  float32 ab_dy = (a->y-b->y);
  float32 cd_dx = (c->x-d->x);
  float32 cd_dy = (c->y-d->y);

  /* Check the bounding boxes first */
  if ((my_min(a->x,b->x) > my_max(c->x,d->x))
      || (my_max(a->x,b->x) < my_min(c->x,d->x))
      || (my_min(a->y,b->y) > my_max(c->y,d->y))
      || (my_max(a->y,b->y) < my_min(c->y,d->y)))
    return(0);

  if ((SAME(ab_dx,0.)) && (SAME(cd_dx,0.))) /*they're both vertical*/
    if (a->x != c->x)
      return (0);
    else
      return (ROUTEMAP_TOTAL_WITHIN_SEGMENT(a,b,c) 
	      || ROUTEMAP_TOTAL_WITHIN_SEGMENT(a,b,d));
  if (SAME(ab_dx,0.))		/*one is vertical*/
    {
      i.x=a->x;
      i.y=((cd_dy*a->x)+(c->y*cd_dx)-(c->x*cd_dy))/cd_dx;
      return (ROUTEMAP_WITHIN_SEGMENT(c,d,&i) 
	      && ROUTEMAP_TOTAL_WITHIN_SEGMENT(a,b,&i));
    }
  if (SAME(cd_dx,0.))                       /*the other is vertical*/
    {
      i.x=c->x;
      i.y=((ab_dy*c->x)+(a->y*ab_dx)-(a->x*ab_dy))/ab_dx;
      return (ROUTEMAP_TOTAL_WITHIN_SEGMENT(a,b,&i) 
	      && ROUTEMAP_WITHIN_SEGMENT(c,d,&i));
    }
  
  if (SAME((cd_dy*ab_dx),(ab_dy*cd_dx))) /*they're parallel*/
    if (SAME(((a->y*ab_dx) + (a->x*ab_dy)),((c->y*cd_dx) + (c->x*cd_dy))))
      return (ROUTEMAP_TOTAL_WITHIN_SEGMENT(a,b,c) 
	      || ROUTEMAP_TOTAL_WITHIN_SEGMENT(a,b,d));
    else
      return (0);
  /* finally, they're not parallel, not vertical, so there's an intersection*/
  i.x=(cd_dx/(ab_dy*cd_dx-cd_dy*ab_dx)) *
    ((-a->y*ab_dx) + (a->x*ab_dy) + (c->y*ab_dx) - (c->x*ab_dx*cd_dy/cd_dx));
  i.y=((ab_dy*i.x)+(a->y*ab_dx)-(a->x*ab_dy))/ab_dx;
  return (ROUTEMAP_TOTAL_WITHIN_SEGMENT(a,b,&i) 
	  && ROUTEMAP_WITHIN_SEGMENT(c,d,&i));
}

#endif

int32 routemap_line_intersection(a,b,c,d)
     point *a,*b,*c,*d;
{
  point i;
  float32 v_x, v_y, n_x, n_y, m, cos, dist, lambda;

  /* Check the bounding boxes first */
  if ((my_min(a->x,b->x) > my_max(c->x,d->x))
      || (my_max(a->x,b->x) < my_min(c->x,d->x))
      || (my_min(a->y,b->y) > my_max(c->y,d->y))
      || (my_max(a->y,b->y) < my_min(c->y,d->y)))
    return(0);

  n_y =  (b->x - a->x);		/* AB normal */
  n_x = -(b->y - a->y);
  m = n_x * a->x + n_y * a->y;	/* normal dist of A */
  v_x = (d->x - c->x);		/* CD line vector */
  v_y = (d->y - c->y);
  cos  = n_x * v_x + n_y * v_y;	/* cos between lines */
  dist = n_x * c->x + n_y * c->y; /* normal dist of C */

  /* Any point on CD can be written as (c+lambda*v) for lambda in */
  /* the range [0,1].  Any point x on AB satisfies N.x=m */
  /* Intersection point is where n.(c+lambda*v)=m */
  /* That is (dist + lambda*cos = m), lambda = (m-dist)/cos */

  if (SAME(cos,0.)) /* they're parallel */
    if (SAME(dist,0.))		/* and collinear */
      return (1);		/* since bounding box intersect */
    else
      return (0);
  
  /* Distance along CD to intersection */
  lambda = (m - dist)/cos;
  /* Check that it is inside CD segment */
  if (lambda < 0. || lambda > 1.) return 0;

  /* Find intersection point */
  i.x= c->x + lambda * v_x;
  i.y= c->y + lambda * v_y;

  /* Check that it is inside AB segment */
  return (ROUTEMAP_TOTAL_WITHIN_SEGMENT(a,b,&i));

}



/* return 1 if segment ab is tangent to poly at point poly[index] */
int32 routemap_is_line_tangent_to_poly(a,b,poly,index)
     point *a,*b;
     polygonal *poly;
     int32 index;
{
  point normal_to_line;
  point edge_a, edge_b;

  normal_to_line.x = -(b->y - a->y);
  normal_to_line.y = b->x - a->x;
  
  edge_a.x = poly->vertex[modulo((index-1),poly->num_vertices)].x;
  edge_a.y = poly->vertex[modulo((index-1),poly->num_vertices)].y;
  edge_a.x -= poly->vertex[index].x;
  edge_a.y -= poly->vertex[index].y;

  edge_b.x = poly->vertex[modulo((index+1),poly->num_vertices)].x;
  edge_b.y = poly->vertex[modulo((index+1),poly->num_vertices)].y;
  edge_b.x -= poly->vertex[index].x;
  edge_b.y -= poly->vertex[index].y;

  return (routemap_equal_sign(routemap_dot_product(&normal_to_line,&edge_a),
		  routemap_dot_product(&normal_to_line,&edge_b),
		  0.01));
}


/*
  returns 1 if the line poly1[index1]poly2[index2] is tangent to both poly1 
  and poly2 
  It checks if the line is tangent by checking if the sign of 
  dot(normal_to_line,edge_a) is equal to dot(normal_to_line,edge_b)
  where edge_a and edge_b are the two edges eminating from the vertex
  we're checking tangency to.
*/
int32 routemap_tangent_line(index1,index2,poly1,poly2)
     int32 index1,index2;
     polygonal *poly1,*poly2;
{

  if (routemap_is_line_tangent_to_poly(&(poly1->vertex[index1]),&(poly2->vertex[index2]),poly1,index1) &&
      routemap_is_line_tangent_to_poly(&(poly1->vertex[index1]),&(poly2->vertex[index2]),poly2,index2))
    return(1);
  else return (0);
}


/*
  returns 1 if the the line ab hits any of the polygons of width w in the map m
*/
int32 routemap_line_hits_poly(m,a,b,list_of_polys_a,list_of_polys_b,w)
     map* m;
     point *a,*b;
     ROUTEMAP_POLY_LIST *list_of_polys_a;
     ROUTEMAP_POLY_LIST *list_of_polys_b;
     int32 w;
{
  int32 i;
  int32 j;
  point min_line,max_line;

  min_line.x=my_min(a->x,b->x);
  max_line.x=my_max(a->x,b->x);
  min_line.y=my_min(a->y,b->y);
  max_line.y=my_max(a->y,b->y);

  for (i=0;i<m->num_no_gos;i++)
    if (!((min_line.x > m->no_gos[w][i].b.top.x)
	  || (max_line.x < m->no_gos[w][i].b.bottom.x)
	  || (min_line.y > m->no_gos[w][i].b.top.y)
	  || (max_line.y < m->no_gos[w][i].b.bottom.y)))
      for (j=0;j<m->no_gos[w][i].num_vertices;j++)
	if (routemap_line_intersection(a,b,
				       &(m->no_gos[w][i].vertex[j]),
				       &(m->no_gos[w][i].vertex[(j+1)%(m->no_gos[w][i].num_vertices)])))
	  return (1);

  if (routemap_point_in_any_poly(m,a,list_of_polys_a,w) 
      || routemap_point_in_any_poly(m,b,list_of_polys_b,w))
    return (1);

  return (0);
}


int32 routemap_line_hits_local_poly(m,a,b,list_of_polys,w)
     map* m;
     point *a,*b;
     ROUTEMAP_POLY_LIST *list_of_polys;
     int32 w;
{
  int32 i;
  int32 j;
  ROUTEMAP_POLY_LIST *ptr;

  for (ptr=list_of_polys; ptr!=NULL; ptr=ptr->next)
    {
      if (ptr->poly_width==w)
	for (j=0;j<m->no_gos[ptr->poly_width][ptr->poly_num].num_vertices;j++)
	  if (routemap_line_intersection(a,b,
					 &(m->no_gos[ptr->poly_width][ptr->poly_num].vertex[j]),
					 &(m->no_gos[ptr->poly_width][ptr->poly_num].vertex[(j+1)%(m->no_gos[ptr->poly_width][ptr->poly_num].num_vertices)])))
	    return (1);
    }
  
  if (routemap_point_in_any_poly(m,a,list_of_polys,w) || 
      routemap_point_in_any_poly(m,b,list_of_polys,w))
    return (1);
  
  return (0);
}


/* points on edge are not cosidered to be in the poly */
int32 routemap_point_in_poly(poly,p)
     polygonal *poly;
     point *p;
{
  int32 inside, loop;
  float32 x1, y1, x2, y2, dx, dy, px, py, vx, vy;
  float32 epsilon=0.001;
  float32 epsilon_sqd=epsilon*epsilon;
  float32 temp;
  float32 dmag, vmag;

  if (ROUTEMAP_POINT_IN_BOUNDING_BOX(&(poly->b),p))
    {
      inside = 0;
      
      px = p->x; py = p->y;
      for (loop=0; loop < poly->num_vertices; loop++)
	{
	  x1=poly->vertex[loop].x;
	  x2=poly->vertex[(loop+1)%poly->num_vertices].x;
	  y1=poly->vertex[loop].y;
	  y2=poly->vertex[(loop+1)%poly->num_vertices].y;

	  dx = x2 - x1; dy = y2 - y1;
	  vx = px - x1; vy = py - y1;

	  dmag = dx*dx+dy*dy;
	  vmag = vx*vx+vy*vy;

	  if (vmag <= epsilon_sqd) return(0);
	  if (dmag <= epsilon_sqd) continue;
	  
	  if (((sqr(abs((dx*vy) - (dy*vx))))/(dmag*vmag)) < epsilon_sqd)
	    {
	      temp=dx*vx + dy*vy;
 	      if ((0<=temp) && (temp < (sqr(dx)+sqr(dy))))
		return (0);
	    }
	  /* check if the edge crosses the x axis */

	  if (((y1 >= py) && (y2 < py)) || ((y1 < py) && (y2 >= py)))	
	    {
	      /* check that the crossing happens to the right of x,y */
	      if ((dy >= 0.) ?
		  (vx*dy < vy*dx) : (vx*dy > vy*dx))
		/* toggle inside state, odd number will leave it on */
		inside = !inside;
	    }
	}
      return inside;
    }
  else return (0);
}

#if 0

int32 routemap_point_in_poly(poly,p)
     polygonal *poly;
     point *p;
{
  int32 inside, loop;
  float32 x1, y1, x2, y2, dx, dy, px, py, vx, vy;
  float32 epsilon=0.0001;
  float32 cross;
  float32 vmag,dmag;
  float32 epsilon_sqd=epsilon*epsilon;
  
  if (ROUTEMAP_POINT_IN_BOUNDING_BOX(&(poly->b),p))
    {
      inside = 0;
      
      px = p->x; py = p->y;
      for (loop=0; loop < poly->num_vertices; loop++)
	{
	  x1=poly->vertex[loop].x;
	  x2=poly->vertex[(loop+1)%poly->num_vertices].x;
	  y1=poly->vertex[loop].y;
	  y2=poly->vertex[(loop+1)%poly->num_vertices].y;
/*	  printf("checking with line %f,%f - %f,%f\n",x1,y1,x2,y2); */
	  dx = x2 - x1; dy = y2 - y1;
	  vx = px - x1; vy = py - y1;

	  /* check if the edge crosses the x axis */
	  
	  if ((SAME(px,x1)) && (SAME(py,y1)))
	    return (0);

/** new version - oded **/
	      dmag = dx*dx+dy*dy;
	      vmag = vx*vx+vy*vy;
	      
	      if (vmag <= epsilon_sqd) return(0);
	      if (dmag <= epsilon_sqd) continue;
	      
	  cross = sqr(abs((dx*vy) - (dy*vx)))/(dmag*vmag);
	      if ((cross < epsilon_sqd) &&
		  (ROUTEMAP_MY_TOTAL_BETWEEN(x1,x2,px)) &&
		  (ROUTEMAP_MY_TOTAL_BETWEEN(y1,y2,py)))
		return(0);



	  if (((y1 >= py) && (y2 < py)) || ((y1 < py) && (y2 >= py)))	
	    {
	      cross = vx*dy - vy*dx;
#if 0
	      cross = vx*dy - vy*dx;
	      /* If cross is almost 0 (and we are in bounding box), we */
	      /* are on the edge. */ 
	      if ((abs(cross) < epsilon) &&
		  (ROUTEMAP_MY_TOTAL_BETWEEN(x1,x2,px)) &&
		  (ROUTEMAP_MY_TOTAL_BETWEEN(y1,y2,py)))
		return(0);
#endif
/*	      printf("dang\n");
	      printf("dy is %f, and cross is %f\n",dy,cross);
*/
	      /* check that the crossing happens to the right of x,y */
	      if ((dy >= 0.) ? (cross < 0.) : (cross >= 0.)) /* used to be cross > 0.0 */
		/* toggle inside state, odd number will leave it on */
		{
/*		  printf("ding dong\n"); */
		  inside = !inside;
		}
	    }
	  else 
	    /* Check for inside horizontal edge */
	    if ((SAME(y1,py)) && (SAME(y2,py)) &&
		(ROUTEMAP_MY_TOTAL_BETWEEN(x1,x2,px))) 
	      return (0);
	}
      return inside;
    }
  else return (0);
}

#endif

/*
   returns 1 if p is in any polygon of width w in the world
*/
int32 routemap_point_in_any_poly(m,p,list_of_polys,w)
     map *m;
     point *p;
     ROUTEMAP_POLY_LIST *list_of_polys;
     int32 w;
{
  int32 i;
  ROUTEMAP_POLY_LIST *ptr;

  for (ptr=list_of_polys; ptr!=NULL; ptr=ptr->next)
    {
      if ((p->which_poly!=ptr->poly_num) && (w==ptr->poly_width))
	if (routemap_point_in_poly(&(m->no_gos[ptr->poly_width][ptr->poly_num]),p))
	  return (1);
    }
  
  return (0);
}


void routemap_calculate_bounding_box(poly)
     polygonal *poly;
{
  int32 i;

  poly->b.top.x=-INFINITY;
  poly->b.bottom.x=INFINITY;
  poly->b.top.y=-INFINITY;
  poly->b.bottom.y=INFINITY;
  for (i=0;i<poly->num_vertices;i++)
    {
      poly->b.top.x=my_max(poly->vertex[i].x,poly->b.top.x);
      poly->b.bottom.x=my_min(poly->vertex[i].x,poly->b.bottom.x);
      poly->b.top.y=my_max(poly->vertex[i].y,poly->b.top.y);
      poly->b.bottom.y=my_min(poly->vertex[i].y,poly->b.bottom.y);
      
    }
}

/* checks if point is in rectangle */
int32 routemap_point_in_rect(p,top,bottom)
     point *p;
     point *top;
     point *bottom;
{
  return ((bottom->x < p->x) &&
	  (p->x < top->x) &&
	  (bottom->y < p->y) &&
	  (p->y < top->y));
}

/* checks if two rectangles overlap */
int32 routemap_rectangle_overlap(top1,bot1,top2,bot2)
     point *top1;
     point *bot1;
     point *top2;
     point *bot2;
{
  point p;

  if (!((bot1->x > top2->x)
	|| (top1->x < bot2->x)
	|| (bot1->y > top2->y)
	|| (top1->y < bot2->y)))
    return (1);
  else
    return (0);
}

/* 
   adds the point p to the linked list of intersections, making sure that the list stays 
   in order of increasing length from the point origin
*/
void
routemap_insert_point_to_ordered_list(p,origin,inter_list)
     point *p;
     point *origin;
     ROUTEMAP_VERT_LIST *inter_list;
{
  ROUTEMAP_VERT_LIST *new_element;
  float dist;
  ROUTEMAP_VERT_LIST *ptr;
  ROUTEMAP_VERT_LIST *back_ptr;

  dist = routemap_distance_squared(p,origin);
  new_element = (ROUTEMAP_VERT_LIST *) (malloc (sizeof(ROUTEMAP_VERT_LIST)));
  new_element->p = (point *) (malloc (sizeof(point)));
  new_element->p->x = p->x;
  new_element->p->y = p->y;
  if (inter_list==NULL)
    {
      new_element->next=NULL;
      inter_list=new_element;
      return;
    }
  if (inter_list->next==NULL)
    {
      if (dist < routemap_distance_squared(origin,inter_list->p))
	{
	  new_element->next=inter_list;
	  inter_list=new_element;
	}
      else
	{
	  new_element->next=NULL;
	  inter_list->next=new_element;
	}
      return;
    }
  back_ptr=inter_list;
  ptr=inter_list->next;
  while (ptr!=NULL)
    {
      if (dist < routemap_distance_squared(origin,ptr->p))
	{
	  new_element->next = ptr;
	  back_ptr->next = new_element;
	  return;
	}
      else
	{
	  back_ptr=back_ptr->next;
	  ptr=ptr->next;
	}
    }
  /* if we got here then the new element is the largest one in the list */
  new_element->next = NULL;
  back_ptr->next=new_element;
}


/* takes an intersection list with point_a as its first element.  Returns
   a list of intersections iwth the grid, with the last point being point_b 
*/
void
routemap_get_intersections_with_grid(routemap,point_a,point_b,inter_list)
     ROUTEMAP_PTR routemap;
     point *point_a;
     point *point_b;
     ROUTEMAP_VERT_LIST *inter_list;
{
  float32 slope,y_inter,intersect;
  int32 i;
  point p;

  if (point_a->x != point_b->x)
    {
      slope=((point_b->y-point_a->y)/(point_b->x-point_a->x));
      y_inter=((point_a->y*point_b->x) - (point_a->x*point_b->y)) / (point_b->x - point_a->x);
    }

  /* check for intersections with vertical grid lines */
  for (i=0;i<routemap->num_x_squares;i++)
    {
      /* if the line is vertical, then dont worry about it.  Either it doesnt
	 hit a vertical grid, or its right on it.  If its right on it, then
	 we will add the endpoints later on anyway 
	 */
      if (point_a->x != point_b->x)
	{
	  if (point_a->y != point_b->y)
	    {
	      /* where the infinite line ab intersects the vertical grid line */
	      intersect=(slope*(routemap->x_min + ((float32)i*routemap->size_square_x))) + y_inter;
	      /* if it intersects within the segment ab, then add it */
	      if (ROUTEMAP_MY_TOTAL_BETWEEN(point_a->y,point_b->y,intersect))
		{
		  p.x=routemap->x_min + ((float32)i*routemap->size_square_x);
		  p.y=intersect;
		  routemap_insert_point_to_ordered_list(&p,point_a,inter_list);
		}
	    }
	  else
	    {
	      /* intersection of a vertical grid with a horizontal line */
	      intersect=routemap->x_min + ((float32)i*routemap->size_square_x);
	      /* if it intersects within the segment ab, then add it */
	      if (ROUTEMAP_MY_TOTAL_BETWEEN(point_a->x,point_b->x,intersect))
		{
		  p.x = intersect;
		  p.y = point_a->y;
		  routemap_insert_point_to_ordered_list(&p,point_a,inter_list);
		}
	    }
	}
    }


  /* check for intersections with horizontal grid lines */
  for (i=0;i<routemap->num_y_squares;i++)
    {
      if (point_a->x != point_b->x)
	{
	  /* where the infinite line ab intersects the horizontal grid line */
	  intersect=((routemap->y_min + ((float32)i*routemap->size_square_y)) - y_inter) / slope;
	  /* if it intersects within the segment ab, then add it */
	  if (ROUTEMAP_MY_TOTAL_BETWEEN(point_a->x,point_b->x,intersect))
	    {
	      p.x = intersect;
	      p.y = routemap->y_min + ((float32)i*routemap->size_square_y);
	      routemap_insert_point_to_ordered_list(&p,point_a,inter_list);
	    }
	}
      else
	{
	  /* intersection of a horizontal grid line with a vertical line */
	  intersect=routemap->y_min + ((float32)i*routemap->size_square_y);
	  /* if it intersects within the segment ab, then add it */
	  if (ROUTEMAP_MY_TOTAL_BETWEEN(point_a->y,point_b->y,intersect))
	    {
	      p.x = point_a->x;
	      p.y = intersect;
	      routemap_insert_point_to_ordered_list(&p,point_a,inter_list);
	    }
	}
    }


  /* finally, add the  end point to the intersection list.  This will be 
   the last point in the list, hopefully
   */
  routemap_insert_point_to_ordered_list(point_b,point_a,inter_list);
}