#ifndef lint
static char rcsid [] = "$RCSfile$ $Revision$ $State$";
#endif
/****************************************************************************
*   File: rtmp_plan.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: Code to preplan a route using the routemap                *
*       Created: Tue Nov  9 1993                                            *
*       Author: jesmith                                                     *
*       Remarks:                                                            *
*                                                                           *
****************************************************************************/

#include "librtmp_local.h"
#include "/usr/modsaf/common/libsrc/libtime/libtime.h"
#include <stdext.h>
#include <math.h>

/* Maximum number of milliseconds allowed for planning */
#define MAX_TIME   1000

/* Maximum rates of expansion (change in width per distance covered) */
#define MAX_GROW   0.33
#define MAX_SHRINK 0.2

/* Minimum distance over which width changes are allowed */
#define MIN_EXP_DIST 100.0

/* Extra room we give around obstacles, beyond the necessary additions
 * for error (used so that formation expansion is less likely to lead to
 * overlaps with obstacles).
 */
#define MIN_ERROR 10.0

static ROUTEMAP_PLAN_STEP *generate_plan();
static void clean_plan();
static void fit_to_goal();
static void trim_input();
static void smooth_plan();
static int32 corridor_crossings();
static void add_closer_points();

/* Add an XY step to list */
static ROUTEMAP_PLAN_STEP *add_xy(x, y, after_step)
    float64    x, y;
    ROUTEMAP_PLAN_STEP *after_step;
{
    ROUTEMAP_PLAN_STEP *step;

    step = routemap_new_step();
    step->x = x;
    step->y = y;
    step->next = after_step->next;
    after_step->next = step;    
    
    return step;
}

/* Add a corridor to the list */
static ROUTEMAP_PLAN_STEP *add_corridor(corr, after_step)
    ROUTEMAP_CORRIDOR  *corr;
    ROUTEMAP_PLAN_STEP *after_step;
{
    ROUTEMAP_PLAN_STEP *step;

    step = routemap_new_step();
    step->corridor = corr;
    step->next = after_step->next;
    after_step->next = step;    

    return step;
}

/* Make a copy of the goal, and call it the plan */
static void copy_goal(goal, unit_width, plan)
    ROUTE_POINTS   *goal;
    int32           unit_width;
    ROUTE_POINTS   *plan;
{
    int32 i;

    NS_ROUTE_ALLOCATE_POINTS(plan, goal->num_pts);
    bcopy(goal->points, plan->points,
	  plan->num_pts * sizeof(ROUTE_POINT));

    for (i=0;i<plan->num_pts;i++)
      plan->points[i].next_segment_width = unit_width;
}

int32 routemap_preplan(routemap, obstacle_mask, goal, unit_width, plan)
    ROUTEMAP_PTR    routemap;
    uint32          obstacle_mask;
    ROUTE_POINTS   *goal;
    int32           unit_width;
    ROUTE_POINTS   *plan;
{
  
    my_preplan(routemap,routemap_my_no_go_map,routemap_my_visib_graph,*goal,plan);

#if 0
    int32 i, len;
    ROUTEMAP_PLAN_STEP *start, *step, *prev;
    int32 failed, step_failed;

    if (!routemap || (goal->num_pts < 2))
    {
	copy_goal(goal, unit_width, plan);
	return TRUE;
    }

    /* Initialize the plan with the first point */
    step = routemap_new_step();
    step->x = goal->points[0].point[X];
    step->y = goal->points[0].point[Y];

    /* Generate subplans to each subsequent point */
    failed = FALSE;
    start = step;
    for (i=1;i<goal->num_pts;i++)
    {
	prev = step;
	step_failed = FALSE;
	step = generate_plan(routemap, obstacle_mask, step,
			     goal->points[i].point[X],
			     goal->points[i].point[Y],
			     &step_failed);
	if (step_failed)
	  prev->failed = step->failed = TRUE;
	failed |= step_failed;
    }

    /* Squeeze the plan down to the necessary widths */
    for (step=start,prev=NULL;step;prev=step,step=step->next)
      fit_to_goal(step, prev, unit_width, routemap->max_error + MIN_ERROR,
		  routemap->left_bound, routemap->right_bound);

    /* Trim the width at input points to ensure they unit does not
     * fall into obstacles
     */
    for (step=start;step;step=step->next)
      trim_input(routemap, step, obstacle_mask,
		 routemap->max_error + MIN_ERROR);

    /* Eliminate points which ended up not being necessary */
    clean_plan(routemap, obstacle_mask, start);

    /* Eliminate sudden changes over short distances */
    smooth_plan(start, routemap->max_grow, routemap->max_shrink,
		routemap->min_expansion_dist, routemap->max_error + MIN_ERROR,
		routemap->left_bound, routemap->right_bound);

    /* Add points before/after tree canopy entrance/exit to avoid earlier
     * shrink/grow problems
     */
    add_closer_points(routemap, obstacle_mask, start);

    /* Figure out how long this is */
    for (i=0,step=start;step;i++,step=step->next)
      ;
    len = i;

    /* Allocate space */
    NS_ROUTE_ALLOCATE_POINTS(plan, len);

    /* Copy in the points */
    for (i=0,step=start;step;i++,step=step->next)
    {
	plan->points[i].point[X] = step->x;
	plan->points[i].point[Y] = step->y;
	plan->points[i].point[Z] = 0.0;
	plan->points[i].point_id = i;
	plan->points[i].next_segment_width = step->width;
	plan->points[i].user_data = step->failed;
    }

    routemap_free_steps(start);

    routemap_check_leaks();

    return !failed;
#endif
}

/* Get the index number of a corridor with respect to one of the
 * obstacles it is attached to.
 */
float64 routemap_corr_index(corr, obst)
    ROUTEMAP_CORRIDOR *corr;
    ROUTEMAP_OBSTACLE *obst;
{
    if (corr->begin_obst == obst)
      return corr->begin_index;
    else
      return corr->end_index;
}

/* Check whether two corridors appear to be at either end of a single
 * linear obstacle.
 */
static int32 ends_of_linear_obstacle(c0, c1)
    ROUTEMAP_CORRIDOR *c0, *c1;
{
    ROUTEMAP_OBSTACLE *obst;
    float64 index0, index1;

    if ((c0->begin_obst == c1->begin_obst) ||
	(c0->begin_obst == c1->end_obst))
      obst = c0->begin_obst;
    else if ((c0->end_obst == c1->begin_obst) ||
	     (c0->end_obst == c1->end_obst))
      obst = c0->end_obst;
    else
      return FALSE;

    index0 = routemap_corr_index(c0, obst);
    index1 = routemap_corr_index(c1, obst);
    return ((index0 == 0.0) && (index1 == obst->n_verts-1) ||
	    (index1 == 0.0) && (index0 == obst->n_verts-1));
}

/* When avoiding an obstacle, there are generally two corridors which
 * can be used (one on the left, one on the right).  The planner cannot
 * choose the optimal corridor before the corridor-to-corridor search
 * has been completed.  Therefore, the corridor not chosen is recalled
 * for a final filtering step.
 */
struct corridor_pair
{
    ROUTEMAP_CORRIDOR *primary;
    ROUTEMAP_CORRIDOR *alternate;
};

