From d2ab3fa9e537292a9e6887e2050b87473f24cb2b Mon Sep 17 00:00:00 2001 From: Hugo Clement Date: Wed, 3 Apr 2002 09:47:54 +0000 Subject: [PATCH] Now using the STL. --- alliance/src/ocr/src/rout/ocrAstar.cpp | 870 +++++++++++++++++++++++++ 1 file changed, 870 insertions(+) create mode 100644 alliance/src/ocr/src/rout/ocrAstar.cpp diff --git a/alliance/src/ocr/src/rout/ocrAstar.cpp b/alliance/src/ocr/src/rout/ocrAstar.cpp new file mode 100644 index 00000000..ea360ae8 --- /dev/null +++ b/alliance/src/ocr/src/rout/ocrAstar.cpp @@ -0,0 +1,870 @@ +extern "C" { +#include +#include +#include "mut.h" +#include "mlo.h" +#include "mlu.h" +#include "mph.h" +#include "mpu.h" +#include "ocr.h" +#include "ocrUtil.h" +#include "ocrWRoutingDataBase.h" +#include "ocrWindow.h" +#include "mbk_tree.h" +#include "ocrWRoutingUtil.h" +#include "ocrWRouting.h" +#include "display.h" +#include "ocrAstar.h" +} + +#include +#include + +using namespace std; + +extern ocrOption *g_pOption; +#define LEVEL (g_pOption->LEVEL) + +/*********************************************************************/ +struct is_a_better_seg { + bool operator() (ocrWSegment *s1, ocrWSegment *s2) { + + return ( + (s1->HCOST < s2->HCOST) + || + ( + (s1->HCOST == s2->HCOST) + && + ( + (s1->COST < s2->COST) + || + ( + (s1->COST == s2->COST) + && + (s1 < s2) + ) + ) + ) + ) + ; + } +}; + +typedef set ocrSegmentKeySet; +typedef set ocrSegmentAdrSet; +/*********************************************************************/ +#define TAG_UNDEF 0 +#define TAG_KEEP 1 +#define TAG_CLEAN 2 +#define TAG_FREED 4 +#define TAG_TERRA 8 +#define TAG_VISITED 16 + +/*********************************************************************/ + +/** + * Variables globales + **/ +ocrWRoutingGrid *grid = NULL; +ocrRoutingParameters *param = NULL; + +ocrNaturalInt xs = 0, ys = 0, zs = 0; +ocrNaturalInt CUR_SIG_INDEX = 0; + +ocrWSegment *ze_best = NULL; + +//ocrNaturalInt (*kost)(ocrWSegment *, ocrWSegment *); + +/*********************************************************************/ + +/** + * + * Local data structures + * + **/ + +typedef struct s_node_cost { + ocrNaturalInt H; + ocrNaturalInt C; + ocrNaturalInt HC; +} node_cost; + + +/*********************************************************************/ + +/** + * Functions + */ + +/* Cost function (Manhattan distance) */ +ocrNaturalInt eval (ocrWSegment *segment_source, ocrWSegment *segment_dest) { + + ocrNaturalInt x1s, x2s, y1s, y2s, x1d, x2d, y1d, y2d, dx, dy, dz; + + x1s = getWSegXCoord (param, segment_source, segment_source->P_MIN); + y1s = getWSegYCoord (param, segment_source, segment_source->P_MIN); + x2s = getWSegXCoord (param, segment_source, segment_source->P_MAX); + y2s = getWSegYCoord (param, segment_source, segment_source->P_MAX); + + x1d = getWSegXCoord (param, segment_dest, segment_dest->P_MIN); + y1d = getWSegYCoord (param, segment_dest, segment_dest->P_MIN); + x2d = getWSegXCoord (param, segment_dest, segment_dest->P_MAX); + y2d = getWSegYCoord (param, segment_dest, segment_dest->P_MAX); + + dx = ( (x2s < x1d)? (x1d - x2s) : ( (x1s > x2d) ? (x1s - x2d) : 0 ) ); + dy = ( (y2s < y1d)? (y1d - y2s) : ( (y1s > y2d) ? (y1s - y2d) : 0 ) ); + + dz = ( + (segment_dest->LAYER > segment_source->LAYER) ? + segment_dest->LAYER - segment_source->LAYER : + segment_source->LAYER - segment_dest->LAYER + ) + ; + + return (dx + dy) + dz /* param->VIA_COST*/; +} + +/* Distance between segment and nearest segment of an equi */ +ocrNaturalInt eval_equi (ocrWSegment *segment_source, ocrWSegment *equi) { + ocrNaturalInt res = OCRNATURALINT_MAX; + ocrNaturalInt aux; + + /*return 0;*/ + while (equi) { + aux = eval (segment_source, equi); + if (aux < res) { + res = aux; + } + equi = equi->NEXT; + } + return res; +} + +/* kost of the path from segment_source to segment_dest */ +ocrNaturalInt kost (ocrWSegment *segment_source, ocrWSegment *segment_dest) { + + ocrWSegment *ps; + ocrNaturalInt dest_offset, p; + ocrNaturalInt res; + + ps = segment_source->ROOT; + dest_offset = segment_dest->OFFSET; + + if ( + segment_source->LAYER + == + segment_dest->LAYER + ) + { + return 0; + } else if (ps) { + res = ( + (ps->OFFSET > dest_offset) ? + (ps->OFFSET - dest_offset) : + (dest_offset - ps->OFFSET) + ) + ; + res += ( + (segment_dest->LAYER > segment_source->LAYER) ? + segment_dest->LAYER - segment_source->LAYER : + segment_source->LAYER - segment_dest->LAYER + ) + /* param->VIA_COST*/ + ; + + return res; + } else { +#if 0 + p = (getWSegDirection (param, segment_source) == ocrHorizontal) ? + xs : ys; +#endif + /* perf optim... */ + p = ( !(segment_source->LAYER & ((ocrNaturalInt) 1)) ) ? + xs : ys + ; + + return + ( + (p > dest_offset) ? + (p - dest_offset) : + (dest_offset - p) + ) + + + ( + (zs > segment_source->LAYER) ? + (zs - segment_source->LAYER) : + (segment_source->LAYER - zs) + ) + /* + param->VIA_COST*/ + ; + } +} + +#if 0 +bool is_a_better_seg (ocrWSegment *s1, ocrWSegment *s2) { + + return ( + (s1->HCOST < s2->HCOST) + || + ( + (s1->HCOST == s2->HCOST) + && + (s1->COST < s2->COST) + ) + ) + ; +} +#endif + +void tag_keep (ocrWSegment *path) { + + while (path) { + path->TAG = TAG_KEEP; + path = path->ROOT; + } + return; +} + +/** + * stack + **/ + + +/** + * clean_mem + **/ +void clean_tag () { + ocrWSegment *seg; + int i, j, k; + + for (k = 0 ; k < grid->NB_OF_LAYERS ; k += 2) + for (j = 0 ; j < grid->SIZE_V ; j++) + for (i = 0 ; i < grid->SIZE_H ; i++) { + seg = getWSegment (grid, i, j, k); + seg->TAG = TAG_UNDEF; + i = seg->P_MAX; +#if 0 + if ( + (seg->SIGNAL_INDEX != WSEGMENT_FREE) + && + (seg->SIGNAL_INDEX != WSEGMENT_OBSTACLE) + ) + { + if (seg->AUX) { + display (LEVEL, DEBUG, "AUX not NULL for free or obstacle segment\n"); + seg->AUX = NULL; + } + } +#endif + } + + for (k = 1 ; k < grid->NB_OF_LAYERS ; k += 2) + for (i = 0 ; i < grid->SIZE_H ; i++) + for (j = 0 ; j < grid->SIZE_V ; j++) { + seg = getWSegment (grid, i, j, k); + seg->TAG = TAG_UNDEF; + j = seg->P_MAX; +#if 0 + if ( + (seg->SIGNAL_INDEX != WSEGMENT_FREE) + && + (seg->SIGNAL_INDEX != WSEGMENT_OBSTACLE) + ) + { + if (seg->AUX) { + display (LEVEL, DEBUG, "AUX not NULL for free or obstacle segment\n"); + seg->AUX = NULL; + } + } +#endif + } + + return; +} + +void clean_tag_list (ocrSegmentAdrSet &set) { + + for (ocrSegmentAdrSet::iterator sit = set.begin(); + sit != set.end(); + sit++) + { + (*sit)->TAG = TAG_UNDEF; + } + + set.clear(); + return; +} + +void clean_tag_set (ocrSegmentKeySet &set) { + + for (ocrSegmentKeySet::iterator sit = set.begin(); + sit != set.end(); + sit++) + { + (*sit)->TAG = TAG_UNDEF; + } + + set.clear(); + return; +} + +/** + * insert elem keeping the list sorted by increasing cost+h + **/ + + +#define XPLORE \ +{\ + if (isObstructed (seg))\ + if (seg->SIGNAL_INDEX != CUR_SIG_INDEX)\ + continue;\ +\ + cost = node->COST + kost (node, seg); /* XXX cout du chemin parcouru ??? */\ +\ + switch (seg->TAG) {\ + case TAG_UNDEF: /* not yet seen segment */\ + seg->ROOT = node;\ + seg->COST = cost;\ + seg->H = eval_equi (seg, segment_target);\ + seg->HCOST = seg->H + cost;\ + seg->TAG = TAG_TERRA;\ + terra_incognita.insert (seg);\ + break;\ + case TAG_TERRA:\ + if (cost < seg->COST) {\ + terra_incognita.erase (seg);\ + seg->COST = cost;\ + seg->HCOST = seg->H + cost;\ + seg->ROOT = node;\ + terra_incognita.insert (seg);\ + } else {\ + /* rien */\ + }\ + break;\ + case TAG_VISITED:\ + if (cost < seg->COST) {\ + visited.erase (seg);\ + seg->COST = cost;\ + seg->HCOST = seg->H + cost;\ + seg->ROOT = node;\ + seg->TAG = TAG_TERRA;\ + terra_incognita.insert (seg);\ + } else {\ + /* rien */\ + }\ + break;\ + default:\ + display (LEVEL, ERROR, "*** unexpected TAG !!!\n");\ + abort ();\ + }\ +} + +#if 0 +ocrWSegment *explore (ocrWSegment *segment_source, ocrWSegment *segment_target) { + + ocrWSegment *visited = NULL; + ocrWSegment *terra_incognita = NULL; + ocrNaturalInt p, cost; + int i; + + assert (segment_source != segment_target); + + display (LEVEL, DEBUG, "d starting exploration\n"); + + terra_incognita = segment_source; + terra_incognita->COST = 0; + terra_incognita->H = eval (segment_source, segment_target); + terra_incognita->HCOST = terra_incognita->H; + terra_incognita->TAG = TAG_TERRA; + terra_incognita->ROOT = NULL; + terra_incognita->AUX = NULL; + + while (terra_incognita) { + ocrWSegment *node = NULL; + ocrWSegment *seg = NULL; + + node = pop (&terra_incognita); + + if (node == segment_target) { + /* bingo ! */ + clean_tag_list (visited); + clean_tag_list (terra_incognita); + node->TAG = TAG_UNDEF; + return node; + } + + node->TAG = TAG_VISITED; + push (&visited, node); + + /* expansion */ + for (i = -1 ; i <= 1 ; i += 2) { + switch (i) { + + case -1: if ( ((node->LAYER) ) == 0 ) + continue; + break; + case 1: if ( ((node->LAYER) + 1) >= (grid->NB_OF_LAYERS) ) + continue; + break; + + default: + /*display (LEVEL, ERROR, __FILE__ ":" __LINE__ "exand_eval Oops\n");*/ + abort (); + } + for (p = node->P_MIN ; p <= node->P_MAX ; p++) { + + /*display (LEVEL, DEBUG, "d expanding %ld\n", i);*/ + seg = getWSegment(grid, + getWSegXCoord (param, node, p), + getWSegYCoord (param, node, p), + node->LAYER + i + ); + +#ifdef DEBUG + if (!seg) { + display (LEVEL, DEBUG, "oddity: null seg at (%ld, %ld, %ld)\n", getWSegXCoord(param, node, p), getWSegXCoord(param, node, p), node->LAYER + i); + continue; + } +#endif + + if (isObstructed (seg)) + continue; + + cost = node->COST + kost (node, seg); /* XXX cout du chemin parcouru ??? */ + + switch (seg->TAG) { + case TAG_UNDEF: /* not yet seen segment */ + seg->ROOT = node; + seg->COST = cost; + seg->H = eval (seg, segment_target); + seg->HCOST = seg->H + cost; + seg->TAG = TAG_TERRA; + terra_incognita = insert (terra_incognita, seg); + break; + case TAG_TERRA: + if (cost < seg->COST) { + seg->COST = cost; + seg->HCOST = seg->H + cost; + seg->ROOT = node; + terra_incognita = _remove (terra_incognita, seg); + terra_incognita = insert (terra_incognita, seg); + } else { + /* rien */ + } + break; + case TAG_VISITED: + if (cost < seg->COST) { + seg->COST = cost; + seg->HCOST = seg->H + cost; + seg->ROOT = node; + visited = _remove (visited, seg); + seg->TAG = TAG_TERRA; + terra_incognita = insert (terra_incognita, seg); + } else { + /* rien */ + } + break; + default: + display (LEVEL, ERROR, "*** unexpected TAG !!!\n"); + abort (); + } + } + } /* end for i */ + } /* while */ + + /* Failed */ + clean_tag (); + + /*isplay (LEVEL, DEBUG, "d exploration failed\n");*/ + + + return NULL; +} +#endif + +#if 0 +void addseglist (ocrSignal *s, ocrWSegment *l2) { + ocrWSegment *l1; + + if (!(s->SEGMENT)) + s->SEGMENT = l2; + else { + l1 = s->SEGMENT; + while (l1->NEXT) + l1 = l1->NEXT; + l1->NEXT = l2; + } + return; +} +#endif + +ocrWSegment *explore_equi (ocrWSegment *segment_source, ocrWSegment *segment_target) { + + ocrNaturalInt cost; + + ocrSegmentKeySet terra_incognita; + ocrSegmentAdrSet visited; + + assert (segment_source != segment_target); + /*display (LEVEL, DEBUG, "d starting exploration\n");*/ + + segment_source->COST = 0; + segment_source->H = eval_equi (segment_source, segment_target); + segment_source->HCOST = segment_source->H; + segment_source->TAG = TAG_TERRA; + segment_source->ROOT = NULL; + segment_source->AUX = NULL; + + terra_incognita.insert(segment_source); + + + while (!(terra_incognita.empty())) { + + ocrWSegment *node = NULL; + ocrWSegment *seg = NULL; + + node = *terra_incognita.begin(); + + terra_incognita.erase(node); + + if (node->SIGNAL_INDEX == CUR_SIG_INDEX) { + /* on a gagne */ + + // XXX FIXME + clean_tag_list (visited); + clean_tag_set (terra_incognita); + node->TAG = TAG_UNDEF; + return node; + } + + node->TAG = TAG_VISITED; + visited.insert(node); + + /* expansion */ + if (getWSegDirection (param, node) == ocrHorizontal) { + if ( (node->P_MIN) > 0 ) { + seg = getWSegment (grid, node->P_MIN - 1, node->OFFSET, node->LAYER); + if (seg->SIGNAL_INDEX == CUR_SIG_INDEX) + XPLORE + } + if ( (node->P_MAX) < (grid->SIZE_H - 2) ) { + seg = getWSegment (grid, node->P_MAX + 1, node->OFFSET, node->LAYER); + if (seg->SIGNAL_INDEX == CUR_SIG_INDEX) + XPLORE + } + } else { /* vertical */ + if ( (node->P_MIN) > 0 ) { + seg = getWSegment (grid, node->OFFSET, node->P_MIN - 1, node->LAYER); + if (seg->SIGNAL_INDEX == CUR_SIG_INDEX) + XPLORE + } + if ( (node->P_MAX) < (grid->SIZE_V - 2) ) { + seg = getWSegment (grid, node->OFFSET, node->P_MAX + 1, node->LAYER); + if (seg->SIGNAL_INDEX == CUR_SIG_INDEX) + XPLORE + } + } + + for (int i = -1 ; i <= 1 ; i += 2) { + switch (i) { + + case -1: if ( ((node->LAYER) ) == 0 ) + continue; + break; + case 1: if ( ((node->LAYER) + 1) >= (grid->NB_OF_LAYERS) ) + continue; + break; + + default: + /*display (LEVEL, ERROR, __FILE__ ":" __LINE__ "exand_eval Oops\n");*/ + abort (); + } + for (ocrNaturalInt p = node->P_MIN ; p <= node->P_MAX ; p++) { + + /*display (LEVEL, DEBUG, "d expanding %ld\n", i);*/ + seg = getWSegment(grid, + getWSegXCoord (param, node, p), + getWSegYCoord (param, node, p), + node->LAYER + i + ); + +#ifdef DEBUG + if (!seg) { + display (LEVEL, DEBUG, "oddity: null seg at (%ld, %ld, %ld)\n", getWSegXCoord(param, node, p), getWSegXCoord(param, node, p), node->LAYER + i); + continue; + } +#endif + XPLORE + } + } /* end for i */ + } /* while */ + + /* Failed */ + clean_tag (); + + /*display (LEVEL, DEBUG, "d exploration failed\n");*/ + + + return NULL; +} + + + +ocrNaturalInt make_segments (ocrWSegment *segment_dest, + ocrWSegment *segment_source, + ocrNaturalInt xdest, + ocrNaturalInt ydest, + ocrNaturalInt zdest, + ocrNaturalInt signal_index, + ocrSignal * i_pSignal + ) +{ + ocrWSegment *seg, *nseg, *root, *aux; + ocrNaturalInt xp, yp, xn, yn; + ocrNaturalInt p1, p2, pp1, pp2; + /*ocrNaturalInt pi;*/ + ocrNaturalInt distance = 0; + ocrNaturalInt nb_segs = 0; + + /* initialisation */ + + aux = i_pSignal->SEGMENT; + + seg = segment_dest; + + /*display (LEVEL, DEBUG, "c dest (%ld, %ld, %ld)\n", xdest, ydest, zdest);*/ + xp = xdest; + yp = ydest; + + if (seg->SIGNAL_INDEX == signal_index) { + /*display (LEVEL, DEBUG, "c joining equi at seg (off=%ld, p_min=%ld, p_max=%ld)\n", seg->OFFSET, seg->P_MIN, seg->P_MAX);*/ + + if ( + getWSegDirection (param, seg) + == + getWSegDirection (param, seg->ROOT) + ) + { + xp = getWSegXCoord (param, seg, seg->P_MIN); + yp = getWSegYCoord (param, seg, seg->P_MIN); + } else { + xp = getWSegXCoord (param, seg, seg->ROOT->OFFSET); + yp = getWSegYCoord (param, seg, seg->ROOT->OFFSET); + } + nseg = seg->ROOT; + seg->ROOT = NULL; + seg = nseg; + } + nseg = NULL; + + /* + + seg, seg->ROOT + nseg <= split (seg) + + nseg -> NEXT = prevseg; + prevseg = nseg; + + */ + + while (seg->ROOT) { + root = seg->ROOT; + xn = getWSegXCoord (param, root, xp); + yn = getWSegYCoord (param, root, yp); + /*display (LEVEL, DEBUG, "p (%ld, %ld, %ld) -> (%ld, %ld, %ld) : d=%ld, c=%ld, h=%ld, hc=%ld\n", xp, yp, seg->LAYER, xn, yn, seg->LAYER, distance, seg->COST, seg->H, seg->HCOST);*/ + ((getWSegDirection(param, seg) == ocrHorizontal) ? (p1 = xp, p2 = xn) : (p1 = yp, p2 = yn) ); + ( (p1 < p2) ? (pp1 = p1, pp2 = p2) : (pp1 = p2, pp2 = p1) ); + nseg = splitWSegment (param, grid, seg, + pp1, pp2, + signal_index + ); + nseg->NEXT = aux; + aux = nseg; + distance += (pp2 - pp1); + nb_segs ++; + xp = xn; + yp = yn; + /*seg->SIGNAL_INDEX = signal_index;*/ + seg = root; + } + + /*assert (nseg == segment_source);*/ + xn = xs; + yn = ys; + /*root = seg->ROOT;*/ + ((getWSegDirection(param, seg) == ocrHorizontal) ? (p1 = xp, p2 = xn) : (p1 = yp, p2 = yn) ); + ( (p1 < p2) ? (pp1 = p1, pp2 = p2) : (pp1 = p2, pp2 = p1) ); + nseg = splitWSegment (param, grid, seg, + pp1, pp2, + signal_index + ); + nseg->NEXT = aux; + i_pSignal->SEGMENT = nseg; + + distance += (pp2 - pp1); + nb_segs ++; +#if 0 + display (LEVEL, DEBUG, "p (%ld, %ld, %ld) -> (%ld, %ld, %ld) : %ld\n", xp, yp, seg->LAYER, xn, yn, zs, distance); + display (LEVEL, DEBUG, "c source (%ld, %ld, %ld)\n", xs, ys, zs); + display (LEVEL, DEBUG, "p num of segments : %ld\n", nb_segs); + display (LEVEL, DEBUG, "i path length : %ld\n", distance); +#endif + /*seg->SIGNAL_INDEX = signal_index;*/ + + + return distance * 5; +} + + + +ocrNaturalInt check_path (ocrSignal *i_pSignal) { + + ocrWSegment *seg; + ocrNaturalInt dist; + + seg = i_pSignal->SEGMENT; + + dist = 0; + + while (seg) { + if ( (seg->SIGNAL_INDEX) != (i_pSignal->INDEX) ) { + display (LEVEL, ERROR, " ---INVALID Segment->SIGNAL_INDEX\n"); + abort (); + } + + display (LEVEL, DEBUG, "(%ld, %ld, %ld, %ld)\n", seg->OFFSET, seg->P_MIN, seg->P_MAX, seg->LAYER); + + dist += seg->P_MAX - seg->P_MIN; + + seg = seg->NEXT; + } + + display (LEVEL, DEBUG, "DIST CHECK --- %ld\n", dist); + + return dist; +} + + + + + +ocrNaturalInt find_path_astar (ocrRoutingParameters * p_param, + ocrWRoutingGrid * p_grid, + ocrNaturalInt xsource, + ocrNaturalInt ysource, + ocrNaturalInt zsource, + ocrNaturalInt xtarget, + ocrNaturalInt ytarget, + ocrNaturalInt ztarget, + ocrNaturalInt signal_index, + ocrSignal * i_pSignal, + ocrNaturalInt mode) +{ + ocrWSegment *path = NULL; + ocrWSegment *segment_source = NULL; + ocrWSegment *segment_dest = NULL; + ocrNaturalInt dist, check_dist; + + grid = p_grid; + param = p_param; + + xs = xsource; + ys = ysource; + zs = zsource; + + CUR_SIG_INDEX = signal_index; + /*clean_tag ();*/ + + /*kost = ( + (mode == AS_K_EQUI) ? + &eval : + &eval_equi + ) + ;*/ + + + + segment_source = getWSegment (grid, + xsource, + ysource, + zsource + ); + /* display (LEVEL, DEBUG, "d source : %p\n", segment_source); */ + + switch (mode) { + case AS_K_SEG: + segment_dest = getWSegment (grid, + xtarget, + ytarget, + ztarget + ); + break; + case AS_K_EQUI: + segment_dest = i_pSignal->SEGMENT; + break; + } + + /*display (LEVEL, DEBUG, "d dest : %p\n", segment_dest);*/ + + if ((segment_source != segment_dest) && (!isObstructed(segment_source))) { + switch (mode) { + case AS_K_SEG: //path = explore (segment_source, segment_dest); + break; + case AS_K_EQUI: assert (i_pSignal->SEGMENT); + ze_best = NULL; + path = explore_equi (segment_source, segment_dest); + break; + } + } else { + path = segment_source; + path->ROOT = NULL; + path->NEXT = NULL; + /*display (LEVEL, DEBUG, "o direct link\n");*/ + } + + if (!path) { +#if 0 + if (g_pOption->RIPUP) { + + + + } +#endif + /*display (LEVEL, DEBUG, "o A* failed\n");*/ + return OCRNATURALINT_MAX; + } + + /*display (LEVEL, DEBUG, "o A* succeeded\n");*/ + + /*clean_tag ();*/ + + dist = make_segments ( + path, + segment_source, + xtarget, + ytarget, + ztarget, + signal_index, + i_pSignal + ); + + + /*assert (((check_dist = check_path (i_pSignal)), dist == check_dist));*/ + + return dist; +} + + +/** + * + **/ +/*void init_Astar (ocrRoutingDataBase *db) { + assert (db); + + grid = db->GRID; + param = db->PARAM; + + return; +} +*/