|
@@ -1,47 +1,74 @@
|
|
|
#include <stdlib.h>
|
|
|
#include <unistd.h>
|
|
|
-#include "Gwater.h"
|
|
|
#include <grass/gis.h>
|
|
|
#include <grass/glocale.h>
|
|
|
+#include "Gwater.h"
|
|
|
+#include "do_astar.h"
|
|
|
|
|
|
+int sift_up(int, CELL);
|
|
|
|
|
|
int do_astar(void)
|
|
|
{
|
|
|
POINT point;
|
|
|
int doer, count;
|
|
|
- double get_slope();
|
|
|
SHORT upr, upc, r, c, ct_dir;
|
|
|
CELL work_val, alt_val, alt_up, asp_up, wat_val;
|
|
|
CELL in_val, drain_val;
|
|
|
- double slope;
|
|
|
+ HEAP heap_pos;
|
|
|
+
|
|
|
+ /* double slope; */
|
|
|
|
|
|
G_message(_("SECTION 2: A * Search."));
|
|
|
|
|
|
count = 0;
|
|
|
+ seg_get(&heap_index, (char *)&heap_pos, 0, 1);
|
|
|
+ first_astar = heap_pos.point;
|
|
|
+
|
|
|
+ /* A * Search: downhill path through all not masked cells */
|
|
|
while (first_astar != -1) {
|
|
|
G_percent(count++, do_points, 1);
|
|
|
- doer = first_astar;
|
|
|
+
|
|
|
+ seg_get(&heap_index, (char *)&heap_pos, 0, 1);
|
|
|
+ doer = heap_pos.point;
|
|
|
+
|
|
|
seg_get(&astar_pts, (char *)&point, 0, doer);
|
|
|
- first_astar = point.nxt;
|
|
|
+
|
|
|
+ /* drop astar_pts[doer] from heap */
|
|
|
+ drop_pt();
|
|
|
+
|
|
|
+ seg_get(&heap_index, (char *)&heap_pos, 0, 1);
|
|
|
+ first_astar = heap_pos.point;
|
|
|
+
|
|
|
point.nxt = first_cum;
|
|
|
seg_put(&astar_pts, (char *)&point, 0, doer);
|
|
|
+
|
|
|
first_cum = doer;
|
|
|
r = point.r;
|
|
|
c = point.c;
|
|
|
+
|
|
|
bseg_put(&worked, &one, r, c);
|
|
|
cseg_get(&alt, &alt_val, r, c);
|
|
|
+
|
|
|
+ /* check all neighbours, breadth first search */
|
|
|
for (ct_dir = 0; ct_dir < sides; ct_dir++) {
|
|
|
+ /* get r, c (upr, upc) for this neighbour */
|
|
|
upr = r + nextdr[ct_dir];
|
|
|
upc = c + nextdc[ct_dir];
|
|
|
+ /* check that upr, upc are within region */
|
|
|
if (upr >= 0 && upr < nrows && upc >= 0 && upc < ncols) {
|
|
|
+ /* check if neighbour is in the list */
|
|
|
+ /* if not, add as new point */
|
|
|
bseg_get(&in_list, &in_val, upr, upc);
|
|
|
if (in_val == 0) {
|
|
|
cseg_get(&alt, &alt_up, upr, upc);
|
|
|
add_pt(upr, upc, r, c, alt_up, alt_val);
|
|
|
+ /* flow direction is set here */
|
|
|
drain_val = drain[upr - r + 1][upc - c + 1];
|
|
|
cseg_put(&asp, &drain_val, upr, upc);
|
|
|
}
|
|
|
else {
|
|
|
+ /* check if neighbour has not been worked on,
|
|
|
+ * update values for asp, wat and slp */
|
|
|
bseg_get(&worked, &work_val, upr, upc);
|
|
|
if (!work_val) {
|
|
|
cseg_get(&asp, &asp_up, upr, upc);
|
|
@@ -54,9 +81,8 @@ int do_astar(void)
|
|
|
cseg_put(&wat, &wat_val, r, c);
|
|
|
cseg_get(&alt, &alt_up, upr, upc);
|
|
|
replace(upr, upc, r, c); /* alt_up used to be */
|
|
|
- slope =
|
|
|
- get_slope(upr, upc, r, c, alt_up, alt_val);
|
|
|
- dseg_put(&slp, &slope, upr, upc);
|
|
|
+ /* slope = get_slope (upr, upc, r, c, alt_up, alt_val);
|
|
|
+ dseg_put (&slp, &slope, upr, upc); */
|
|
|
}
|
|
|
}
|
|
|
}
|
|
@@ -65,58 +91,176 @@ int do_astar(void)
|
|
|
}
|
|
|
bseg_close(&worked);
|
|
|
bseg_close(&in_list);
|
|
|
+ seg_close(&heap_index);
|
|
|
|
|
|
G_percent(count, do_points, 1); /* finish it */
|
|
|
return 0;
|
|
|
}
|
|
|
|
|
|
+/* new add point routine for min heap */
|
|
|
int add_pt(SHORT r, SHORT c, SHORT downr, SHORT downc, CELL ele, CELL downe)
|
|
|
{
|
|
|
- int p;
|
|
|
- CELL check_ele;
|
|
|
- POINT point, new_point;
|
|
|
- double slp_value, get_slope();
|
|
|
+ POINT point;
|
|
|
+ HEAP heap_pos;
|
|
|
|
|
|
- if (nxt_avail_pt == do_points)
|
|
|
- G_fatal_error(_("problem w/ astar algorithm"));
|
|
|
+ /* double slp_value; */
|
|
|
|
|
|
- slp_value = get_slope(r, c, downr, downc, ele, downe);
|
|
|
- dseg_put(&slp, &slp_value, r, c);
|
|
|
+ /* slp_value = get_slope(r, c, downr, downc, ele, downe);
|
|
|
+ dseg_put (&slp, &slp_value, r, c); */
|
|
|
bseg_put(&in_list, &one, r, c);
|
|
|
- new_point.r = r;
|
|
|
- new_point.c = c;
|
|
|
- new_point.downr = downr;
|
|
|
- new_point.downc = downc;
|
|
|
- if (first_astar == -1) {
|
|
|
- new_point.nxt = -1;
|
|
|
- seg_put(&astar_pts, (char *)&new_point, 0, nxt_avail_pt);
|
|
|
- first_astar = nxt_avail_pt;
|
|
|
- nxt_avail_pt++;
|
|
|
+
|
|
|
+ /* add point to next free position */
|
|
|
+
|
|
|
+ heap_size++;
|
|
|
+
|
|
|
+ if (heap_size > do_points)
|
|
|
+ G_fatal_error(_("heapsize too large"));
|
|
|
+
|
|
|
+ heap_pos.point = nxt_avail_pt;
|
|
|
+ heap_pos.ele = ele;
|
|
|
+ seg_put(&heap_index, (char *)&heap_pos, 0, heap_size);
|
|
|
+
|
|
|
+ point.r = r;
|
|
|
+ point.c = c;
|
|
|
+ point.downr = downr;
|
|
|
+ point.downc = downc;
|
|
|
+
|
|
|
+ seg_put(&astar_pts, (char *)&point, 0, nxt_avail_pt);
|
|
|
+
|
|
|
+ nxt_avail_pt++;
|
|
|
+
|
|
|
+ /* sift up: move new point towards top of heap */
|
|
|
+
|
|
|
+ sift_up(heap_size, ele);
|
|
|
+
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+/* drop point routine for min heap */
|
|
|
+int drop_pt(void)
|
|
|
+{
|
|
|
+ int child, childr, parent;
|
|
|
+ int childp, childrp, parentp;
|
|
|
+ CELL ele, eler;
|
|
|
+ int i;
|
|
|
+ POINT point;
|
|
|
+ HEAP heap_pos;
|
|
|
+
|
|
|
+ if (heap_size == 1) {
|
|
|
+ parent = -1;
|
|
|
+ heap_pos.point = -1;
|
|
|
+ heap_pos.ele = 0;
|
|
|
+ seg_put(&heap_index, (char *)&heap_pos, 0, 1);
|
|
|
+ heap_size = 0;
|
|
|
return 0;
|
|
|
}
|
|
|
- p = first_astar;;
|
|
|
- while (1) {
|
|
|
- seg_get(&astar_pts, (char *)&point, 0, p);
|
|
|
- cseg_get(&alt, &check_ele, point.r, point.c);
|
|
|
- if (check_ele > ele) {
|
|
|
- new_point.nxt = nxt_avail_pt;
|
|
|
- seg_put(&astar_pts, (char *)&new_point, 0, p);
|
|
|
- seg_put(&astar_pts, (char *)&point, 0, nxt_avail_pt);
|
|
|
- nxt_avail_pt++;
|
|
|
- return 0;
|
|
|
+
|
|
|
+ /* start with heap root */
|
|
|
+ parent = 1;
|
|
|
+
|
|
|
+ /* sift down: move hole back towards bottom of heap */
|
|
|
+ /* sift-down routine customised for A * Search logic */
|
|
|
+
|
|
|
+ while ((child = GET_CHILD(parent)) <= heap_size) {
|
|
|
+ /* select child with lower ele, if both are equal, older child
|
|
|
+ * older child is older startpoint for flow path, important */
|
|
|
+ seg_get(&heap_index, (char *)&heap_pos, 0, child);
|
|
|
+ childp = heap_pos.point;
|
|
|
+ ele = heap_pos.ele;
|
|
|
+ if (child < heap_size) {
|
|
|
+ childr = child + 1;
|
|
|
+ i = 1;
|
|
|
+ while (childr <= heap_size && i < 3) {
|
|
|
+ seg_get(&heap_index, (char *)&heap_pos, 0, childr);
|
|
|
+ childrp = heap_pos.point;
|
|
|
+ eler = heap_pos.ele;
|
|
|
+ if (eler < ele) {
|
|
|
+ child = childr;
|
|
|
+ childp = childrp;
|
|
|
+ ele = eler;
|
|
|
+ }
|
|
|
+ /* make sure we get the oldest child */
|
|
|
+ else if (ele == eler && childp > childrp) {
|
|
|
+ child = childr;
|
|
|
+ childp = childrp;
|
|
|
+ }
|
|
|
+ childr++;
|
|
|
+ i++;
|
|
|
+ }
|
|
|
}
|
|
|
- if (point.nxt == -1) {
|
|
|
- point.nxt = nxt_avail_pt;
|
|
|
- seg_put(&astar_pts, (char *)&point, 0, p);
|
|
|
- new_point.nxt = -1;
|
|
|
- seg_put(&astar_pts, (char *)&new_point, 0, nxt_avail_pt);
|
|
|
- nxt_avail_pt++;
|
|
|
|
|
|
- return 0;
|
|
|
+ /* move hole down */
|
|
|
+ heap_pos.point = childp;
|
|
|
+ heap_pos.ele = ele;
|
|
|
+ seg_put(&heap_index, (char *)&heap_pos, 0, parent);
|
|
|
+ parent = child;
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ /* hole is in lowest layer, move to heap end */
|
|
|
+ if (parent < heap_size) {
|
|
|
+ seg_get(&heap_index, (char *)&heap_pos, 0, heap_size);
|
|
|
+ seg_put(&heap_index, (char *)&heap_pos, 0, parent);
|
|
|
+
|
|
|
+ ele = heap_pos.ele;
|
|
|
+
|
|
|
+ /* sift up last swapped point, only necessary if hole moved to heap end */
|
|
|
+ sift_up(parent, ele);
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ /* the actual drop */
|
|
|
+ heap_size--;
|
|
|
+
|
|
|
+ return 0;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+/* standard sift-up routine for d-ary min heap */
|
|
|
+int sift_up(int start, CELL ele)
|
|
|
+{
|
|
|
+ int parent, parentp, child, childp;
|
|
|
+ POINT point;
|
|
|
+ CELL elep;
|
|
|
+ HEAP heap_pos;
|
|
|
+
|
|
|
+ child = start;
|
|
|
+ seg_get(&heap_index, (char *)&heap_pos, 0, child);
|
|
|
+ childp = heap_pos.point;
|
|
|
+
|
|
|
+ while (child > 1) {
|
|
|
+ parent = GET_PARENT(child);
|
|
|
+ seg_get(&heap_index, (char *)&heap_pos, 0, parent);
|
|
|
+ parentp = heap_pos.point;
|
|
|
+ elep = heap_pos.ele;
|
|
|
+
|
|
|
+ /* parent ele higher */
|
|
|
+ if (elep > ele) {
|
|
|
+
|
|
|
+ /* push parent point down */
|
|
|
+ seg_put(&heap_index, (char *)&heap_pos, 0, child);
|
|
|
+ child = parent;
|
|
|
+
|
|
|
}
|
|
|
- p = point.nxt;
|
|
|
+ /* same ele, but parent is younger */
|
|
|
+ else if (elep == ele && parentp > childp) {
|
|
|
+
|
|
|
+ /* push parent point down */
|
|
|
+ seg_put(&heap_index, (char *)&heap_pos, 0, child);
|
|
|
+ child = parent;
|
|
|
+
|
|
|
+ }
|
|
|
+ else
|
|
|
+ /* no more sifting up, found new slot for child */
|
|
|
+ break;
|
|
|
}
|
|
|
|
|
|
+ /* no more sifting up, found new slot for child */
|
|
|
+ if (child < start) {
|
|
|
+ heap_pos.point = childp;
|
|
|
+ heap_pos.ele = ele;
|
|
|
+ seg_put(&heap_index, (char *)&heap_pos, 0, child);
|
|
|
+ }
|
|
|
return 0;
|
|
|
}
|
|
|
|
|
@@ -140,11 +284,15 @@ int replace( /* ele was in there */
|
|
|
SHORT upr, SHORT upc, SHORT r, SHORT c)
|
|
|
/* CELL ele; */
|
|
|
{
|
|
|
- int now;
|
|
|
+ int now, heap_run;
|
|
|
POINT point;
|
|
|
+ HEAP heap_pos;
|
|
|
+
|
|
|
+ heap_run = 0;
|
|
|
|
|
|
- now = first_astar;
|
|
|
- while (now != -1) {
|
|
|
+ while (heap_run <= heap_size) {
|
|
|
+ seg_get(&heap_index, (char *)&heap_pos, 0, heap_run);
|
|
|
+ now = heap_pos.point;
|
|
|
seg_get(&astar_pts, (char *)&point, 0, now);
|
|
|
if (point.r == upr && point.c == upc) {
|
|
|
point.downr = r;
|
|
@@ -152,7 +300,7 @@ int replace( /* ele was in there */
|
|
|
seg_put(&astar_pts, (char *)&point, 0, now);
|
|
|
return 0;
|
|
|
}
|
|
|
- now = point.nxt;
|
|
|
+ heap_run++;;
|
|
|
}
|
|
|
return 0;
|
|
|
}
|