/* Generate a subplan from the start point to the specified location */
static ROUTEMAP_PLAN_STEP *generate_plan(routemap, obstacle_mask,
					 start, x, y, failed)
    ROUTEMAP_PTR        routemap;
    uint32              obstacle_mask;
    ROUTEMAP_PLAN_STEP *start;
    float64             x, y;
    int32              *failed;
{
    ROUTEMAP_PLAN_STEP *step;
    int32 i, n_corridors;
    float64 ign;
    uint32 stop_time = time_realtime_clock() + MAX_TIME;
    struct corridor_pair corridors[1024];

    /* Allocate a step at the place where we want to go */
    step = add_xy(x, y, start);

    /* Find the first and last corridor intersections with the
     * line connecting our previous location to our next location.
     */
    n_corridors = corridor_crossings(routemap, obstacle_mask,
				     start->x, start->y, x, y,
				     TRUE, NULL, NULL, corridors, &ign, &ign,
				     failed);

    /* Simpleset case: Nothing in the way */
    if (!n_corridors)
      return step;

    /* Link up each returned pair of corridors */
    for (i=0;i<n_corridors;i+=2)
    {
	ROUTEMAP_CORRIDOR *p0, *a0, *p1, *a1;

	p0 = corridors[i].primary;
	p1 = corridors[i+1].primary;
	a0 = corridors[i].alternate;
	a1 = corridors[i+1].alternate;

	/* First check for trivial pairs:
	 *
	 * 1) Both primary and alternate match.  This usually means that
	 *    there is really no need to do any avoidance between the two.
	 *    An exception to this is when the primary and alternate are
	 *    opposite ends of a linear obstacle.
	 *
	 * 2) One common corridor between the two steps.  If we go through
	 *    that corridor, we'll be all set.
	 */
	if (a0 && a1 &&
	    (((p0 == p1) && (a0 == a1)) ||
	     ((p0 == a1) && (a0 == p1))) &&
	    !ends_of_linear_obstacle(p0, a0))
	{
	    /* Case 1 */
	    continue;
	}
	else if ((p0 == p1) || (a1 && (p0 == a1)))
	{
	    /* Case 2 */
	    add_corridor(p0, start);
	    start = start->next;
	}
	else if (a0 && ((a0 == p1) || (a0 == a1)))
	{
	    /* Also Case 2 */
	    add_corridor(a0, start);
	    start = start->next;
	}
	else if (p0->clump == p1->clump)
	{
	    /* Find a connected path from one corridor to the other */
	    start = routemap_link_corridors(routemap, start,
					    p0, p1, a0, a1, failed,
					    stop_time);
	}
	else
	{
	    /* The corridor choosing algorithm failed to come up with
	     * an acceptable connection between these two corridors.
	     * Just dump them into the plan, and hope for the best...
	     */
	    add_corridor(p0, start);
	    start = start->next;
	    add_corridor(p1, start);
	    start = start->next;
	}
    }

    return step;
}

/* Determine where parametric lines cross (return the second parameter) */
static void parametric_intersection1(s0, s1, t1_ret)
    ROUTEMAP_PSEG *s0;
    ROUTEMAP_PSEG *s1;
    float64       *t1_ret;
{
    float64 numer, denom;

    /* We do this using parametric equations. */

    numer = s0->dy * (s1->x - s0->x) + s0->dx * (s0->y - s1->y);
    denom = s0->dx * s1->dy - s1->dx * s0->dy;

    if ((numer == 0.0) && (denom == 0.0))
    {
	/* The segments are colinear.  Determine if they overlap */
	if ((s0->x <= s1->x) && (s1->x <= s0->x+s0->dx) ||
	    (s1->x <= s0->x) && (s0->x <= s1->x+s1->dx) ||
	    (s0->y <= s1->y) && (s1->y <= s0->y+s0->dy) ||
	    (s1->y <= s0->y) && (s0->y <= s1->y+s1->dy))
	  *t1_ret = 0.5;
	else
	  *t1_ret = -1.0;
	return;
    }

    *t1_ret = numer / denom;
}

/* Determine where parametric lines cross (return both parameters) */
static void parametric_intersection2(s0, s1, t0_ret, t1_ret)
    ROUTEMAP_PSEG *s0;
    ROUTEMAP_PSEG *s1;
    float64       *t0_ret;
    float64       *t1_ret;
{
    float64 numer, denom;
    float64 t0, t1;

    /* We do this using parametric equations. */

    numer = s0->dy * (s1->x - s0->x) + s0->dx * (s0->y - s1->y);
    denom = s0->dx * s1->dy - s1->dx * s0->dy;

    if ((numer == 0.0) && (denom == 0.0))
    {
	/* The segments are colinear.  Determine if they overlap */
	if ((s0->x <= s1->x) && (s1->x <= s0->x+s0->dx) ||
	    (s1->x <= s0->x) && (s0->x <= s1->x+s1->dx) ||
	    (s0->y <= s1->y) && (s1->y <= s0->y+s0->dy) ||
	    (s1->y <= s0->y) && (s0->y <= s1->y+s1->dy))
	  *t0_ret = *t1_ret = 0.5;
	else
	  *t0_ret = *t1_ret = -1.0;
	return;
    }

    t1 = numer / denom;

    /* Derive t0 from t1 using either the x or the y equation.
     */
    if (s0->dx)
      t0 = (s1->x - s0->x + s1->dx * t1) / s0->dx;
    else
      t0 = (s1->y - s0->y + s1->dy * t1) / s0->dy;

    *t1_ret = t1;
    *t0_ret = t0;
}

/* Determine if and where parametric segments cross (return both parameters)
 */
static int32 parametric_intersection_pred(s0, s1, t0_ret, t1_ret)
    ROUTEMAP_PSEG *s0;
    ROUTEMAP_PSEG *s1;
    float64       *t0_ret;
    float64       *t1_ret;
{
    float64 numer, denom;
    float64 t0, t1;

    /* We do this using parametric equations. */

    numer = s0->dy * (s1->x - s0->x) + s0->dx * (s0->y - s1->y);
    denom = s0->dx * s1->dy - s1->dx * s0->dy;

    if ((numer == 0.0) && (denom == 0.0))
    {
	/* The segments are colinear.  Determine if they overlap */
	if ((s0->x <= s1->x) && (s1->x <= s0->x+s0->dx) ||
	    (s1->x <= s0->x) && (s0->x <= s1->x+s1->dx) ||
	    (s0->y <= s1->y) && (s1->y <= s0->y+s0->dy) ||
	    (s1->y <= s0->y) && (s0->y <= s1->y+s1->dy))
	{
	    *t0_ret = *t1_ret = 0.5;
	    return TRUE;
	}
	return FALSE;
    }

    t1 = numer / denom;

    /* NOTE: The boundary tests below are open on one end: [ )
     *       This allows us to use the even-odd rule without needing
     *       to worry about hitting vertices.
     */

    /* Are we within the bounds of the second segment? */
    if (t1 >= 0.0 && t1 < 1.0)
    {
	/* Derive t0 from t1 using either the x or the y equation.
	 */
	if (s0->dx)
	  t0 = (s1->x - s0->x + s1->dx * t1) / s0->dx;
	else
	  t0 = (s1->y - s0->y + s1->dy * t1) / s0->dy;

	/* Are we within the bounds of the first segment? */
	if (t0 >= 0.0 && t0 < 1.0)
	{
	    *t0_ret = t0;
	    *t1_ret = t1;
	    return TRUE;
	}
    }

    return FALSE;
}

/* Determine if and where a parametric segment crosses its perpendicular
 * through a point.
 */
