#ifndef lint static char rcsid [] = "$RCSfile$ $Revision$ $State$"; #endif /**************************************************************************** * File: rtmp_init.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 create and initialize a routemap * * Created: Tue Nov 9 1993 * * Author: jesmith * * Remarks: * * * ****************************************************************************/ #include #include "librtmp_local.h" #include "/usr/modsaf/common/libsrc/libctdb/libctdb.h" #include "/usr/modsaf/common/libsrc/libreader/libreader.h" #include #include #include /*common/include/global*/ #include /*common/include/global*/ #include /*common/include/global*/ /**** MY OWN GLOBALS ********/ map *routemap_my_no_go_map; /* my representation of the obstacles */ graph *routemap_my_visib_graph; /* visibility graph */ static void add_river(); static void add_abstract(); static void clump_obstacle(); static void find_corridors(); static void find_bypasses(); static void filter_corridors(); static void invent_corridors(); static void sort_obstacle_corridors(); static void sort_corridors(); static void create_clumps(); static void identify_shortcuts(); /* Square of the minimum distance between obstacles. Anything closer is * considered 'touching'. */ #define TOUCHING_SQ (0.5*0.5) ROUTEMAP_PTR routemap_create(ctdb, data_path, reader_flags, num_squares, num_glue_points) CTDB *ctdb; char *data_path; uint32 reader_flags; char *num_squares; char *num_glue_points; { float32 min_x,max_x,min_y,max_y; int32 max_verts,max_polys; static char vis_graph_filename[200]; CTDB_SEARCH_SPACE_PTR space; READER_UNION ru, *db, *values, *val; static int32 four=4, fourteen=14; ROUTEMAP_PTR result = (ROUTEMAP_PTR) STDALLOC(sizeof(struct routemap_data)); bzero(result, sizeof(struct routemap_data)); result->ctdb = ctdb; /* Stuff in some default values, in case something goes wrong with * the data file. */ result->max_error = 50.0; result->max_corridor = 1000.0; result->max_grow = 0.2; result->max_shrink = 0.33; result->min_expansion_dist = 50.0; result->lakes.num_soils = 1; result->lakes.soils = &four; result->boulders.num_soils = 1; result->boulders.soils = &fourteen; result->distance_weighting=1.5; result->small_width=12; result->large_width=40; min_x=0.0; min_y=0.0; max_x=999999.0; max_y=999999.0; max_verts=200; max_polys=2000; routemap_my_strcpy(vis_graph_filename,"./default_vis_graph_filename.vis"); if (reader_read("routemap.rdr", data_path, &ru, reader_flags & ~READER_TYPING)) { printf("Error reading routemap data file. Using default values\n"); } else { char down_name[CTDB_NAME_LENGTH+1]; char *c, *d; for (c=ctdb->name,d=down_name;*c;c++,d++) if (isupper(*c)) *d = tolower(*c); else *d = *c; *d = 0; db = reader_find_tag(reader_get_symbol("databases"), ru.array, READER_UNTAGGED, READER_ERRORS_WITH_CONTEXT); if (!(values = reader_find_tag(reader_get_symbol(down_name), db, READER_TAGGED, READER_NO_ERRORS))) { printf("Routemap rules not specified for \"%s\", using defaults\n", down_name); values = reader_find_tag(reader_get_symbol("default"), db, READER_TAGGED, READER_ERRORS_WITH_CONTEXT); } if (values) { /*** my additions to the redaer file - oded *********/ if (val = reader_find_tag(reader_get_symbol("distance_weighting"), values, READER_TAGGED, READER_ERRORS_WITH_DESCRIPTION, "Error in routemap.rdr")) result->distance_weighting = val[2].real; if (val = reader_find_tag(reader_get_symbol("small_width"), values, READER_TAGGED, READER_ERRORS_WITH_DESCRIPTION, "Error in routemap.rdr")) result->small_width = val[2].real; if (val = reader_find_tag(reader_get_symbol("large_width"), values, READER_TAGGED, READER_ERRORS_WITH_DESCRIPTION, "Error in routemap.rdr")) result->large_width = val[2].real; if (val = reader_find_tag(reader_get_symbol("min_x"), values, READER_TAGGED, READER_ERRORS_WITH_DESCRIPTION, "Error in routemap.rdr")) min_x = val[2].real; if (val = reader_find_tag(reader_get_symbol("max_x"), values, READER_TAGGED, READER_ERRORS_WITH_DESCRIPTION, "Error in routemap.rdr")) max_x = val[2].real; if (val = reader_find_tag(reader_get_symbol("min_y"), values, READER_TAGGED, READER_ERRORS_WITH_DESCRIPTION, "Error in routemap.rdr")) min_y = val[2].real; if (val = reader_find_tag(reader_get_symbol("max_y"), values, READER_TAGGED, READER_ERRORS_WITH_DESCRIPTION, "Error in routemap.rdr")) max_y = val[2].real; if (val = reader_find_tag(reader_get_symbol("vis_graph_file"), values, READER_TAGGED, READER_ERRORS_WITH_DESCRIPTION, "Error in routemap.rdr")) routemap_my_strcpy(vis_graph_filename,val[2].charptr); if (val = reader_find_tag(reader_get_symbol("max_num_verts"), values, READER_TAGGED, READER_ERRORS_WITH_DESCRIPTION, "Error in routemap.rdr")) max_verts = val[2].integer; if (val = reader_find_tag(reader_get_symbol("max_num_polys"), values, READER_TAGGED, READER_ERRORS_WITH_DESCRIPTION, "Error in routemap.rdr")) max_polys = val[2].integer; /****************************************************/ if (val = reader_find_tag(reader_get_symbol("max_error"), values, READER_TAGGED, READER_ERRORS_WITH_DESCRIPTION, "Error in routemap.rdr")) result->max_error = val[2].real; if (val = reader_find_tag(reader_get_symbol("max_corridor"), values, READER_TAGGED, READER_ERRORS_WITH_DESCRIPTION, "Error in routemap.rdr")) result->max_corridor = val[2].real; if (val = reader_find_tag(reader_get_symbol("max_grow"), values, READER_TAGGED, READER_ERRORS_WITH_DESCRIPTION, "Error in routemap.rdr")) result->max_grow = val[2].real; if (val = reader_find_tag(reader_get_symbol("max_shrink"), values, READER_TAGGED, READER_ERRORS_WITH_DESCRIPTION, "Error in routemap.rdr")) result->max_shrink = val[2].real; if (val = reader_find_tag(reader_get_symbol("min_expansion_dist"), values, READER_TAGGED, READER_ERRORS_WITH_DESCRIPTION, "Error in routemap.rdr")) result->min_expansion_dist = val[2].real; if (val = reader_find_tag(reader_get_symbol("add_point_distance"), values, READER_TAGGED, READER_ERRORS_WITH_DESCRIPTION, "Error in routemap.rdr")) result->add_point_distance = val[2].real; if (val=reader_find_tag(reader_get_symbol("add_point_width_ratio"), values, READER_TAGGED, READER_ERRORS_WITH_DESCRIPTION, "Error in routemap.rdr")) result->add_point_width_ratio = val[2].real; } db = reader_find_tag(reader_get_symbol("soils"), ru.array, READER_UNTAGGED, READER_ERRORS_WITH_CONTEXT); if (val = reader_find_tag(reader_get_symbol("lakes"), db, READER_TAGGED, READER_ERRORS_WITH_DESCRIPTION, "Error in routemap.rdr")) { result->lakes.num_soils = val[0].integer - 2; result->lakes.soils = &val[2].integer; } if (val = reader_find_tag(reader_get_symbol("boulders"), db, READER_TAGGED, READER_ERRORS_WITH_DESCRIPTION, "Error in routemap.rdr")) { result->boulders.num_soils = val[0].integer - 2; result->boulders.soils = &val[2].integer; } } /* Look at each patch */ printf("Building simplified map..."); fflush(stdout); space = ctdb_create_search_space(ctdb, ctdb->min_x, ctdb->min_y, ctdb->max_x, ctdb->max_y, CTDB_BY_PATCH); while (ctdb_next_patch(space, NULL)) { add_abstract(space, CTDB_ABSTRACT_SOIL_DEFRAG, result); add_abstract(space, CTDB_ABSTRACT_CANOPY, result); add_abstract(space, CTDB_ABSTRACT_STEEP_SLOPE, result); } /* Do rivers by traversing the much-longer edges, not the much-shorter * linear features stored per patch. */ add_river(result); /* Find bypasses for linear obstacles which are blocked at both * ends. */ printf("Finding bypasses..."); fflush(stdout); /* ELIMINATING THIS SINCE WE ARE DOING VISIBILITY GRAPH STUFF NOW */ /* find_bypasses(result); */ /* Create the corridors between obstacles */ printf("Finding corridors..."); fflush(stdout); find_corridors(result, (int32)ctdb->max_x, (int32)ctdb->max_y); #if 0 /* Create corridors in free space */ printf("Inventing corridors..."); fflush(stdout); invent_corridors(result); /* Eliminate corridors which violate triangulation */ printf("Filtering corridors..."); fflush(stdout); filter_corridors(result); /* Sort corridors into a network */ printf("Sorting corridors..."); fflush(stdout); sort_corridors(result); /* Combine obstacles and corridors into clumps */ printf("Creating clumps..."); fflush(stdout); create_clumps(result); /* Add shortcut corridor connections */ printf("Adding shortcuts..."); fflush(stdout); identify_shortcuts(result); #endif routemap_my_no_go_map=(map *) (malloc (sizeof(map))); routemap_my_visib_graph=(graph *) (malloc (sizeof(graph))); result->num_x_squares=atoi(num_squares); result->num_y_squares=atoi(num_squares); result->num_glue_points=atoi(num_glue_points); routemap_create_map_and_initial_visib_graph(result,routemap_my_no_go_map,routemap_my_visib_graph, max_verts,max_polys,min_x,min_y,max_x, max_y,vis_graph_filename); printf("done\n"); /* exit(2); */ return result; } /* Allocate an obstacle, and put it in the master list */ static ROUTEMAP_OBSTACLE *create_obstacle(routemap, mask) ROUTEMAP_PTR routemap; uint32 mask; { ROUTEMAP_OBSTACLE *result = (ROUTEMAP_OBSTACLE *) STDALLOC(sizeof(ROUTEMAP_OBSTACLE)); bzero(result, sizeof(ROUTEMAP_OBSTACLE)); result->type_mask = mask; result->next = routemap->obstacles; routemap->obstacles = result; return result; } /* Add a corridor to an obstacle's list */ static void add_corridor(obstacle, corridor) ROUTEMAP_OBSTACLE *obstacle; ROUTEMAP_CORRIDOR *corridor; { /* Don't add to the fake obstacle we use when we invent corridors * around obstacles. */ if (!obstacle->type_mask) return; if (!obstacle->n_corridors) obstacle->corridors = (ROUTEMAP_CORRIDOR **) STDALLOC(sizeof(ROUTEMAP_CORRIDOR *)); else obstacle->corridors = (ROUTEMAP_CORRIDOR **) STDREALLOC(obstacle->corridors, (obstacle->n_corridors+1) * sizeof(ROUTEMAP_CORRIDOR *)); obstacle->corridors[obstacle->n_corridors++] = corridor; } /* Allocate a corridor between two obstacles */ static void create_corridor(routemap, ob0, index0, ob1, index1, prototype) ROUTEMAP_PTR routemap; ROUTEMAP_OBSTACLE *ob0; float64 index0; ROUTEMAP_OBSTACLE *ob1; float64 index1; ROUTEMAP_CORRIDOR *prototype; { ROUTEMAP_CORRIDOR *result; result = (ROUTEMAP_CORRIDOR *)STDALLOC(sizeof(ROUTEMAP_CORRIDOR)); *result = *prototype; result->begin_obst = ob0; result->begin_index = index0; result->end_obst = ob1; result->end_index = index1; result->prev = NULL; result->next = routemap->corridors; if (result->next) result->next->prev = result; routemap->corridors = result; result->length = fsqrt(result->seg.dx*result->seg.dx + result->seg.dy*result->seg.dy); add_corridor(ob0, result); add_corridor(ob1, result); } /* Take a corridor out of an obstacle's list */ static void compress_out_corridor(obst, corr) ROUTEMAP_OBSTACLE *obst; ROUTEMAP_CORRIDOR *corr; { int32 i; for (i=0;in_corridors;i++) if (obst->corridors[i] == corr) break; for (;in_corridors-1;i++) obst->corridors[i] = obst->corridors[i+1]; obst->n_corridors--; } /* Remove a corridor */ static void remove_corridor(routemap, corr) ROUTEMAP_PTR routemap; ROUTEMAP_CORRIDOR *corr; { compress_out_corridor(corr->begin_obst, corr); compress_out_corridor(corr->end_obst, corr); if (routemap->corridors == corr) routemap->corridors = corr->next; if (corr->prev) corr->prev->next = corr->next; if (corr->next) corr->next->prev = corr->prev; STDDEALLOC(corr); } /* Format of point in our approximation of an obstacle */ struct approx_point { int32 source_index; int32 x, y; }; /* Assign an approximation point the value of a real point */ static void assign_approx64(approx, points, source) struct approx_point *approx; float64 *points; int32 source; { approx->source_index = source; approx->x = points[2*source]; approx->y = points[2*source+1]; } /* Assign an approximation point the value of a real point */ static void assign_approx32(approx, points, source) struct approx_point *approx; float32 *points; int32 source; { approx->source_index = source; approx->x = points[2*source]; approx->y = points[2*source+1]; } /* Assign an approximation point the value of a point along a real segment */ static void assign_clip_approx64(approx, clip, points, source, next_source) struct approx_point *approx; float64 clip; float64 *points; int32 source; int32 next_source; { float64 dx = points[2*next_source] - points[2*source]; float64 dy = points[2*next_source+1] - points[2*source+1]; float64 mag = fsqrt(dx*dx + dy*dy); /* Don't clip more than there is */ if (mag <= clip) clip = mag * 0.9; approx->source_index = source; approx->x = points[2*source] + clip * dx / mag; approx->y = points[2*source+1] + clip * dy / mag; } /* Insert a new point into the approximation */ static int32 insert_point64(approx, where, count, points, source) struct approx_point *approx; int32 where; int32 count; float64 *points; int32 source; { int32 i; /* Make room */ for (i=count;i>where;i--) approx[i] = approx[i-1]; /* Insert */ assign_approx64(approx+where, points, source); return count+1; } /* Insert a new point into the approximation */ static int32 insert_point32(approx, where, count, points, source) struct approx_point *approx; int32 where; int32 count; float32 *points; int32 source; { int32 i; /* Make room */ for (i=count;i>where;i--) approx[i] = approx[i-1]; /* Insert */ assign_approx32(approx+where, points, source); return count+1; } /* Finds the distance (squared) between a source point and an * approximating segment. */ static float64 dist_to_seg(x, y, a0, a1, alen_sq) float64 x, y; struct approx_point *a0, *a1; float64 alen_sq; { float64 cross = ((x - a0->x) * (a1->y - a0->y) - (y - a0->y) * (a1->x - a0->x)); return cross * cross / alen_sq; } /* Finds the distance (squared) between a point and an obstacle edge */ static float64 dist_to_edge(x, y, seg, len_sq) float64 x, y; ROUTEMAP_PSEG *seg; float64 len_sq; { float64 cross = ((x - seg->x) * seg->dy - (y - seg->y) * seg->dx); return cross * cross / len_sq; } /* Determines if a point is "next to" an edge by testing the sign of * the dot products from either end. Returns one of these dot products * (which depends upon the passed `reverse' flag) if the point is next * to the edge. Otherwise, it returns a negative number. */ static float64 next_to_edge(x, y, seg, reverse) float64 x, y; ROUTEMAP_PSEG *seg; int32 reverse; { float64 forward_dot, reverse_dot; if (((forward_dot = ((x - seg->x) * seg->dx + (y - seg->y) * seg->dy)) < 0.0) || ((reverse_dot = ((seg->x + seg->dx - x) * seg->dx + (seg->y + seg->dy - y) * seg->dy)) < 0.0)) return -1.0; if (reverse) return reverse_dot; else return forward_dot; } /* Determine in two parametric segment overlap (counting tips touching) */ static int32 segments_overlap_inclusive(s0, s1) ROUTEMAP_PSEG *s0; ROUTEMAP_PSEG *s1; { float64 lo, hi; #undef CHECK #define CHECK(a,b,c,d) \ if (a->d > 0.0) \ lo = a->c, hi = a->c + a->d; \ else \ hi = a->c, lo = a->c + a->d; \ if ((lo <= b->c) && (b->c <= hi)) \ return TRUE; CHECK(s0, s1, x, dx); CHECK(s0, s1, y, dy); CHECK(s1, s0, x, dx); CHECK(s1, s0, y, dy); return FALSE; } /* Determine in two parametric segment overlap (not counting tips touching) */ static int32 segments_overlap_exclusive(s0, s1) ROUTEMAP_PSEG *s0; ROUTEMAP_PSEG *s1; { float64 lo, hi; #undef CHECK #define CHECK(a,b,c,d) \ if (a->d > 0.0) \ lo = a->c, hi = a->c + a->d; \ else \ hi = a->c, lo = a->c + a->d; \ if ((lo < b->c) && (b->c < hi)) \ return TRUE; CHECK(s0, s1, x, dx); CHECK(s0, s1, y, dy); CHECK(s1, s0, x, dx); CHECK(s1, s0, y, dy); return FALSE; } /* Determine if two parametric segments cross (counting tips touching) */ static int32 segments_intersect_inclusive(s0, s1) ROUTEMAP_PSEG *s0; ROUTEMAP_PSEG *s1; { 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 */ return segments_overlap_inclusive(s0, s1); } t1 = numer / denom; /* 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) return TRUE; } return FALSE; } /* Determine if two parametric segments cross (not counting tips touching) */ static int32 segments_intersect_exclusive(s0, s1) ROUTEMAP_PSEG *s0; ROUTEMAP_PSEG *s1; { 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 */ return segments_overlap_exclusive(s0, s1); } t1 = numer / denom; /* Allow a few decimal place of FP error here */ #define LOWER_BOUND 0.001 #define UPPER_BOUND 0.999 /* Are we within the bounds of the second segment? */ if (t1 > LOWER_BOUND && t1 < UPPER_BOUND) { /* 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 > LOWER_BOUND && t0 < UPPER_BOUND) return TRUE; } return FALSE; } /* Copies a set of approximation points into an obstacle */ static void init_obst_points(obst, approx, count, reverse) ROUTEMAP_OBSTACLE *obst; struct approx_point *approx; int32 count; int32 reverse; { uint32 size = sizeof(struct routemap_obstacle_vertex) * count; int32 i, j, incr; int32 x, y, min_x, max_x, min_y, max_y; obst->n_verts = count; obst->verts = (struct routemap_obstacle_vertex *)STDALLOC(size); bzero(obst->verts, size); min_x = max_x = approx[0].x; min_y = max_y = approx[0].y; if (reverse) j = count-1, incr = -1; else j = 0, incr = 1; for (i=0;iverts[j].seg.x = x; obst->verts[j].seg.y = y; if (x < min_x) min_x = x; if (x > max_x) max_x = x; if (y < min_y) min_y = y; if (y > max_y) max_y = y; } for (i=0;iverts[i].seg.dx = obst->verts[i+1].seg.x - obst->verts[i].seg.x; obst->verts[i].seg.dy = obst->verts[i+1].seg.y - obst->verts[i].seg.y; obst->verts[i].len_sq = (obst->verts[i].seg.dx*obst->verts[i].seg.dx + obst->verts[i].seg.dy*obst->verts[i].seg.dy); } obst->center_x = (min_x + max_x) >> 1; obst->center_y = (min_y + max_y) >> 1; obst->width = (max_x - min_x) >> 1; obst->height = (max_y - min_y) >> 1; } /* Generate a simple approximation of a line. */ static void approximate_line(obst, points, n_points, clip_start, clip_end, max_error_sq) ROUTEMAP_OBSTACLE *obst; float64 *points; int32 n_points; float64 clip_start, clip_end; float64 max_error_sq; { int32 i, j, count; int32 farthest_index; float64 dist, farthest_dist; float64 dx, dy, alen_sq; struct approx_point approx[1024]; struct approx_point *a0, *a1; /* Start with just the first and last points */ if (clip_start > 0.0) assign_clip_approx64(approx+0, clip_start, points, 0, 1); else assign_approx64(approx+0, points, 0); if (clip_end > 0.0) assign_clip_approx64(approx+1, clip_end, points, n_points-1, n_points-2); else assign_approx64(approx+1, points, n_points-1); count = 2; /* Add extra points as needed to generate a close-enough approximation * of the line. */ for (i=0;ix - a0->x; dy = a1->y - a0->y; alen_sq = dx*dx + dy*dy; /* Find the point in the real obstacle which deviates the most * from our approximation (ignore points which are relatively * close). */ farthest_index = 0; farthest_dist = max_error_sq; for (j=a0->source_index+1;jsource_index;j++) { dist = dist_to_seg(points[2*j], points[2*j+1], a0, a1, alen_sq); if (dist > farthest_dist) { farthest_dist = dist; farthest_index = j; } } /* Are there not any worth worrying about? */ if (!farthest_index) { /* Move on to the next segment in the approximation */ i++; continue; } /* Insert this point into our approximation */ count = insert_point64(approx, i+1, count, points, farthest_index); } /* Create the obstacle's points */ init_obst_points(obst, approx, count, FALSE); } /* Generate a simple approximation of a polygon. */ static void approximate_polygon(obst, points, n_points, max_error_sq) ROUTEMAP_OBSTACLE *obst; float32 *points; int32 n_points; float64 max_error_sq; { int32 i, j, first, last, count; int32 farthest_index; int32 north, south; float64 dist, farthest_dist; float64 dx, dy, alen_sq; int32 reverse; struct approx_point approx[1024]; struct approx_point *a0, *a1; /* Start with the polar extremes */ north = south = 0; for (i=1;i points[2*north+1]) north = i; else if (points[2*i+1] < points[2*south+1]) south = i; } assign_approx32(approx+0, points, north); assign_approx32(approx+1, points, south); count = 2; /* Add extra points as needed to generate a close-enough approximation * of the polygon. */ for (i=0;ix - a0->x; dy = a1->y - a0->y; alen_sq = dx*dx + dy*dy; /* Find the point in the real obstacle which deviates the most * from our approximation (ignore points which are relatively * close). */ farthest_index = -1; farthest_dist = max_error_sq; first = (a0->source_index+1)%n_points; if (first != a1->source_index) { last = a1->source_index; for (j=first;j!=last;j=(j+1)%n_points) { dist = dist_to_seg(points[2*j], points[2*j+1], a0, a1, alen_sq); if (dist > farthest_dist) { farthest_dist = dist; farthest_index = j; } } } /* Are there not any worth worrying about? */ if (farthest_index == -1) { /* Move on to the next segment in the approximation */ i++; continue; } /* Insert this point into our approximation */ count = insert_point32(approx, i+1, count, points, farthest_index); } /* We determine if we will need to reverse these points to * get them counter-clockwise. The 0'th point is the northernmost * point of the polygon. Since it is an extrema, it cannot possibly * be the interior vertex in a concave section of the polygon. * Therefore, we can easily tell whether the concave is currently * counter-clockwise by using a cross product of the two vectors * attached to this point. */ if (count > 2) reverse = (((approx[1].x - approx[0].x) * (approx[count-1].y - approx[0].y) - (approx[1].y - approx[0].y) * (approx[count-1].x - approx[0].x)) < 0); else reverse = FALSE; /* Create the obstacle's points */ approx[count++] = approx[0]; init_obst_points(obst, approx, count, reverse); } /* Utility function to determine if there are any unfordable edges * coming out of this node (other than the one which we took into the node). */ static int32 intersection_is_open(routemap, node, ignore_edge) ROUTEMAP_PTR routemap; CTDB_NODE *node; int32 ignore_edge; { int32 i, j; int32 soil; for (i=0;inum_connections;i++) { if (node->connection[i].edge == ignore_edge) continue; soil = ctdb_edge_soil(routemap->ctdb, node->connection[i].edge); /* See if this soil is one we like to avoid */ for (j=0;jlakes.num_soils;j++) if (routemap->lakes.soils[j] == soil) break; if (j == routemap->lakes.num_soils) continue; return FALSE; } return TRUE; } /* Store all the water */ static void add_river(routemap) ROUTEMAP_PTR routemap; { int32 i, j; int32 n_verts; float64 *verts; int32 soil; float64 clip0, clip1; ROUTEMAP_OBSTACLE *obst; CTDB_NODE *node; for (i=0;ictdb->num_edges;i++) { n_verts = ctdb_expand_edge(routemap->ctdb, i, 0, &verts, NULL, &soil); /* Skip weird rivers */ if (n_verts < 2) continue; /* See if this soil is one we like to avoid */ for (j=0;jlakes.num_soils;j++) if (routemap->lakes.soils[j] == soil) break; if (j == routemap->lakes.num_soils) continue; /* Make the obstacle */ if (!(obst = create_obstacle(routemap, ROUTEMAP_RIVERS))) continue; obst->linear = 1; /* See if either endpoint is a bridge */ node = routemap->ctdb->nodes[routemap->ctdb->edges[i]->first_node]; if (node->is_bridge) { clip0 = 4.0; } else { obst->start_open = intersection_is_open(routemap, node, i); clip0 = 0.0; } node = routemap->ctdb->nodes[routemap->ctdb->edges[i]->last_node]; if (node->is_bridge) { clip1 = 4.0; } else { obst->end_open = intersection_is_open(routemap, node, i); clip1 = 0.0; } /* Construct a simple approximation of the river */ approximate_line(obst, verts, n_verts, clip0, clip1, routemap->max_error*routemap->max_error); } } /* Store all the abstracts of a type */ static void add_abstract(space, type_code, routemap) CTDB_SEARCH_SPACE_PTR space; int32 type_code; ROUTEMAP_PTR routemap; { int32 j; int32 n_verts; float32 *verts; CTDB_ABSTRACT_DATA *data; int32 soil; ROUTEMAP_OBSTACLE *obst; while (n_verts = ctdb_next_abstract(space, type_code, &verts, &data, NULL)) { /* Skip weird polygons */ if (n_verts < 3) continue; obst = NULL; if (type_code == CTDB_ABSTRACT_SOIL_DEFRAG) { soil = routemap->ctdb->soil_map[data->soil_defrag.soil_index]; for (j=0;jlakes.num_soils;j++) if (soil == routemap->lakes.soils[j]) { obst = create_obstacle(routemap, ROUTEMAP_LAKES); break; } if (!obst) for (j=0;jboulders.num_soils;j++) if (soil == routemap->boulders.soils[j]) { obst = create_obstacle(routemap, ROUTEMAP_BOULDERS); break; } } else if (type_code == CTDB_ABSTRACT_CANOPY) obst = create_obstacle(routemap, ROUTEMAP_CANOPIES); else if (type_code == CTDB_ABSTRACT_STEEP_SLOPE) obst = create_obstacle(routemap, ROUTEMAP_STEEP_AREAS); if (!obst) continue; /* Construct a simple approximation of the lake */ approximate_polygon(obst, verts, n_verts, routemap->max_error*routemap->max_error); } } /* Determine which quad node a box belongs in */ int32 routemap_select_quad(routemap, depth, index, min_x, min_y, max_x, max_y) ROUTEMAP_PTR routemap; int32 depth; int32 index; int32 min_x, min_y, max_x, max_y; { int32 corr, child; struct routemap_quad_info *info = routemap->quad_info+index; /* If this is a leaf node, then it must belong here. */ if (depth == ROUTEMAP_QUAD_DEPTH) return index; /* See if it belongs in a child. We put an obstacle into a child * node only if it does not come within one-half a corridor width * of the center lines of this quad. */ corr = routemap->max_corridor * 0.5; child = -1; if (min_x > info->cx+corr) { if (min_y > info->cy+corr) child = 0; else if (max_y < info->cy-corr) child = 3; } else if (max_x < info->cx-corr) { if (min_y > info->cy+corr) child = 1; else if (max_y < info->cy-corr) child = 2; } /* Does it go here? */ if (child == -1) return index; else return routemap_select_quad(routemap, depth+1, info->children[child], min_x, min_y, max_x, max_y); } /* Determine if a segment intersects any obstacles other than the * ones it connects. */ static int32 segment_intersects_obstacle(routemap, seg, ob0, ob1, check_corrs) ROUTEMAP_PTR routemap; ROUTEMAP_PSEG *seg; ROUTEMAP_OBSTACLE *ob0, *ob1; int32 check_corrs; { ROUTEMAP_OBSTACLE *i; int32 j, q, r, s; int32 cx, cy, size; int32 min_x, min_y, max_x, max_y; int32 (*intersection_test)(); /* Find the index of the topmost quad which contains this corridor */ if (seg->dx > 0.0) min_x = seg->x, max_x = seg->x + seg->dx; else max_x = seg->x, min_x = seg->x + seg->dx; if (seg->dy > 0.0) min_y = seg->y, max_y = seg->y + seg->dy; else max_y = seg->y, min_y = seg->y + seg->dy; q = routemap_select_quad(routemap, 1, 0, min_x, min_y, max_x, max_y); /* Find the rough location of this corridor */ cx = seg->x + 0.5 * seg->dx; cy = seg->y + 0.5 * seg->dy; size = max_x - min_x; if (size < max_y - min_y) size = max_y - min_y; /* Compare the corridor to obstacles in this quad and parent quads. */ for (r=q;r!=-1;r=routemap->quad_info[r].parent) for (i=routemap->quad_info[r].obstacles;i;i=i->next_quad) { /* Might the obstacle and the corridor overlap? */ if (((abs(i->center_x - cx)+abs(i->center_y - cy)) > i->width + i->height + size) || i->not_real) continue; if ((i == ob0) || (i == ob1)) intersection_test = segments_intersect_exclusive; else intersection_test = segments_intersect_inclusive; /* Look for intersections */ for (j=0;jn_verts-1;j++) if (intersection_test(&i->verts[j].seg, seg)) return TRUE; if (check_corrs) for (j=0;jn_corridors;j++) if (segments_intersect_exclusive(&i->corridors[j]->seg, seg)) return TRUE; } /* Same deal for descendant quads */ s = q + routemap->quad_info[q].num_descendants; for (r=q+1;r<=s;r++) for (i=routemap->quad_info[r].obstacles;i;i=i->next_quad) { /* Might the obstacle and the corridor overlap? */ /* Is this one which we are supposed to ignore? */ if (((abs(i->center_x - cx)+abs(i->center_y - cy)) > i->width + i->height + size) || i->not_real) continue; if ((i == ob0) || (i == ob1)) intersection_test = segments_intersect_exclusive; else intersection_test = segments_intersect_inclusive; /* Look for intersections */ for (j=0;jn_verts-1;j++) if (intersection_test(&i->verts[j].seg, seg)) return TRUE; if (check_corrs) for (j=0;jn_corridors;j++) if (segments_intersect_exclusive(&i->corridors[j]->seg, seg)) return TRUE; } return FALSE; } /* Find the shortest corridor between two obstacles, given their * closest vertices. */ static void best_corridor(ob0, ob1, vert0, vert1, vert_sep, seg, index0, index1) ROUTEMAP_OBSTACLE *ob0; ROUTEMAP_OBSTACLE *ob1; int32 vert0; int32 vert1; float64 vert_sep; ROUTEMAP_PSEG *seg; float64 *index0; float64 *index1; { float64 dist, shortest, dot; struct routemap_obstacle_vertex *v, *best_edge, *best_point; float64 x, y; /* There are up to four corridors to consider: * * -----0---- * \ / \/ * \/ /\ * /\ / \ * ----1------ */ shortest = vert_sep; best_edge = NULL; /* Check the two corridors going from vert0 */ x = ob0->verts[vert0].seg.x; y = ob0->verts[vert0].seg.y; /* Is there a segment coming into vert1? */ v = NULL; if (vert1) v = ob1->verts+vert1-1; else if (!ob1->linear && !ob1->not_real) v = ob1->verts+ob1->n_verts-2; if (v && ((dot = next_to_edge(x, y, &v->seg, TRUE)) > 0.0)) { dist = dist_to_edge(x, y, &v->seg, v->len_sq); if (dist < shortest) { shortest = dist; best_edge = v; best_point = ob0->verts+vert0; *index0 = vert0; *index1 = vert1 - dot / v->len_sq; if (*index1 < 0.0) *index1 += ob1->n_verts-1; } } /* Is there a segment coming out of vert1? */ if (vert1 < ob1->n_verts-1) { v = ob1->verts+vert1; if ((dot = next_to_edge(x, y, &v->seg, FALSE)) > 0.0) { dist = dist_to_edge(x, y, &v->seg, v->len_sq); if (dist < shortest) { shortest = dist; best_edge = v; best_point = ob0->verts+vert0; *index0 = vert0; *index1 = vert1 + dot / v->len_sq; } } } /* Check the two corridors going from vert1 */ x = ob1->verts[vert1].seg.x; y = ob1->verts[vert1].seg.y; /* Is there a segment coming into vert0? */ v = NULL; if (vert0) v = ob0->verts+vert0-1; else if (!ob0->linear && !ob0->not_real) v = ob0->verts+ob0->n_verts-2; if (v && ((dot = next_to_edge(x, y, &v->seg, TRUE)) > 0.0)) { dist = dist_to_edge(x, y, &v->seg, v->len_sq); if (dist < shortest) { shortest = dist; best_edge = v; best_point = ob1->verts+vert1; *index0 = vert0 - dot / v->len_sq; if (*index0 < 0.0) *index0 += ob0->n_verts-1; *index1 = vert1; } } /* Is there a segment coming out of vert0? */ if (vert0 < ob0->n_verts-1) { v = ob0->verts+vert0; if ((dot = next_to_edge(x, y, &v->seg, FALSE)) > 0.0) { dist = dist_to_edge(x, y, &v->seg, v->len_sq); if (dist < shortest) { shortest = dist; best_edge = v; best_point = ob1->verts+vert1; *index0 = vert0 + dot / v->len_sq; *index1 = vert1; } } } /* Did we find anything better than the point-to-point relationship * we started with? */ if (best_edge) { /* Find the corridor */ seg->x = best_point->seg.x; seg->y = best_point->seg.y; dist = fsqrt(shortest / best_edge->len_sq); if (((seg->x - best_edge->seg.x) * best_edge->seg.dy - (seg->y - best_edge->seg.y) * best_edge->seg.dx) < 0.0) { seg->dx = best_edge->seg.dy * dist; seg->dy = -best_edge->seg.dx * dist; } else { seg->dx = -best_edge->seg.dy * dist; seg->dy = best_edge->seg.dx * dist; } if (best_point != ob0->verts+vert0) { seg->x += seg->dx; seg->y += seg->dy; seg->dx = -seg->dx; seg->dy = -seg->dy; } } else { /* Just fill in the corridor between the two points */ seg->x = ob0->verts[vert0].seg.x; seg->y = ob0->verts[vert0].seg.y; seg->dx = ob1->verts[vert1].seg.x - seg->x; seg->dy = ob1->verts[vert1].seg.y - seg->y; *index0 = vert0; *index1 = vert1; } } /* Determine if the point is on the negative side of the linear obstacle */ int32 routemap_negative_side(x, y, verts, indx) float64 x, y; struct routemap_obstacle_vertex *verts; int32 indx; { float64 dx, dy; struct routemap_obstacle_vertex *v, *w; int32 outgoing_negative; int32 incoming_negative; v = verts+indx; dx = x - v->seg.x; dy = y - v->seg.y; outgoing_negative = (dx * v->seg.dy - dy * v->seg.dx < 0.0); if (!indx) return outgoing_negative; w = v-1; incoming_negative = (dx * (-w->seg.dy) - dy * (-w->seg.dx) < 0.0); /* Determine if the incoming-to-outgoing transition is a left * or right turn. */ if (v->seg.dx * (-w->seg.dy) - v->seg.dy * (-w->seg.dx) > 0.0) /* left */ return outgoing_negative && !incoming_negative; else /* right */ return !(incoming_negative && !outgoing_negative); } /* Find the corridor between two obstacles */ static void find_corridor(routemap, ob0, ob1, choose_only) ROUTEMAP_PTR routemap; ROUTEMAP_OBSTACLE *ob0; ROUTEMAP_OBSTACLE *ob1; int32 choose_only; { int32 i, j; float64 dx, dy, dist, p_i, p_j; float64 shortest, limit; float64 index_i, index_j; int32 nv0, nv1; ROUTEMAP_CORRIDOR corr; #define MAX_CLOSE_VERTS 1024 int32 n_close_verts, best, sh_i, sh_j; struct { /* Index of the two close vertices */ int32 i, j; /* Distance between them */ float64 dist; } close_verts[MAX_CLOSE_VERTS]; bzero(&corr, sizeof(corr)); /* Collect up all the pair of vertices which are close enough to * have a corridor between them. */ limit = routemap->max_corridor * routemap->max_corridor; n_close_verts = 0; if (ob0->linear || ob0->not_real) nv0 = ob0->n_verts; else nv0 = ob0->n_verts-1; if (ob1->linear || ob1->not_real) nv1 = ob1->n_verts; else nv1 = ob1->n_verts-1; for (i=0;iverts[j].seg.x - ob0->verts[i].seg.x; dy = ob1->verts[j].seg.y - ob0->verts[i].seg.y; dist = dx*dx + dy*dy; /* Obstacles which touch have no corridors between them */ if (dist < TOUCHING_SQ) return; if ((dist < limit) && (n_close_verts < MAX_CLOSE_VERTS)) { close_verts[n_close_verts].dist = dist; close_verts[n_close_verts].i = i; close_verts[n_close_verts].j = j; n_close_verts++; } } /* Are these obstacles too far apart? */ if (!n_close_verts) return; for(;;) { /* Find the closest pair */ shortest = limit; best = -1; for (i=0;ilinear) { float64 x, y; if (index_i == 0.0) ob0->start_open = 0; if (index_i == ob0->n_verts-1) ob0->end_open = 0; x = ob1->verts[sh_j].seg.x + ob1->verts[sh_j].seg.dx * p_j; y = ob1->verts[sh_j].seg.y + ob1->verts[sh_j].seg.dy * p_j; if ((index_i > 0.0) && (index_i < ob0->n_verts-1) && routemap_negative_side(x, y, ob0->verts, sh_i)) index_i = -index_i; } if (ob1->linear) { float64 x, y; if (index_j == 0.0) ob1->start_open = 0; if (index_j == ob1->n_verts-1) ob1->end_open = 0; x = ob0->verts[sh_i].seg.x + ob0->verts[sh_i].seg.dx * p_i; y = ob0->verts[sh_i].seg.y + ob0->verts[sh_i].seg.dy * p_i; if ((index_j > 0.0) && (index_j < ob1->n_verts-1) && routemap_negative_side(x, y, ob1->verts, sh_j)) index_j = -index_j; } create_corridor(routemap, ob0, index_i, ob1, index_j, &corr); } } /* Find the center and size of a quad node and its children */ static void define_quad(routemap, parent, index, depth, cx, cy, width) ROUTEMAP_PTR routemap; int32 parent; int32 *index; int32 depth; int32 cx, cy; int32 width; { int32 this = *index; int32 half; /* Define this node */ routemap->quad_info[this].parent = parent; routemap->quad_info[this].cx = cx; routemap->quad_info[this].cy = cy; (*index)++; /* Stop at the leaf nodes */ if (depth == ROUTEMAP_QUAD_DEPTH) return; /* Define the children */ width /= 2; half = width/2; routemap->quad_info[this].children[0] = *index; define_quad(routemap, this, index, depth+1, cx+half, cy+half, width); routemap->quad_info[this].children[1] = *index; define_quad(routemap, this, index, depth+1, cx-half, cy+half, width); routemap->quad_info[this].children[2] = *index; define_quad(routemap, this, index, depth+1, cx-half, cy-half, width); routemap->quad_info[this].children[3] = *index; define_quad(routemap, this, index, depth+1, cx+half, cy-half, width); /* Note how many descendants we have */ routemap->quad_info[this].num_descendants = *index - this - 1; } /* Compare an obstacle to all the obstacles in a quad node, and * all the obstacle in this quad node's children. */ static void compare_children(routemap, obst, depth, index) ROUTEMAP_PTR routemap; ROUTEMAP_OBSTACLE *obst; int32 depth; int32 index; { ROUTEMAP_OBSTACLE *i; int32 c; struct routemap_quad_info *info = routemap->quad_info+index; /* Compare to the obstacles in this quad */ for (i=info->obstacles;i;i=i->next_quad) { if (!(i->not_real && obst->not_real)) find_corridor(routemap, i, obst, -1); } /* Compare to all the children */ if (depth < ROUTEMAP_QUAD_DEPTH) for (c=0;c<4;c++) compare_children(routemap, obst, depth+1, info->children[c]); } /* Compare each obstacle in a quad to all the other obstacles in that * quad, as well as to all the obstacles in all the quad's child quads. */ static void compare_quad(routemap, depth, index) ROUTEMAP_PTR routemap; int32 depth; int32 index; { ROUTEMAP_OBSTACLE *i, *j; int32 c; struct routemap_quad_info *info = routemap->quad_info+index; /* Do the comparison in this quad */ for (i=info->obstacles;i;i=i->next_quad) { /* Compare to the other obstacles in this quad */ for (j=i->next_quad;j;j=j->next_quad) if (!(i->not_real && j->not_real)) find_corridor(routemap, i, j, -1); /* Compare to all the children */ if (depth < ROUTEMAP_QUAD_DEPTH) for (c=0;c<4;c++) compare_children(routemap, i, depth+1, info->children[c]); } /* Do a similar comparison in each child quad */ if (depth < ROUTEMAP_QUAD_DEPTH) for (c=0;c<4;c++) compare_quad(routemap, depth+1, info->children[c]); } /* Find all the corridors between all the obstacles */ static void find_corridors(routemap, max_x, max_y) ROUTEMAP_PTR routemap; int32 max_x, max_y; { ROUTEMAP_OBSTACLE *i; int32 index; /* Find the center point and size of each quad node */ index = 0; define_quad(routemap, -1, &index, 1, max_x/2, max_y/2, (max_x > max_y) ? max_x : max_y); /* Put each obstacle into the appropriate quad node */ for (i=routemap->obstacles;i;i=i->next) { index = routemap_select_quad(routemap, 1, 0, i->center_x - i->width, i->center_y - i->height, i->center_x + i->width, i->center_y + i->height); i->next_quad = routemap->quad_info[index].obstacles; routemap->quad_info[index].obstacles = i; } /* Compare each obstacle to neighboring obstacles */ /* ELIMINATING THIS LINE SINCE WE ARE NO LONGER USING CORRIDORS - ODED */ /* compare_quad(routemap, 1, 0); */ } /* Determine if two segments are similar. This means that they are * nearly parallel, and the distance between them is small. */ static int32 corridors_are_similar(c0, c1) ROUTEMAP_CORRIDOR *c0, *c1; { float64 cross, x, y; /* Use the cross product to determine if they are close to parallel */ cross = c0->seg.dx * c1->seg.dy - c0->seg.dy * c1->seg.dx; #define SIN_30 0.5 if (fabs(cross / (c0->length * c1->length)) > SIN_30) return FALSE; #define CLOSE_SEGS (20.0*20.0) /* See if either has a midpoint next to the other. */ x = c0->seg.x + 0.5 * c0->seg.dx; y = c0->seg.y + 0.5 * c0->seg.dy; if (next_to_edge(x, y, &c1->seg, FALSE) > 0.0) return (dist_to_edge(x, y, &c1->seg, c1->length * c1->length) < CLOSE_SEGS); x = c1->seg.x + 0.5 * c1->seg.dx; y = c1->seg.y + 0.5 * c1->seg.dy; if (next_to_edge(x, y, &c0->seg, FALSE) > 0.0) return (dist_to_edge(x, y, &c0->seg, c0->length * c0->length) < CLOSE_SEGS); return FALSE; } /* Eliminate corridors which violate triangulation or which are * too close to other corridors to be useful. */ static void filter_corridors(routemap) ROUTEMAP_PTR routemap; { ROUTEMAP_OBSTACLE *obst, *n; int32 i, j, k, not_real, nr; int32 nneigh; float64 len; ROUTEMAP_PSEG *seg; ROUTEMAP_CORRIDOR **corr; static int32 max_neighbors = 0; static ROUTEMAP_OBSTACLE **neighbors; /* Examine the corridors leading from each obstacle */ for (obst=routemap->obstacles;obst;obst=obst->next) { /* On rare occasion, corridors coming out of a single obstacle * might cross each other. In those cases, eliminate the longer * corridor. */ /* Check each corridor (in reverse order, in case we eliminate * some). We only eliminate corridors from the outer 'for' loop, * therefore the inner loop has to cover all the corridors, not * just the ones we haven't already tested. */ for (i=obst->n_corridors-1;i>=0;i--) { len = obst->corridors[i]->length; #define NOTREAL(c) ((c)->begin_obst->not_real || (c)->end_obst->not_real) not_real = NOTREAL(obst->corridors[i]); seg = &obst->corridors[i]->seg; for (j=0;jn_corridors;j++) if (i != j) { /* When two corridors cross, we keep the one connected to * real obstacles (in contrast to fake obstacles which * generate the corridors into freespace). * * If the corridors have the same reality level, then * we use distance as the selector (keep the shorter one). */ nr = NOTREAL(obst->corridors[j]); if (!not_real && nr) continue; if ((not_real == nr) && (obst->corridors[j]->length > len)) continue; if (segments_intersect_exclusive(&obst->corridors[j]->seg, seg) || corridors_are_similar(obst->corridors[j], obst->corridors[i])) { remove_corridor(routemap, obst->corridors[i]); break; } } } /* Make a list of neighboring obstacles */ nneigh = 0; for (i=0;in_corridors;i++) { n = obst->corridors[i]->begin_obst; if (n == obst) n = obst->corridors[i]->end_obst; for (j=0;jn_corridors-1;i>=0;i--) { len = obst->corridors[i]->length; /* Look for intersections with the corridors coming out * of neighboring obstacles. */ n = obst->corridors[i]->begin_obst; if (n == obst) n = obst->corridors[i]->end_obst; seg = &obst->corridors[i]->seg; for (j=0;jcorridors; for (k=0;kn_corridors;k++) { /* When two corridors cross, we keep the shortest * one. Therefore, only check for intersections * with shorter (or same length) corridors. */ if ((corr[k]->length <= len) && segments_intersect_exclusive(&corr[k]->seg, seg)) { remove_corridor(routemap, obst->corridors[i]); n = NULL; break; } } /* See if we already blew off this corridor */ if (!n) break; } } } } /* Invent a corridor coming out of a polygon vertex */ static void invent_from_vert(routemap, obst, v, fake_obst) ROUTEMAP_PTR routemap; ROUTEMAP_OBSTACLE *obst; int32 v; ROUTEMAP_OBSTACLE *fake_obst; { float64 mag, x, y, dx, dy; struct routemap_obstacle_vertex *edge_in, *edge_out; float64 len = routemap->max_corridor * 0.5; /* Get the incoming and outgoing edges to this vertex */ if (v) edge_in = &obst->verts[v-1]; else edge_in = &obst->verts[obst->n_verts-2]; edge_out = &obst->verts[v]; /* Make a reverse unit bisector */ mag = fsqrt(edge_in->len_sq); dx = edge_in->seg.dx / mag; dy = edge_in->seg.dy / mag; mag = fsqrt(edge_out->len_sq); dx -= edge_out->seg.dx / mag; dy -= edge_out->seg.dy / mag; mag = dx*dx + dy*dy; if (mag == 0.0) return; /* Shouldn't happen */ mag = 1.0 / fsqrt(mag); dx *= mag; dy *= mag; /* Make sure we got the right bisector */ if (dx * edge_out->seg.dy - dy * edge_out->seg.dx < 0.0) dx = -dx, dy = -dy; /* Find the point at the end of our bisecting vector */ x = obst->verts[v].seg.x + dx * len; y = obst->verts[v].seg.y + dy * len; /* Make a corridor */ fake_obst->verts->seg.x = x; fake_obst->verts->seg.y = y; find_corridor(routemap, obst, fake_obst, -1); } /* Find the vertex of the most acute convex corner between the * specified vertices of the polygon. */ static int32 find_acute_convex(obst, v0, v1, result) ROUTEMAP_OBSTACLE *obst; float64 v0, v1; int32 *result; { int32 first, last; int32 i, v, nv, found_one = FALSE; ROUTEMAP_PSEG *s_out, *s_in; ROUTEMAP_PSEG hold; float64 cross, dot, cos_angle, most_acute_angle; float64 len_out, len_in; first = v0 + 1.0; last = ceil(v1); nv = obst->n_verts - 1; /* The first & last are the same */ v = (first+nv-1) % nv; s_in = &obst->verts[v].seg; len_in = fsqrt(s_in->dx*s_in->dx + s_in->dy*s_in->dy); most_acute_angle = -2.0; for (i=first;iverts[v].seg; len_out = fsqrt(s_out->dx*s_out->dx + s_out->dy*s_out->dy); /* See if the corner is convex */ cross = -(s_out->dx * s_in->dy - s_in->dx * s_out->dy); if (cross > 0.0) { dot = -(s_out->dx * s_in->dx + s_out->dy * s_in->dy); cos_angle = dot / (len_in * len_out); if (cos_angle > most_acute_angle) { most_acute_angle = cos_angle; *result = v; found_one = TRUE; } } s_in = s_out; len_in = len_out; } return found_one; } /* Create corridors out the ends of a linear obstacle * points along the obstacle. */ static void invent_for_line(routemap, obst, fake_obst) ROUTEMAP_PTR routemap; ROUTEMAP_OBSTACLE *obst; ROUTEMAP_OBSTACLE *fake_obst; { ROUTEMAP_PSEG *seg, *prev_seg; int32 i, j, skip_pos, skip_neg, side; float64 len_quot, dx, dy, xoff, yoff, index; float64 prev_len, len, cos_theta; /* Attempt to generate corridors from the midpoints of short, * closed-off river segments. */ if (!obst->start_open && !obst->end_open && (obst->n_verts == 2)) { seg = &obst->verts[0].seg; len_quot = 0.5 * routemap->max_corridor / fsqrt(obst->verts[0].len_sq); xoff = seg->x + seg->dx * 0.5; yoff = seg->y + seg->dy * 0.5; dx = seg->dx * len_quot; dy = seg->dy * len_quot; /* Attempt to generate corridors in both directions */ fake_obst->verts->seg.x = xoff + dy; fake_obst->verts->seg.y = yoff - dx; find_corridor(routemap, obst, fake_obst, -1); fake_obst->verts->seg.x = xoff - dy; fake_obst->verts->seg.y = yoff + dx; find_corridor(routemap, obst, fake_obst, -1); /* There is nothing more we can do, so return */ return; } /* Generate corridors around the open ends of rivers */ if (obst->start_open) { seg = &obst->verts[0].seg; len_quot = 0.5 * routemap->max_corridor / fsqrt(obst->verts[0].len_sq); dx = seg->dx * len_quot; dy = seg->dy * len_quot; fake_obst->verts->seg.x = seg->x - dx; fake_obst->verts->seg.y = seg->y - dy; find_corridor(routemap, obst, fake_obst, -1); xoff = seg->x + dx * 0.001; yoff = seg->y + dy * 0.001; fake_obst->verts->seg.x = xoff + dy; fake_obst->verts->seg.y = yoff - dx; find_corridor(routemap, obst, fake_obst, -1); fake_obst->verts->seg.x = xoff - dy; fake_obst->verts->seg.y = yoff + dx; find_corridor(routemap, obst, fake_obst, -1); } if (obst->end_open) { seg = &obst->verts[obst->n_verts-1].seg; prev_seg = &obst->verts[obst->n_verts-2].seg; len_quot = 0.5 * routemap->max_corridor / fsqrt(obst->verts[obst->n_verts-2].len_sq); dx = prev_seg->dx * len_quot; dy = prev_seg->dy * len_quot; fake_obst->verts->seg.x = seg->x + dx; fake_obst->verts->seg.y = seg->y + dy; find_corridor(routemap, obst, fake_obst, -1); xoff = seg->x - dx * 0.001; yoff = seg->y - dy * 0.001; fake_obst->verts->seg.x = xoff + dy; fake_obst->verts->seg.y = yoff - dx; find_corridor(routemap, obst, fake_obst, -1); fake_obst->verts->seg.x = xoff - dy; fake_obst->verts->seg.y = yoff + dx; find_corridor(routemap, obst, fake_obst, -1); } /* Generate corridors on the convex side of each vertex */ prev_seg = &obst->verts[0].seg; prev_len = fsqrt(obst->verts[0].len_sq); for (i=1;in_verts-1;i++) { /* See if there is already a corridor on this vertex. */ skip_pos = skip_neg = FALSE; for (j=0;jn_corridors;j++) { index = routemap_corr_index(obst->corridors[j], obst); if (fabs(index) == i) { if (index > 0.0) skip_pos = TRUE; if (index < 0.0) skip_neg = TRUE; } } seg = &obst->verts[i].seg; len = fsqrt(obst->verts[i].len_sq); if (!skip_pos || !skip_neg) { len_quot = 1.0 / (prev_len * len); /* See if the angle between these segments is more than a * small number of degrees. */ cos_theta = -(seg->dx * prev_seg->dx + seg->dy * prev_seg->dy) * len_quot; if (cos_theta > -0.9397 /* < 160 degrees */) { /* Get a bisector */ dx = seg->dx / len - prev_seg->dx / prev_len; dy = seg->dy / len - prev_seg->dy / prev_len; len_quot = 0.51 * routemap->max_corridor / fsqrt(dx*dx+dy*dy); fake_obst->verts->seg.x = seg->x - dx * len_quot; fake_obst->verts->seg.y = seg->y - dy * len_quot; /* See if there is already a point on this side */ if (skip_pos || skip_neg) { if (routemap_negative_side(fake_obst->verts->seg.x, fake_obst->verts->seg.y, obst->verts, i)) side = -1; else side = 1; } else side = 0; if (!((skip_neg && (side == -1)) || (skip_pos && (side == 1)))) find_corridor(routemap, obst, fake_obst, i); } } prev_seg = seg; prev_len = len; } } /* Create corridors around a polygonal obstacle */ static void invent_for_polygon(routemap, obst, fake_obst) ROUTEMAP_PTR routemap; ROUTEMAP_OBSTACLE *obst; ROUTEMAP_OBSTACLE *fake_obst; { int32 i, k, v, n_corr; ROUTEMAP_CORRIDOR *c0, *c1; float64 v0, v1; /* Make sure there is at least on corridor on every obstacle */ if (!obst->n_corridors) invent_from_vert(routemap, obst, 0, fake_obst); /* Make sure we succeeded */ if (!obst->n_corridors) return; /* Look at each pair of successive corridors, and identify places * which need intermediaries. */ for (k=0;k<4;k++) { /* Sort the existing corridors of this obstacle */ sort_obstacle_corridors(obst); n_corr = obst->n_corridors; /* This will increase in the loop! */ for (i=0;icorridors[i]; c1 = obst->corridors[(i+1) % n_corr]; /* Find the indices of these two corridors. */ v0 = routemap_corr_index(c0, obst); v1 = routemap_corr_index(c1, obst); /* Choose a vertex to hang this invented corridor on. */ /* Make sure the second one is larger */ if ((i == n_corr-1) && (v1 <= v0)) v1 += (obst->n_verts-1); /* Don't bother if the vertices are close */ if (v1 - v0 < 2) continue; /* Choose an index in between. Look for the most acute * convex angle we can find. */ if (!find_acute_convex(obst, v0, v1, &v)) continue; /* Add a corridor comping out of the selected vertex */ invent_from_vert(routemap, obst, v, fake_obst); } /* If we didn't add any corridors, there is no sense looping */ if (obst->n_corridors == n_corr) break; } } /* Create corridors in free space around obstacles */ static void invent_corridors(routemap) ROUTEMAP_PTR routemap; { ROUTEMAP_OBSTACLE *obst; static ROUTEMAP_OBSTACLE fake_obst; static struct routemap_obstacle_vertex fake_vert; bzero(&fake_obst, sizeof(fake_obst)); bzero(&fake_vert, sizeof(fake_vert)); fake_obst.not_real = 1; fake_obst.n_verts = 1; fake_obst.verts = &fake_vert; for (obst=routemap->obstacles;obst;obst=obst->next) if (!obst->not_real) { if (obst->linear) invent_for_line(routemap, obst, &fake_obst); else invent_for_polygon(routemap, obst, &fake_obst); } } /* Helper function to sort an obstacle's corridors */ static ROUTEMAP_OBSTACLE *cmp_obst; static int cmp_corridors(c0, c1) ROUTEMAP_CORRIDOR **c0; ROUTEMAP_CORRIDOR **c1; { float64 index0, index1; int32 point_index; float64 dx0, dy0, dx1, dy1; float64 ref_dx, ref_dy, cross0, cross1, dot0, dot1; /* Get the index number of this end of each corridor */ index0 = routemap_corr_index(*c0, cmp_obst); index1 = routemap_corr_index(*c1, cmp_obst); if (index0 > index1) return 1; if (index0 < index1) return -1; /* When the corridors emminate from the same point, order them * counter clockwise. We have to be extremely careful here, because * some implementations of qsort go nuts if there isn't a largest * and a smallest value. Therefore, we will partion the corridors * on a point against a reference segment on the obstacle. Imagine * that the reference segment is 12 O'clock. We are going to declare * that 11:00 is less than 1:00. */ /* First get the vectors pointing along each corridor */ dx0 = (*c0)->seg.dx; dy0 = (*c0)->seg.dy; if ((*c0)->begin_obst != cmp_obst) dx0 = -dx0, dy0 = -dy0; dx1 = (*c1)->seg.dx; dy1 = (*c1)->seg.dy; if ((*c1)->begin_obst != cmp_obst) dx1 = -dx1, dy1 = -dy1; /* Get a reference segment of the polygon or line which goes out from * this point. */ point_index = index0; if (point_index < cmp_obst->n_verts-1) { ref_dx = cmp_obst->verts[point_index].seg.dx; ref_dy = cmp_obst->verts[point_index].seg.dy; } else { ref_dx = -cmp_obst->verts[point_index-1].seg.dx; ref_dy = -cmp_obst->verts[point_index-1].seg.dy; } /* Determine which side each corridor is on, relative to this reference * segment. */ cross0 = dx0 * ref_dy - dy0 * ref_dx; cross1 = dx1 * ref_dy - dy1 * ref_dx; /* If a corridor is exactly aligned with the reference segment, * (cross product == 0), then we need to make some determinations * based on dot products. */ if ((cross0 == 0.0) || (cross1 == 0.0)) { dot0 = dx0 * ref_dx + dy0 * ref_dy; dot1 = dx1 * ref_dx + dy1 * ref_dy; /* Are both aligned with the reference edge? */ if ((cross0 == 0.0) && (dot0 > 0.0) && (cross1 == 0.0) && (dot1 > 0.0)) return 0; /* If one is aligned with the reference edge, then that * corridor is larger. */ if ((cross0 == 0.0) && (dot0 > 0.0)) return 1; if ((cross1 == 0.0) && (dot1 > 0.0)) return -1; /* At this point, any zero cross products must be the result * of corridors going the opposite direction from the reference * edge. Those can be compared to any other vector with a * cross product and get the right answer... */ } /* Corridors on the right of the reference segment are more counter- * clockwise that those on the left. */ if ((cross0 > 0.0) && (cross1 < 0.0)) return 1; if ((cross1 > 0.0) && (cross0 < 0.0)) return -1; /* Corridors on the same side of the reference segment can be compared * with a simple cross product. */ cross0 = dx1 * dy0 - dx0 * dy1; if (cross0 > 0.0) return 1; if (cross0 < 0.0) return -1; return 0; } /* Add a corridor to another's list of connections */ static void add_connection(corr0, corr1) ROUTEMAP_CORRIDOR *corr0; ROUTEMAP_CORRIDOR *corr1; { int32 i; /* Don't repeat a connection */ for (i=0;in_connect;i++) if (corr0->connect[i] == corr1) return; corr0->connect[corr0->n_connect++] = corr1; } /* Connect two corridors to each other */ static int32 connect_corridors(obst, c0, c1) ROUTEMAP_OBSTACLE *obst; int32 c0, c1; { ROUTEMAP_CORRIDOR *corr0 = obst->corridors[c0]; ROUTEMAP_CORRIDOR *corr1 = obst->corridors[c1]; float64 index0, index1, lst; /* If the obstacle is linear, ensure that the corridors are * allowed to be connected. */ if (obst->linear) { index0 = routemap_corr_index(corr0, obst); index1 = routemap_corr_index(corr1, obst); /* The rules for getting around a river are a little complicated. * Here are the cases which are allowed: * * Negative -> Negative * Negative -> Zero * Zero -> Zero (c1 > c0) * Zero -> Positive * Positive -> Positive (c1 > c0) * Positive -> Last (c1 > c0) * Last -> Last (c1 > c0) * Last -> Negative */ lst = obst->n_verts-1; if (!(((index0 < 0.0) && (index1 < 0.0)) || ((index0 < 0.0) && (index1 == 0.0)) || ((index0 == 0.0) && (index1 == 0.0) && (c1 > c0)) || ((index0 == 0.0) && (index1 > 0.0)) || ((index0 > 0.0) && (index1 > 0.0) && (c1 > c0)) || ((index0 > 0.0) && (index1 == lst) && (c1 > c0)) || ((index0 == lst) && (index1 == lst) && (c1 > c0)) || ((index0 == lst) && (index1 < 0.0)))) return FALSE; } add_connection(corr0, corr1); add_connection(corr1, corr0); return TRUE; } /* Sort the corridors around an obstacle */ static void sort_obstacle_corridors(obst) ROUTEMAP_OBSTACLE *obst; { if (obst->n_corridors > 1) { cmp_obst = obst; qsort(obst->corridors, obst->n_corridors, sizeof(ROUTEMAP_CORRIDOR *), cmp_corridors); } } /* Find the closest pair of corridors attached to a pair of obstacles, * and connect them. */ static int32 connect_obstacles(obst0, obst1, positive0, positive1) ROUTEMAP_OBSTACLE *obst0; ROUTEMAP_OBSTACLE *obst1; int32 positive0; int32 positive1; { int32 i, j; int32 best_i, best_j; ROUTEMAP_CORRIDOR *c0, *c1; float64 index0, index1, dist, closest; if (!obst0 || !obst1 || !obst0->n_corridors || !obst1->n_corridors) return FALSE; /* Find the closest pair on the right sides */ best_i = best_j = -1; closest = 1.0E+99; for (i=0;in_corridors;i++) { c0 = obst0->corridors[i]; index0 = routemap_corr_index(c0, obst0); /* Make sure this corridor is on the allowed side */ if ((index0 != 0.0) && (index0 != obst0->n_verts-1) && ((index0 > 0.0) != positive0)) continue; for (j=0;jn_corridors;j++) { c1 = obst1->corridors[j]; index1 = routemap_corr_index(c1, obst1); /* Make sure this corridor is on the allowed side */ if ((index1 != 0.0) && (index1 != obst1->n_verts-1) && ((index1 > 0.0) != positive1)) continue; /* Get the corridor-corridor distance */ dist = routemap_seg_seg_dist_sq(&c0->seg, &c1->seg); if (dist < closest) { best_i = i; best_j = j; closest = dist; } } } if (best_i == -1) return FALSE; /* Connect them */ c0 = obst0->corridors[best_i]; c1 = obst1->corridors[best_j]; add_connection(c0, c1); add_connection(c1, c0); return TRUE; } /* Sort corridors into a network */ static void sort_corridors(routemap) ROUTEMAP_PTR routemap; { ROUTEMAP_OBSTACLE *obst; struct routemap_bypass *bypass; int32 i, first_connected; /* Sort corridors so that the list for each obstacle is in order */ for (obst=routemap->obstacles;obst;obst=obst->next) sort_obstacle_corridors(obst); /* Connect each corridor to its neighbors */ for (obst=routemap->obstacles;obst;obst=obst->next) if (obst->n_corridors > 1) { first_connected = connect_corridors(obst, 0, 1); for (i=1;in_corridors-1;i++) connect_corridors(obst, i, i+1); if ((obst->n_corridors > 2) || !first_connected) connect_corridors(obst, obst->n_corridors-1, 0); } /* Connect each obstacle to its bypasses (if an obstacle has no * corridors, just connect its bypasses to each other) */ for (obst=routemap->obstacles;obst;obst=obst->next) if (bypass = obst->bypass) { /* Note that I'm using bitwise |, to avoid shortcutting the * second clause of the 'if'. */ if (!connect_obstacles(obst, bypass->positive_end, TRUE, !bypass->positive_end_flip) | !connect_obstacles(bypass->positive_start, obst, !bypass->positive_start_flip, TRUE)) connect_obstacles(bypass->positive_start, bypass->positive_end, !bypass->positive_start_flip, !bypass->positive_end_flip); if (!connect_obstacles(obst, bypass->negative_end, FALSE, bypass->negative_end_flip) | !connect_obstacles(bypass->negative_start, obst, bypass->negative_start_flip, FALSE)) connect_obstacles(bypass->negative_start, bypass->negative_end, bypass->negative_start_flip, bypass->negative_end_flip); } } /* Add a corridor and its obstacles to a clump */ static void clump_corridor(clump, corr) ROUTEMAP_CLUMP *clump; ROUTEMAP_CORRIDOR *corr; { corr->clump = clump; /* Add the corridor to the clump's list */ if (!clump->n_corridors) clump->corridors = (ROUTEMAP_CORRIDOR **) STDALLOC(sizeof(ROUTEMAP_CORRIDOR *)); else clump->corridors = (ROUTEMAP_CORRIDOR **) STDREALLOC(clump->corridors, (clump->n_corridors+1) * sizeof(ROUTEMAP_CORRIDOR *)); clump->corridors[clump->n_corridors++] = corr; /* Add the corridor's obstacles to the clump (pre-test * to avoid lots of useless invocations). */ if (!corr->begin_obst->clump) clump_obstacle(clump, corr->begin_obst); if (!corr->end_obst->clump) clump_obstacle(clump, corr->end_obst); } /* Add an obstacle and its neighbors to a clump */ static void clump_obstacle(clump, obst) ROUTEMAP_CLUMP *clump; ROUTEMAP_OBSTACLE *obst; { ROUTEMAP_CORRIDOR *corr; int32 i, j; /* If we've already got it, stop recursing */ if (obst->clump) return; obst->clump = clump; /* Add the obstacle to the clump's list */ if (!clump->n_obstacles) clump->obstacles = (ROUTEMAP_OBSTACLE **) STDALLOC(sizeof(ROUTEMAP_OBSTACLE *)); else clump->obstacles = (ROUTEMAP_OBSTACLE **) STDREALLOC(clump->obstacles, (clump->n_obstacles+1) * sizeof(ROUTEMAP_OBSTACLE *)); clump->obstacles[clump->n_obstacles++] = obst; /* Add the corridors and their connections to the clump (this will * add more obstacles through mutual recursion). Pre-test to avoid * lots of useless invocations. */ for (i=0;in_corridors;i++) { corr = obst->corridors[i]; if (!corr->clump) clump_corridor(clump, corr); for (j=0;jn_connect;j++) if (!corr->connect[j]->clump) clump_corridor(clump, corr->connect[j]); } } /* Create clumps of obstacles */ static void create_clumps(routemap) ROUTEMAP_PTR routemap; { ROUTEMAP_OBSTACLE *obst; ROUTEMAP_CLUMP *clump; /* Create a clump for each obstacle which needs one */ for (obst=routemap->obstacles;obst;obst=obst->next) if (!obst->clump) { /* Make a clump */ clump = (ROUTEMAP_CLUMP *)STDALLOC(sizeof(ROUTEMAP_CLUMP)); bzero(clump, sizeof(ROUTEMAP_CLUMP)); clump->next = routemap->clumps; routemap->clumps = clump; /* Add this obstacle and all its neighbors */ clump_obstacle(clump, obst); } } /* Identify shortcuts across the corridor network */ static void identify_shortcuts(routemap) ROUTEMAP_PTR routemap; { ROUTEMAP_CORRIDOR *corr, *next; ROUTEMAP_CLUMP *clump; ROUTEMAP_PSEG test_seg; int32 i, j, k, c, stop; for (corr=routemap->corridors;corr;corr=corr->next) { /* Ignore corridors which are already fully connected */ if (corr->n_connect == ROUTEMAP_MAX_CONNECT) continue; clump = corr->clump; /* We will be tesing segments from the middle of this corridor */ test_seg.x = corr->seg.x + 0.5 * corr->seg.dx; test_seg.y = corr->seg.y + 0.5 * corr->seg.dy; /* Look at each corridor which can be reached in two steps */ stop = 0; for (i=0;(in_connect)&&!stop;i++) for (j=0;jconnect[i]->n_connect;j++) { next = corr->connect[i]->connect[j]; if (next->n_connect == ROUTEMAP_MAX_CONNECT) continue; /* Ignore corridors touching the same obstacle as the one being * tested. */ if ((!corr->begin_obst->not_real && ((corr->begin_obst == next->begin_obst) || (corr->begin_obst == next->end_obst))) || (!corr->end_obst->not_real && ((corr->end_obst == next->begin_obst) || (corr->end_obst == next->end_obst)))) continue; /* Ignore corridors which we already know we are connected * with. */ for (k=0;kn_connect;k++) if (corr->connect[k] == next) break; if (k < corr->n_connect) continue; /* Make the test segment go to the middle of the test * corridor. */ test_seg.dx = next->seg.x + 0.5 * next->seg.dx - test_seg.x; test_seg.dy = next->seg.y + 0.5 * next->seg.dy - test_seg.y; /* See if this segment intersects any corridors */ if (segment_intersects_obstacle(routemap, &test_seg, NULL, NULL, TRUE)) continue; corr->connect[corr->n_connect++] = next; next->connect[next->n_connect++] = corr; if (corr->n_connect == ROUTEMAP_MAX_CONNECT) { stop = 1; break; } } } } /* Find the corridors near a point */ ROUTEMAP_CORRIDOR_LIST *routemap_find_corridors(routemap, x, y, rad) ROUTEMAP_PTR routemap; float64 x, y, rad; { ROUTEMAP_CORRIDOR *i; ROUTEMAP_CORRIDOR_LIST *result; int32 count; #define MAX_FIND 1024 ROUTEMAP_CORRIDOR *save_list[MAX_FIND]; rad *= rad; count = 0; for (i=routemap->corridors;i;i=i->next) { if ((next_to_edge(x, y, &i->seg, FALSE) > 0.0) && (dist_to_edge(x, y, &i->seg, i->length*i->length) < rad)) { save_list[count++] = i; if (count == MAX_FIND) break; } } if (!count) return NULL; result = (ROUTEMAP_CORRIDOR_LIST *) STDALLOC(sizeof(ROUTEMAP_CORRIDOR_LIST)+ (count-1) * sizeof(ROUTEMAP_CORRIDOR *)); result->num_corridors = count; bcopy(save_list, result->corridors, count * sizeof(ROUTEMAP_CORRIDOR *)); return result; } /* Determine if two linear obstacles touch, and if so, return the angle * from the first to the second (increasing clockwise). */ static int32 obstacles_touch(seg, second, angle, at_start) ROUTEMAP_PSEG *seg; ROUTEMAP_OBSTACLE *second; float64 *angle; int32 *at_start; { float64 dx, dy; int32 nv; dx = seg->x - second->verts[0].seg.x; dy = seg->y - second->verts[0].seg.y; if (fabs(dx)+fabs(dy) < 1.0) { dx = second->verts[0].seg.dx; dy = second->verts[0].seg.dy; *at_start = TRUE; } else { nv = second->n_verts-1; if (!nv) return FALSE; dx = seg->x - second->verts[nv].seg.x; dy = seg->y - second->verts[nv].seg.y; if (fabs(dx)+fabs(dy) < 1.0) { dx = -second->verts[nv-1].seg.dx; dy = -second->verts[nv-1].seg.dy; *at_start = FALSE; } else return FALSE; } *angle = NS_ATAN2(seg->dy, seg->dx) - NS_ATAN2(dy, dx); if (*angle < 0.0) *angle += TWO_PI; return TRUE; } /* Find obstacles which can be used to bypass linear obstacles which * are blocked on both ends. */ static void find_bypasses(routemap) ROUTEMAP_PTR routemap; { ROUTEMAP_OBSTACLE *obst, *i; float64 angle; float64 pos_start, neg_start, pos_end, neg_end; int32 nv, at_start; struct routemap_bypass bypass; ROUTEMAP_PSEG forward_seg, backward_seg; /* Search for troubled obstacles */ for (obst=routemap->obstacles;obst;obst=obst->next) { if (!obst->linear || (obst->start_open && obst->end_open)) continue; pos_start = neg_end = 10.0; neg_start = pos_end = -10.0; forward_seg = obst->verts[0].seg; nv = obst->n_verts - 1; if (!nv) continue; backward_seg.x = obst->verts[nv].seg.x; backward_seg.y = obst->verts[nv].seg.y; backward_seg.dx = -obst->verts[nv-1].seg.dx; backward_seg.dy = -obst->verts[nv-1].seg.dy; bypass.positive_start = bypass.positive_end = bypass.negative_start = bypass.negative_end = NULL; /* Find all the other obstacles which touch this intersection. */ for (i=routemap->obstacles;i;i=i->next) if ((i != obst) && i->linear) { if (!obst->start_open && obstacles_touch(&forward_seg, i, &angle, &at_start)) { if (angle < pos_start) { pos_start = angle; bypass.positive_start = i; bypass.positive_start_flip = at_start; } if (angle > neg_start) { neg_start = angle; bypass.negative_start = i; bypass.negative_start_flip = at_start; } } if (!obst->end_open && obstacles_touch(&backward_seg, i, &angle, &at_start)) { if (angle > pos_end) { pos_end = angle; bypass.positive_end = i; bypass.positive_end_flip = !at_start; } if (angle < neg_end) { neg_end = angle; bypass.negative_end = i; bypass.negative_end_flip = !at_start; } } } /* If both of the ends turned out untouched, don't bother * with bypasses. */ if (!bypass.positive_start && !bypass.positive_end) continue; obst->bypass = (struct routemap_bypass *) STDALLOC(sizeof(struct routemap_bypass)); *obst->bypass = bypass; } } /* Determine if the passed corridor intersects the boundary. */ static int32 corridor_intersects_boundary(bound, corr) ROUTEMAP_BOUNDARY_PTR bound; ROUTEMAP_CORRIDOR *corr; { int32 i; for (i=0;in_verts;i++) if (segments_intersect_inclusive(&corr->seg, &bound->verts[i].seg)) return TRUE; return FALSE; } /* Determine if the passed corridor is on the wrong side of a boundary. */ int32 routemap_wrong_side(bound, x, y, side) ROUTEMAP_BOUNDARY_PTR bound; float64 x, y; int32 side; { int32 i, best_i; float64 dx, dy, dist, best_dist; /* Determine the point on the boundary which is closest to the * corridor midpoint. */ best_dist = 1.0E+99; best_i = 0; for (i=0;in_verts;i++) { dx = bound->verts[i].seg.x - x; dy = bound->verts[i].seg.y - y; dist = dx*dx + dy*dy; if (dist < best_dist) { best_dist = dist; best_i = i; } } /* If the point isn't an end point, we can use the negative_side * test (this test doesn't work on end points). */ if ((best_i > 0) && (best_i < bound->n_verts-1)) { return (routemap_negative_side(x, y, bound->verts, best_i) != (side == -1)); } /* If the closest point is an end point, see if it is beyond a * perpendicular to the segment thru that point. This is to * prevent us from going around our boundaries. */ if (best_i) best_i -= 1; dx = x - bound->verts[0].seg.x; dy = y - bound->verts[0].seg.y; if ((dx*bound->verts[0].seg.dx + dy*bound->verts[0].seg.dy) < 0.0) return FALSE; /* Check the cross product */ return (((dx * bound->verts[best_i].seg.dy - dy * bound->verts[best_i].seg.dx) < 0.0) != (side == -1)); } /* Determine if the passed corridor is outside the search boundaries. */ int32 routemap_outside_boundaries(routemap, corridor) ROUTEMAP_PTR routemap; ROUTEMAP_CORRIDOR *corridor; { float64 x, y; x = corridor->seg.x + 0.5 * corridor->seg.dx; y = corridor->seg.y + 0.5 * corridor->seg.dy; if (routemap->left_bound && !corridor_intersects_boundary(routemap->left_bound, corridor) && routemap_wrong_side(routemap->left_bound, x, y, 1)) return TRUE; if (routemap->right_bound && !corridor_intersects_boundary(routemap->right_bound, corridor) && routemap_wrong_side(routemap->right_bound, x, y, -1)) return TRUE; return FALSE; }