static int32 parametric_point_intersection_pred(s0, x, y, r, t0_ret, t1_ret)
    ROUTEMAP_PSEG *s0;
    float64        x, y, r;
    float64       *t0_ret;
    float64       *t1_ret;
{
    float64 numer, denom;
    float64 dx, dy, dist;
    float64 t0;

    /* We do this using parametric equations. */

    dx = x - s0->x;
    dy = y - s0->y;
    dist = s0->dx * s0->dx + s0->dy * s0->dy;
    numer = s0->dy * dx - s0->dx * dy;
    numer *= numer;
    denom = r * r * dist;

    if (numer > denom)
      return FALSE;

    t0 = (dx * s0->dx + dy * s0->dy) / dist;

    if (t0 < 0.0 || t0 > 1.0)
      return FALSE;

    *t0_ret = t0;
    *t1_ret = fsqrt(numer / denom);
    return TRUE;
}

/* Determine where a parametric segment crosses its perpendicular
 * through a point.
 */
static int32 parametric_point_intersection(s0, x, y, t0_ret)
    ROUTEMAP_PSEG *s0;
    float64        x, y;
    float64       *t0_ret;
{
    float64 numer;
    float64 dx, dy, dist;

    /* We do this using parametric equations. */

    dx = x - s0->x;
    dy = y - s0->y;
    dist = s0->dx * s0->dx + s0->dy * s0->dy;
    numer = s0->dy * dx - s0->dx * dy;
    numer *= numer;

    *t0_ret = (dx * s0->dx + dy * s0->dy) / dist;
}

/* Structure used for bounds checking */
struct bounding_box
{
    int32 min_x, max_x, min_y, max_y;
};

/* Get the bounding box of a segment */
static void get_bounding(seg, box)
    ROUTEMAP_PSEG       *seg;
    struct bounding_box *box;
{
    if (seg->dx > 0.0)
      box->min_x = seg->x, box->max_x = seg->x + seg->dx;
    else
      box->max_x = seg->x, box->min_x = seg->x + seg->dx;
    if (seg->dy > 0.0)
      box->min_y = seg->y, box->max_y = seg->y + seg->dy;
    else
      box->max_y = seg->y, box->min_y = seg->y + seg->dy;
}

/* Check two bounding boxes for possible overlap */
static int32 bounding_bounding_overlap(a, b)
    struct bounding_box *a, *b;
{
    /* It's easiest to express when they boxes cannot overlap... */
    return (!((a->max_x < b->min_x) ||
	      (a->max_y < b->min_y) ||
	      (a->min_x > b->max_x) ||
	      (a->min_y > b->max_y)));
}

/* Check a bounding box versus an obstacle for possible overlap */
static int32 bounding_obst_overlap(b, o)
    struct bounding_box *b;
    ROUTEMAP_OBSTACLE   *o;
{
    /* It's easiest to express when they boxes cannot overlap... */
    return (!(((o->center_x+o->width) < b->min_x) ||
	      ((o->center_y+o->height) < b->min_y) ||
	      ((o->center_x-o->width) > b->max_x) ||
	      ((o->center_y-o->height) > b->max_y)));
}

/* Determine if a segment intersects an obstacle in a meaningful way.
 * (Using the even-odd rule for linear obstacles.)
 */
static int32 obstacle_intersection(mask, obst, seg, box,
				   t_small_ret, t_large_ret,
				   obst_small_index, obst_large_index)
    uint32               mask;
    ROUTEMAP_OBSTACLE   *obst;
    ROUTEMAP_PSEG       *seg;
    struct bounding_box *box;
    float64             *t_small_ret;
    float64             *t_large_ret;
    float64             *obst_small_index;
    float64             *obst_large_index;
{
    int32 i, count;
    float64 t, o, small_t, large_t, small_index, large_index;

    if (!(mask & obst->type_mask))
      return 0;

    if (!bounding_obst_overlap(box, obst))
      return 0;

    count = 0;
    small_t = 2.0;
    large_t = -1.0;
    small_index = large_index = 0.0;
    for (i=0;i<obst->n_verts-1;i++)
      if (parametric_intersection_pred(&obst->verts[i].seg, seg, &o, &t))
      {
	  count++;
	  if (t < small_t)
	    small_t = t, small_index = i+o;
	  if (t > large_t)
	    large_t = t, large_index = i+o;
      }
    *t_small_ret = small_t;
    *obst_small_index = small_index;
    *t_large_ret = large_t;
    *obst_large_index = large_index;

    if (obst->linear)
      return count;
    else
      return count != 0;
}

/* Determine if a point is inside an obstacle */
static int32 point_in_obstacle(x, y, obst)
    float64            x, y;
    ROUTEMAP_OBSTACLE *obst;
{
    int32 i, count;
    float64 ign0, ign1;
    ROUTEMAP_PSEG seg;

    if (obst->linear)
      return FALSE;

    /* Make a ray from 0,0 to the point */
    seg.x = 0.0;
    seg.y = 0.0;
    seg.dx = x;
    seg.dy = y;

    /* Count intersections */
    count = 0;
    for (i=0;i<obst->n_verts-1;i++)
      if (parametric_intersection_pred(&obst->verts[i].seg, &seg,
				       &ign0, &ign1))
	count++;

    return count & 0x1;
}

/* Determine if a point gets too close to an obstacle.
 */
static int32 obstacle_point_intersection(mask, obst, x, y, r, t_small_ret)
    uint32             mask;
    ROUTEMAP_OBSTACLE *obst;
    float64            x, y, r;
    float64           *t_small_ret;
{
    int32 i, count;
    float64 ign, t, small_t;

    if (!(mask & obst->type_mask))
      return 0;

    count = 0;
    small_t = 2.0;
    for (i=0;i<obst->n_verts-1;i++)
      if (parametric_point_intersection_pred(&obst->verts[i].seg, x, y, r,
					     &ign, &t))
      {
	  count++;
	  if (t < small_t)
	    small_t = t;
      }
    *t_small_ret = small_t;

    return count;
}

/* Determine if a segment intersects a corridor */
static int32 corridor_intersection(mask, corr, seg, t_ret)
    uint32               mask;
    ROUTEMAP_CORRIDOR   *corr;
    ROUTEMAP_PSEG       *seg;
    float64             *t_ret;
{
    float64 ign;

    if (!((corr->begin_obst->type_mask | corr->end_obst->type_mask) & mask))
      return 0;

    return parametric_intersection_pred(&corr->seg, seg, &ign, t_ret);
}

/* Ensure the passed corridor does not exceed unit boundaries */
static void trim_corridor(corridor, lbound, rbound, seg, len)
    ROUTEMAP_CORRIDOR    *corridor;
    ROUTEMAP_BOUNDARY_PTR lbound, rbound;
    ROUTEMAP_PSEG        *seg;
    float64              *len;
{
    int32 i, hit_left, hit_right;
    float64 ign, lt, rt, tlow, thigh;

    /* Find the intersections, if any, with the boundaries */
    hit_left = FALSE;
    hit_right = FALSE;
    if (lbound)
      for (i=0;i<lbound->n_verts;i++)
	if (parametric_intersection_pred(&corridor->seg, &lbound->verts[i].seg,
					 &lt, &ign))
	{
	    hit_left = TRUE;
	    break;
	}
    if (rbound)
      for (i=0;i<rbound->n_verts;i++)
	if (parametric_intersection_pred(&corridor->seg, &rbound->verts[i].seg,
					 &rt, &ign))
	{
	    hit_right = TRUE;
	    break;
	}

    /* If there are no intersections, we're done */
    if (!hit_left && !hit_right)
    {
	*seg = corridor->seg;
	*len = corridor->length;
	return;
    }

    /* Start by assuming we will go from t=0 to t=1, then move those
     * limits to be within the boundaries.
     */
    tlow = 0.0;
    thigh = 1.0;

    /* Deal with the left boundary */
    if (hit_left)
    {
	/* Is the start point of the corridor outside the boundaries? */
	if (routemap_wrong_side(lbound, corridor->seg.x, corridor->seg.y, 1))
	  tlow = lt;
	else
	  thigh = lt;
    }

    /* And now the right boundary */
    if (hit_right)
    {
	/* Is the start point of the corridor outside the boundaries? */
	if (routemap_wrong_side(rbound, corridor->seg.x, corridor->seg.y, -1))
	  tlow = rt;
	else
	  thigh = rt;
    }

    /* If the boundaries cross in a non-sensible manner, flip the low
     * and high values.  It isn't really correct, but it isn't clear what
     * one should do in this case.
     */
    if (tlow > thigh)
    {
	lt = tlow;
	tlow = thigh;
	thigh = lt;
    }

    /* Make the abbreviated corridor */
    seg->x = corridor->seg.x + tlow * corridor->seg.dx;
    seg->y = corridor->seg.y + tlow * corridor->seg.dy;
    seg->dx = corridor->seg.dx * (thigh - tlow);
    seg->dy = corridor->seg.dy * (thigh - tlow);
    *len = corridor->length * (thigh - tlow);
}

/* Generate a point which is properly aligned to fit through the corridor */
static void align_to_corridor(seg, corridor, unit_width, step, max_error,
			      lbound, rbound)
    ROUTEMAP_PSEG        *seg;
    ROUTEMAP_CORRIDOR    *corridor;
    float64               unit_width;
    ROUTEMAP_PLAN_STEP   *step;
    float64               max_error;
    ROUTEMAP_BOUNDARY_PTR lbound, rbound;
{
    ROUTEMAP_PSEG cseg;
    float64 clen, t_corr;
    float64 max_parametric_radius;

    unit_width += max_error;

    /* If there are boundaries, we may need to trim the corridor, to
     * ensure the unit doesn't cross them.
     */
    if (lbound || rbound)
      trim_corridor(corridor, lbound, rbound, &cseg, &clen);
    else
    {
	cseg = corridor->seg;
	clen = corridor->length;
    }

    /* If we can't fit, do our best */
    if (clen <= unit_width)
    {
	step->x = cseg.x + cseg.dx * 0.5;
	step->y = cseg.y + cseg.dy * 0.5;
	step->width = clen - max_error;
	if (step->width < 0.0)
	  step->width = 0.0;
	return;
    }

    /* We can be as wide as we want, but we need to move over */
    step->width = unit_width;

    /* Find the intersection between the test segment LINE and the
     * corridor LINE.
     */
    parametric_intersection1(seg, &cseg, &t_corr);

    /* How wide does the unit want to be, in terms of the corridor */
    max_parametric_radius = 0.5 * unit_width / clen;

    /* Is the current intersection unacceptable? */
    if ((t_corr - max_parametric_radius <= 0.0) ||
	(t_corr + max_parametric_radius >= 1.0))
    {
	/* Go around the closer end */
	if (t_corr < 0.5)
	  t_corr = max_parametric_radius;
	else
	  t_corr = 1.0 - max_parametric_radius;
    }

    step->x = cseg.x + cseg.dx * t_corr;
    step->y = cseg.y + cseg.dy * t_corr;
    step->width -= max_error;
}

/* Determine if there is a clear path between a step and the goal.
 */
static int32 clear_path(routemap, step, goal, obstacle_mask, check_corridors)
    ROUTEMAP_PTR        routemap;
    ROUTEMAP_PLAN_STEP *step;
    ROUTEMAP_PLAN_STEP *goal;
    uint32              obstacle_mask;
    int32               check_corridors;
{
    ROUTEMAP_CORRIDOR *ca, *cb, *corr;
    ROUTEMAP_OBSTACLE *obst;
    ROUTEMAP_PSEG seg;
    struct bounding_box box;
    float64 ign;

    /* Construct a segment from the current point directly to the goal.
     */
    seg.x = step->x;
    seg.y = step->y;
    seg.dx = goal->x - seg.x;
    seg.dy = goal->y - seg.y;
    get_bounding(&seg, &box);

    ca = step->corridor;
    cb = goal->corridor;

    /* Search the database for corridors or obstacles which cross the
     * segment.
     */

    if (check_corridors)
      for (corr=routemap->corridors;corr;corr=corr->next)
	if ((corr != ca) && (corr != cb) &&
	    corridor_intersection(obstacle_mask, corr, &seg, &ign))
	  return FALSE;

    for (obst=routemap->obstacles;obst;obst=obst->next)
      if (obstacle_intersection(obstacle_mask, obst, &seg, &box,
				&ign, &ign, &ign, &ign))
	return FALSE;

    return TRUE;
}

/* Clean up a plan by removing superflous corridors */
static void clean_plan(routemap, obstacle_mask, start)
    ROUTEMAP_PTR        routemap;
    uint32              obstacle_mask;
    ROUTEMAP_PLAN_STEP *start;
{
    ROUTEMAP_PLAN_STEP *step, *goal, *collect, *next, *prev;
    ROUTEMAP_OBSTACLE *obst;
    int32 repeat;

#if 1
    /* We choose the entrance and exit points of clumps without knowing
     * exactly where we will be coming from.  As a result, we often
     * don't choose the most optimal corridor.  However, we generally
     * will pass through the optimal corridor on the way to the one
     * we chose.  Therefore, we will identify clump-to-clump transitions,
     * and look for more direct routes.
     */
    for (step=start;step->next;step=step->next)
    {
	/* We are looking for a case where the point after the current
	 * point is a corridor, and there is another corridor from a
	 * different clump prior to the next goal point.
	 */
	if (!step->corridor || !step->next->corridor)
	  continue;

	for (goal=step->next->next;goal && goal->corridor; goal=goal->next)
	{
	    if ((goal->corridor->clump != step->corridor->clump) &&
		clear_path(routemap, step, goal, obstacle_mask, TRUE))
	    {
		/* We can get directly to this step without the intervening
		 * steps, so splice them out.
		 */
		collect = step->next;
		step->next = goal;
		for (;collect != goal; collect=next)
		{
		    next = collect->next;
		    routemap_free_step(collect);
		}
	    }
	}
    }
#endif
    /* Sometimes, after doing avoidance, we might find that we went through
     * a corridor which was very near to a goal point.  Detect those cases.
     */
    if (start->next->corridor &&
	start->next->next &&
	clear_path(routemap, start, start->next->next, obstacle_mask, TRUE))
    {
	collect = start->next;
	start->next = start->next->next;
	routemap_free_step(collect);
    }
    for (step=start;step->next;step=step->next)
    {
	/* Only consider corridor steps */
	if (!step->next->corridor)
	  continue;

	/* Find the next goal point after this step */
	for (goal=step->next; goal->corridor; goal=goal->next)
	  ;

	/* Check the segment from the current point directly to the goal
	 * point.
	 */
	if (clear_path(routemap, step, goal, obstacle_mask, TRUE))
	{
	    /* We can get directly to the goal step without the next few
	     * steps, so splice them out.
	     */
	    for (collect=step->next;collect!=goal;collect=next)
	    {
		next = collect->next;
		routemap_free_step(collect);
	    }
	    step->next = goal;
	}
    }

    /* Determine which (if any) goal points are inside obstacles, and
     * eliminate corridors attached to those obstacles.
     */
    prev = NULL;
    do
    {
	repeat = FALSE;
	for (goal=start;goal;prev=goal,goal=goal->next)
	{
	    if (goal->corridor)
	      continue;

	    /* This is a goal step.  If it is in an obstacle, then the next
	     * step or the previous step will have a corridor which is
	     * attached to that obstacle.  Check each possible candidate.
	     */
	    obst = NULL;
	    if (prev && prev->corridor)
	    {
		if (point_in_obstacle(goal->x, goal->y,
				      prev->corridor->begin_obst))
		  obst = prev->corridor->begin_obst;
		else if (point_in_obstacle(goal->x, goal->y,
					   prev->corridor->end_obst))
		  obst = prev->corridor->end_obst;
	    }
	    if (!obst && goal->next && goal->next->corridor)
	    {
		if (point_in_obstacle(goal->x, goal->y,
				      goal->next->corridor->begin_obst))
		  obst = goal->next->corridor->begin_obst;
		else if (point_in_obstacle(goal->x, goal->y,
					   goal->next->corridor->end_obst))
		  obst = goal->next->corridor->end_obst;
	    }
	    if (!obst)
	      continue;

	    /* The point is inside an obstacle.  Splice out any corridors
	     * which touch this obstacle, then start over.
	     */
	    prev = start;
	    for (step=start->next;step;prev=step,step=step->next)
	    {
		if (step->corridor &&
		    ((step->corridor->begin_obst == obst) ||
		     (step->corridor->end_obst == obst)))
		{
		    collect = step;
		    prev->next = step->next;
		    step = prev;
		    routemap_free_step(collect);
		}
	    }

	    repeat = TRUE;
	    break;
	}
    } while (repeat);
}

/* Translate the step into a point, fitting it to our goal width */
static void fit_to_goal(step, prev, unit_width, max_error, lbound, rbound)
    ROUTEMAP_PLAN_STEP   *step;
    ROUTEMAP_PLAN_STEP   *prev;
    int32                 unit_width;
    float64               max_error;
    ROUTEMAP_BOUNDARY_PTR lbound;
    ROUTEMAP_BOUNDARY_PTR rbound;
{
    ROUTEMAP_PSEG seg;
    ROUTEMAP_CORRIDOR *corr;

    if (!step->corridor)
      step->width = unit_width;
    else
    {
	seg.x = prev->x;
	seg.y = prev->y;
	if (corr = step->next->corridor)
	{
	    seg.dx = corr->seg.x + 0.5 * corr->seg.dx;
	    seg.dy = corr->seg.y + 0.5 * corr->seg.dy;
	}
	else
	{
	    seg.dx = step->next->x;
	    seg.dy = step->next->y;
	}
	seg.dx -= seg.x;
	seg.dy -= seg.y;

	align_to_corridor(&seg, step->corridor, (float64)unit_width, step,
			  max_error, lbound, rbound);
    }
}

/* Ensure that any steps which were provided as input (non-corridor steps)
 * are not so wide that they intersect obstacles.
 */
static void trim_input(routemap, step, obstacle_mask, max_error)
    ROUTEMAP_PTR        routemap;
    ROUTEMAP_PLAN_STEP *step;
    uint32              obstacle_mask;
    float64             max_error;
{
    float64 x, y, r;
    ROUTEMAP_OBSTACLE *obst;
    float64 ts, t_small;

    /* We know that these are OK */
    if (step->corridor)
      return;

    x = step->x;
    y = step->y;
    r = (step->width * 0.5) + max_error;

    /* Look for intersections with obstacles.  Keep track of the smallest
     * parameter values of the intersections (closest to the point).
     */
    t_small = 2.0;

    for (obst=routemap->obstacles;obst;obst=obst->next)
      if (obstacle_point_intersection(obstacle_mask, obst, x, y, r, &ts))
      {
	  if (ts < t_small)
	    t_small = ts;
      }

    if (t_small < 1.0)
      step->width *= t_small;
}

/* Smooth this step such that it is not too much larger than the
 * next step, and not too much larger than the previous step.
 * Recursively check the entire list (we use recursion because this
 * list has no backward pointers).
 */
static void smooth_step(step, prev, max_grow, max_shrink,
			min_expansion_dist, max_error, lbound, rbound)
    ROUTEMAP_PLAN_STEP   *step;
    ROUTEMAP_PLAN_STEP   *prev;
    float64               max_grow; 
    float64               max_shrink; 
    float64               min_expansion_dist; 
    float64               max_error; 
    ROUTEMAP_BOUNDARY_PTR lbound;
    ROUTEMAP_BOUNDARY_PTR rbound;
{
    ROUTEMAP_PSEG seg;
    float64 dx, dy, max_width, dist;
    float64 max_next, max_prev;

    max_next = max_prev = step->width;

    if (step->next)
    {
	/* Smooth the next step, then smooth this one */
	smooth_step(step->next, step, max_grow, max_shrink,
		    min_expansion_dist, max_error, lbound, rbound);

	/* How large can we be, when constrained by our next step */
	if (step->next->width < step->width)
	{
	    dx = step->next->x - step->x;
	    dy = step->next->y - step->y;
	    dist = dx*dx + dy*dy;
	    if (dist < min_expansion_dist*min_expansion_dist)
	      dist = 0.0;
	    else
	      dist = fsqrt(dist);
	    max_next = step->next->width + dist * max_shrink;
	}
    }

    if (prev)
    {
	/* How large can we be, when constrained by our previous step */
	if (step->width > prev->width)
	{
	    dx = prev->x - step->x;
	    dy = prev->y - step->y;
	    dist = dx*dx + dy*dy;
	    if (dist < min_expansion_dist*min_expansion_dist)
	      dist = 0.0;
	    else
	      dist = fsqrt(dist);
	    max_prev = prev->width + dist * max_grow;
	}
    }

    /* Our maximum width is the tighter of the two constraints */
    if (max_next < max_prev)
      max_width = max_next;
    else
      max_width = max_prev;

    /* Adjust the width, if necessary */
    if (step->width > max_width)
    {
	if (step->corridor)
	{
	    if (prev)
	      seg.x = prev->x, seg.y = prev->y;
	    else
	      seg.x = step->x, seg.y = step->y;
	    if (step->next)
	      seg.dx = step->next->x - seg.x, seg.dy = step->next->y - seg.y;
	    else
	      seg.dx = step->x - seg.x, seg.dy = step->y - seg.y;

	    align_to_corridor(&seg, step->corridor, max_width,
			      step, max_error, lbound, rbound);
	}
	else
	  step->width = max_width;
    }
}

/* Eliminate accordian width changes */
static void smooth_plan(start, max_grow, max_shrink,
		        min_expansion_dist, max_error, lbound, rbound)
    ROUTEMAP_PLAN_STEP   *start;
    float64               max_grow;
    float64               max_shrink;
    float64               min_expansion_dist;
    float64               max_error;
    ROUTEMAP_BOUNDARY_PTR lbound;
    ROUTEMAP_BOUNDARY_PTR rbound;
{
    /* See the recursive helper routine, above.  We have to do this
     * recursively, because the list has no backward pointers.
     */
    smooth_step(start, NULL, max_grow, max_shrink,
		min_expansion_dist, max_error, lbound, rbound);

    /* Do it twice to ensure proper propagation of constraints */
    smooth_step(start, NULL, max_grow, max_shrink,
		min_expansion_dist, max_error, lbound, rbound);
}

/* Place points just prior to narrow corridors to encourage prompt
 * width changes.
 */
static void add_closer_points(routemap, obstacle_mask, start)
    ROUTEMAP_PTR          routemap;
    uint32                obstacle_mask;
    ROUTEMAP_PLAN_STEP   *start;
{
    ROUTEMAP_PLAN_STEP *step;
    ROUTEMAP_PLAN_STEP *prev;
    float64 x, y, dx, dy, dist, f;
    float64 max_dist2 = 
      routemap->add_point_distance * routemap->add_point_distance;
    
    if (routemap->add_point_distance)
    {
	for (step=start,prev=NULL;step;prev=step,step=step->next)
	{
	    if (!step->corridor)
	      continue;
	    
	    if (prev && !prev->corridor && !(prev->failed && step->failed) &&
		(step->width < routemap->add_point_width_ratio * prev->width))
	    {
		/* Check entrance of the tree canopy */
		dx = prev->x - step->x;
		dy = prev->y - step->y;
		if ((dist = dx*dx + dy*dy) < max_dist2)
		  continue;
		
		/* add a step between this step and the prev step */
		f = routemap->add_point_distance/fsqrt(dist);
		x = step->x + dx * f;
		y = step->y + dy * f;
		
		add_xy(x, y, prev);
		prev->next->width = prev->width;
		trim_input(routemap, step, obstacle_mask, 
			   routemap->max_error + MIN_ERROR);
	    }
	    else if (step->next && !step->next->corridor && 
		     !(step->failed && step->next->failed) &&
		     (step->width < 
		      routemap->add_point_width_ratio * step->next->width))
	    {
		/* Check exit of the tree canopy */
		dx = step->next->x - step->x;
		dy = step->next->y - step->y;
		if ((dist = dx*dx + dy*dy) < max_dist2)
		  continue;
		
		/* add a step between this step and the prev step */
		f = routemap->add_point_distance/fsqrt(dist);
		x = step->x + dx * f;
		y = step->y + dy * f;
		
		add_xy(x, y, step);
		step->next->width = step->next->next->width;
		trim_input(routemap, step->next, obstacle_mask, 
			   routemap->max_error + MIN_ERROR);
	    }
	}
    }
}

/* Determine on which side of a linear obstacle the point lies */
static int32 side_of_line(x, y, obst, use_index)
    float64            x, y;
    ROUTEMAP_OBSTACLE *obst;
    float64           *use_index;
{
    float64 dx, dy, dist, shortest_dist;
    int32 i, shortest_i;
    int32 side;

    shortest_dist = 1.0E+99;
    shortest_i = -1;
    for (i=0;i<obst->n_verts;i++)
    {
	dx = x - obst->verts[i].seg.x;
	dy = y - obst->verts[i].seg.y;
	dist = dx*dx + dy*dy;
	if (dist < shortest_dist)
	{
	    shortest_dist = dist;
	    shortest_i = i;
	}
    }

    if (shortest_i == -1)
      return 0;

    if (routemap_negative_side(x, y, obst->verts, shortest_i))
      side = -1;
    else
      side = 1;

    /* See if we are beyond the end of the obstacle */
    if (!shortest_i)
    {
	dx = x - obst->verts[0].seg.x;
	dy = y - obst->verts[0].seg.y;
	if ((dx * obst->verts[0].seg.dx + dy * obst->verts[0].seg.dy) < 0.0)
	{
	    *use_index = side * 1.0E-10;
	    return 0;
	}
    }
    else if (shortest_i == obst->n_verts-1)
    {
	dx = x - obst->verts[shortest_i].seg.x;
	dy = y - obst->verts[shortest_i].seg.y;
	shortest_i = obst->n_verts-2;
	if ((dx * obst->verts[shortest_i].seg.dx +
	     dy * obst->verts[shortest_i].seg.dy) > 0.0)
	{
	    *use_index = side * (obst->n_verts-1 - 1.0E-10);
	    return 0;
	}
    }

    return side;
}

/* Determine if the corridor is on the required side of the linear obstacle.
 */
static int32 same_side(obst, corr, side)
    ROUTEMAP_OBSTACLE *obst;
    ROUTEMAP_CORRIDOR *corr;
    int32              side;
{
    float64 index;

    index = routemap_corr_index(corr, obst);

    /* Corridors coming out the ends are acceptable from either side */
    if ((index == 0.0) || (index == obst->n_verts-1))
      return TRUE;

    return ((index < 0.0) == (side < 0));
}

/* Find the shortest distance between a point and a segment */
static float64 pt_seg_dist(x, y, seg_in, seg_in_len_sq)
    float64        x, y;
    ROUTEMAP_PSEG *seg_in;
    float64        seg_in_len_sq;
{
    float64 dx, dy, dist, best_dist;
    float64 dot, cross;
    ROUTEMAP_PSEG seg;
    float64 seg_len_sq;

#define SMIDGE 0.01
#define SMIDGE_L 0.98
#define SMIDGE_SQ 0.9604

    /* Shorten the segment a little so we can distinguish between
     * corridors with the same starting point.
     */
    seg.x = seg_in->x + seg_in->dx * SMIDGE;
    seg.y = seg_in->y + seg_in->dy * SMIDGE;
    seg.dx = seg_in->dx * SMIDGE_L;
    seg.dy = seg_in->dy * SMIDGE_L;
    seg_len_sq = seg_in_len_sq * SMIDGE_SQ;

    /* Check the distance to one end */
    dx = x - seg.x;
    dy = y - seg.y;
    best_dist = dx*dx + dy*dy;

    /* See if we are next to the segment */
    dot = dx * seg.dx + dy * seg.dy;
    dx -= seg.dx;
    dy -= seg.dy;
    if (dot > 0.0)
    {
	dot = dx * seg.dx + dy * seg.dy;
	if (dot < 0.0)
	{
	    /* Check the distance to the segment */
	    cross = dx * seg.dy - dy * seg.dx;
	    dist = cross * cross / seg_len_sq;

	    if (dist < best_dist)
	      best_dist = dist;
	}
    }

    /* Check the distance to the other end */
    dist = dx*dx + dy*dy;

    if (dist < best_dist)
      best_dist = dist;

    return best_dist;
}

/* Determine if there is a connection between two corridors */
static int32 are_connected(c0, c1)
    ROUTEMAP_CORRIDOR *c0, *c1;
{
    int32 i;

    for (i=0;i<c0->n_connect;i++)
      if (c0->connect[i] == c1)
	return TRUE;
    return FALSE;
}

/* Get the safest corridor around a bypass obstacle */
static ROUTEMAP_CORRIDOR *bypass_corridor(obst, seg, do_positive)
    ROUTEMAP_OBSTACLE *obst;
    ROUTEMAP_PSEG     *seg;
    int32              do_positive;
{
    int32 i, best;
    int32 side;
    float64 use_index, dist, closest, dx, dy;

    /* Find which side of the bypass obstacle we are on */
#if 0
    side = side_of_line(seg->x, seg->y, obst, &use_index);
    if (!side)
    {
	if (use_index < 0.0)
	  side = -1;
	else
	  side = 1;
    }
#else
    if (do_positive)
      side = 1;
    else
      side = -1;
#endif

    /* Get the closest corridor on that side */
    best = -1;
    closest = 1.0E+99;
    for (i=0;i<obst->n_corridors;i++)
    {
	if (!same_side(obst, obst->corridors[i], side))
	  continue;

	dx = obst->corridors[i]->seg.x+0.5*obst->corridors[i]->seg.dx-seg->x;
	dy = obst->corridors[i]->seg.y+0.5*obst->corridors[i]->seg.dy-seg->y;
	dist = dx*dx + dy*dy;
	if (dist < closest)
	{
	    best = i;
	    closest = dist;
	}
    }
    if (best == -1)
      return NULL;
    return obst->corridors[best];
}

/* Choose a bypass to get around an intransigent obstacle */
static ROUTEMAP_CORRIDOR *choose_bypass(obst, from, do_positive, seg)
    ROUTEMAP_OBSTACLE *obst;
    ROUTEMAP_OBSTACLE *from;
    int32              do_positive;
    ROUTEMAP_PSEG     *seg;
{
    struct routemap_bypass *bypass;
    ROUTEMAP_CORRIDOR *start_corr, *end_corr;
    float64 start_dist, end_dist;

    start_corr = bypass_corridor(obst, seg, do_positive);
    if (!obst->bypass || start_corr)
      return start_corr;

    bypass = obst->bypass;
    start_corr = end_corr = NULL;

    if (do_positive)
    {
	if (bypass->positive_start && (bypass->positive_start != from))
	  start_corr = choose_bypass(bypass->positive_start, obst,
				     !bypass->positive_start_flip, seg);
	if (bypass->positive_end && (bypass->positive_end != from))
	  end_corr = choose_bypass(bypass->positive_end, obst,
				   !bypass->positive_end_flip, seg);
    }
    else
    {
	if (bypass->negative_start && (bypass->negative_start != from))
	  start_corr = choose_bypass(bypass->negative_start, obst,
				     bypass->negative_start_flip, seg);
	if (bypass->negative_end && (bypass->negative_end != from))
	  end_corr = choose_bypass(bypass->negative_end, obst,
				   bypass->negative_end_flip, seg);
    }

    /* If we only found one choice, return it */
    if (!end_corr)
      return start_corr;
    if (!start_corr)
      return end_corr;

    /* Choose the closer choice */
    start_dist = routemap_seg_seg_dist_sq(seg, &start_corr->seg);
    end_dist = routemap_seg_seg_dist_sq(seg, &end_corr->seg);
    if (start_dist < end_dist)
      return start_corr;
    else
      return end_corr;
}

/* Choose a corridor to get around an obstacle */
static ROUTEMAP_CORRIDOR *choose_corridor(routemap, obst, index, seg,
					  alternate)
    ROUTEMAP_PTR        routemap;
    ROUTEMAP_OBSTACLE  *obst;
    float64             index;
    ROUTEMAP_PSEG      *seg;
    ROUTEMAP_CORRIDOR **alternate;
{
    int32 i, skipit;
    int32 side;
    ROUTEMAP_CORRIDOR *corr0, *corr1;
    float64 cindex0, cindex1, use_index;
    float64 dist0, dist1;

    *alternate = NULL;

    if (!obst->bypass && !obst->n_corridors)
      return NULL;

    /* If the obstacle is linear give the index the appropriate sign */
    if (obst->linear)
    {
	side = side_of_line(seg->x, seg->y, obst, &use_index);
	if (side == -1)
	  index = -index;
	else if (!side)
	  index = use_index;
    }
    else
      side = 0;

    if (obst->n_corridors)
    {
	/* The passed index tells us between which vertices the path
	 * intersected the obstacle.  We have to choose a corridor
	 * which is adjacent to this intersection point.  There will
	 * generally be two points.  Find them.
	 */
	corr0 = obst->corridors[0];
	cindex0 = routemap_corr_index(corr0, obst);
	corr1 = NULL;
	for (i=1;i<obst->n_corridors;i++)
	{
	    corr1 = obst->corridors[i];
	    cindex1 = routemap_corr_index(corr1, obst);

	    /* If we are on the positive or negative side of a river
	     * (side != 0), then we have to be careful about corridors
	     * attached to the end points, as there may be more than one.
	     */
	    if (side)
	      skipit = (cindex0 == cindex1) &&
		((cindex0 == 0.0) || (cindex0 == obst->n_verts-1));
	    else
	      skipit = FALSE;

	    /* Is the index between these two corridors? */
	    if (!skipit && ((cindex0 <= index) && (index <= cindex1)))
	      break;

	    corr0 = corr1;
	    cindex0 = cindex1;
	}
	if (i==obst->n_corridors)
	{
	    corr1 = obst->corridors[0];
	    cindex1 = routemap_corr_index(corr1, obst);
	}

	/* Don't go to a corridor on the other side of a linear obstacle */
	if (obst->linear)
	{
	    /* If we are beyond an end, the `side' is 0, but the sign of
	     * the `index' tells us which side we really are on.
	     */
	    if (!side)
	    {
		if (index < 0.0)
		  side = -1;
		else
		  side = 1;
	    }
	    if (corr1 && !same_side(obst, corr1, side))
	      corr1 = NULL;
	    if (!same_side(obst, corr0, side))
	      corr0 = corr1, corr1 = NULL;

	    /* If we found two corridors but they are not connect to each
	     * other, one of them must be wrong.
	     */
	    if (corr0 && corr1 && !are_connected(corr0, corr1))
	    {
		if ((cindex1 == 0.0) || (cindex1 == obst->n_verts-1))
		  corr1 = NULL;
		else
		  corr0 = corr1, corr1 = NULL;
	    }
	}

	/* If we have two corridors available, go to the closer one */
	if (corr1)
	{
	    dist0 = pt_seg_dist(seg->x, seg->y, &corr0->seg,
				corr0->length*corr0->length);
	    dist1 = pt_seg_dist(seg->x, seg->y, &corr1->seg,
				corr1->length*corr1->length);
	    if (dist1 < dist0)
	    {
		*alternate = corr0;
		return corr1;
	    }
	}
	*alternate = corr1;
    }
    else
      corr0 = NULL, *alternate = NULL;

    /* If we didn't find any corridors, look for a bypass obstacle */
    if (!corr0 && obst->bypass && (index != 0.0))
    {
	corr0 = choose_bypass(obst, NULL, (index > 0.0), seg);
    }

    return corr0;
}

/* Find the first and last corridors crossed by the segment.  Optionally,
 * guarantee the clump from which a corridor is/isn't selected.
 */
static int32 corridor_crossings(routemap, obstacle_mask, x0, y0, x1, y1,
				explore_all, first_clump, last_clump,
				corr_list, t_small_ret, t_large_ret, failed)
    ROUTEMAP_PTR           routemap;
    uint32                 obstacle_mask;
    float64                x0, y0;
    float64                x1, y1;
    int32                  explore_all;
    ROUTEMAP_CLUMP        *first_clump;
    ROUTEMAP_CLUMP        *last_clump;
    struct corridor_pair   corr_list[];
    float64               *t_small_ret;
    float64               *t_large_ret;
    int32                 *failed;
{
    ROUTEMAP_PSEG seg;
    struct bounding_box box;
    int32 i, cnt;
    ROUTEMAP_CORRIDOR *corr, *small_corr, *large_corr, *small_alt, *large_alt;
    ROUTEMAP_OBSTACLE *obst, *small_obst, *large_obst;
    ROUTEMAP_CLUMP *small_clump, *large_clump;
    struct corridor_pair sub_list[2];
    float64 ts, tl, ins, inl, t_small, t_large, small_index, large_index, ign;

    seg.x = x0;
    seg.y = y0;
    seg.dx = x1 - x0;
    seg.dy = y1 - y0;
    get_bounding(&seg, &box);

    small_corr = large_corr = small_alt = large_alt = NULL;
    small_obst = large_obst = NULL;
    t_small = 2.0;
    t_large = -1.0;

    if (explore_all)
    {
	/* Search the whole database for corridors which cross the
	 * segment
	 */
	for (corr=routemap->corridors;corr;corr=corr->next)
	  if (corridor_intersection(obstacle_mask, corr, &seg, &ts))
	  {
	      if (ts < t_small)
	      {
		  t_small = ts;
		  small_corr = corr;
	      }
	      if (ts > t_large)
	      {
		  t_large = ts;
		  large_corr = corr;
	      }
	  }
    }
    else
    {
	/* We can limit the search to the clumps we're interested in.
	 * (Note that this is never called with the same clump specified
	 * for first_clump and last_clump, so there is no use trying to
	 * optimize that case.)
	 */
	for (i=0;i<first_clump->n_corridors;i++)
	{
	    corr = first_clump->corridors[i];
	    if (corridor_intersection(obstacle_mask, corr, &seg, &ts) &&
		(ts < t_small))
	    {
		t_small = ts;
		small_corr = corr;
	    }
	}
	for (i=0;i<last_clump->n_corridors;i++)
	{
	    corr = last_clump->corridors[i];
	    if (corridor_intersection(obstacle_mask, corr, &seg, &ts) &&
		(ts > t_large))
	    {
		t_large = ts;
		large_corr = corr;
	    }
	}
    }

    /* Search for obstacles which cross the segment closer to the
     * start/end points than those corridors.
     */
    if (explore_all)
    {
	/* Search the whole database */
	for (obst=routemap->obstacles;obst;obst=obst->next)
	  if ((obst->clump != first_clump) &&
	      (obst->clump != last_clump) &&
	      obstacle_intersection(obstacle_mask, obst, &seg, &box, &ts, &tl,
				    &ins, &inl))
	  {
	      if (ts < t_small)
	      {
		  t_small = ts;
		  small_obst = obst;
		  small_index = ins;
	      }
	      if (tl > t_large)
	      {
		  t_large = tl;
		  large_obst = obst;
		  large_index = inl;
	      }
	  }
    }
    else
    {
	/* Limit the search to the clumps of interest */
	for (i=0;i<first_clump->n_obstacles;i++)
	  if (obstacle_intersection(obstacle_mask,
				    first_clump->obstacles[i],
				    &seg, &box, &ts, &tl, &ins, &inl) &&
	      (ts < t_small))
	  {
	      t_small = ts;
	      small_obst = first_clump->obstacles[i];
	      small_index = ins;
	  }
	for (i=0;i<last_clump->n_obstacles;i++)
	  if (obstacle_intersection(obstacle_mask,
				    last_clump->obstacles[i],
				    &seg, &box, &ts, &tl, &ins, &inl) &&
	      (tl > t_large))
	  {
	      t_large = tl;
	      large_obst = last_clump->obstacles[i];
	      large_index = inl;
	  }
    }

    *t_small_ret = t_small;
    *t_large_ret = t_large;

    /* Change obstacles to corridors */
    if (small_obst)
      small_corr = choose_corridor(routemap, small_obst, small_index,
				   &seg, &small_alt);
    if (large_obst)
    {
	ROUTEMAP_PSEG rev_seg;
	rev_seg.x = seg.x+seg.dx;
	rev_seg.y = seg.y+seg.dy;
	rev_seg.dx = -seg.dx;
	rev_seg.dy = -seg.dy;
	large_corr = choose_corridor(routemap, large_obst, large_index,
				     &rev_seg, &large_alt);
    }

    /* Did we have trouble with that step? */
    if (small_obst && !small_corr ||
	large_obst && !large_corr)
      *failed = TRUE;

    /* Determine which, if any, clumps the intersections fall in */
    if (small_corr)
      small_clump = small_corr->clump;
    else
      return 0;
    if (large_corr)
      large_clump = large_corr->clump;
    else
      return 0;

    /* If we were told which clumps, then just return what we came
     * up with.  Also, if the first and last crossing belong to
     * the same clump, we can just return them.
     */
    if (!explore_all || (small_clump == large_clump))
    {
	corr_list[0].primary = small_corr;
	corr_list[0].alternate = small_alt;
	corr_list[1].primary = large_corr;
	corr_list[1].alternate = large_alt;
	return 2;
    }

    /* Avoid infinite recursion by catching the case of getting stuck
     * in a pair of clumps.
     */
    if (explore_all &&
	(small_clump == first_clump) && (large_clump == last_clump))
    {
	return 0;
    }

    /* If we are searching the whole database, then it is up to us to
     * ensure that the first and last intersections come from the same
     * clump.  If they don't, then we need to subdivide the space.
     */

    /* Find a line segment from the first corridor to the last
     * corridor.  Extend it slightly, so it intersects the corridors.
     */
    seg.x = small_corr->seg.x + 0.5 * small_corr->seg.dx;
    seg.y = small_corr->seg.y + 0.5 * small_corr->seg.dy;
    seg.dx = large_corr->seg.x + 0.5 * large_corr->seg.dx - seg.x;
    seg.dy = large_corr->seg.y + 0.5 * large_corr->seg.dy - seg.y;
    {
	float64 mag = 0.01 / fsqrt(seg.dx*seg.dx + seg.dy*seg.dy);
	seg.x -= seg.dx*mag;
	seg.y -= seg.dy*mag;
	seg.dx *= 1.0+mag+mag;
	seg.dy *= 1.0+mag+mag;
    }

    /* Find the opposite points for the clumps we have (this should always
     * return 2, but check to make sure).
     */
    if (corridor_crossings(routemap, obstacle_mask,
			   seg.x, seg.y, seg.x + seg.dx, seg.y + seg.dy,
			   FALSE, large_clump, small_clump,
			   sub_list, &t_large, &t_small, failed) != 2)
    {
	*failed = TRUE;
	return 0;
    }

    /* Compute the shorter line which connects these two clumps */
    x0 = seg.x + seg.dx * t_small;
    y0 = seg.y + seg.dy * t_small;
    x1 = seg.x + seg.dx * t_large;
    y1 = seg.y + seg.dy * t_large;

    /* The list of corridors will now be as follows:
     *
     * Entrance point to first clump         [small_corr]
     *  Exit point of first clump            [sub_list[1]]
     *   (any other corridors in between)    recurse
     *     Entrance point of the last clump  [sub_list[0]]
     *      Exit point to last clump         [large_corr]
     */
    corr_list[0].primary = small_corr;
    corr_list[0].alternate = small_alt;
    corr_list[1] = sub_list[1];
    cnt = corridor_crossings(routemap, obstacle_mask, x0, y0, x1, y1,
			     TRUE, small_clump, large_clump, corr_list+2,
			     &ign, &ign, failed);
    corr_list[cnt+2] = sub_list[0];
    corr_list[cnt+3].primary = large_corr;
    corr_list[cnt+3].alternate = large_alt;
    return cnt+4;
}

/* Plan a course without violating certain constraints */
int32 routemap_preplan_constrained(routemap, obstacle_mask, goal,
				   unit_width, num_clists, clists,
				   left_bound, right_bound, plan)
    ROUTEMAP_PTR            routemap;
    uint32                  obstacle_mask;
    ROUTE_POINTS           *goal;
    int32                   unit_width;
    int32                   num_clists;
    ROUTEMAP_CORRIDOR_LIST *clists[];
    ROUTEMAP_BOUNDARY_PTR   left_bound;
    ROUTEMAP_BOUNDARY_PTR   right_bound;
    ROUTE_POINTS           *plan;
{  

    my_preplan(routemap,routemap_my_no_go_map,routemap_my_visib_graph,*goal,plan);

#if 0
    int32 result;

    if (!routemap)
      return routemap_preplan(routemap, obstacle_mask, goal, unit_width, plan);

    /* Set the constraints */
    routemap->left_bound = left_bound;
    routemap->right_bound = right_bound;
    routemap->num_clists = num_clists;
    routemap->clists = clists;

    /* Plan the course */
    result =
      routemap_preplan(routemap, obstacle_mask, goal, unit_width, plan);

    /* Forget the constraints */
    routemap->left_bound = routemap->right_bound = NULL;
    routemap->num_clists = 0;

    return result;
#endif
}