int
plan_do_local(plan_t *plan, double lx, double ly, double plan_halfwidth)
{
double gx, gy;
int li, lj;
int xmin,ymin,xmax,ymax;
plan_cell_t* cell;
double t0,t1;
t0 = get_time();
// Set bounds as directed
xmin = PLAN_GXWX(plan, lx - plan_halfwidth);
ymin = PLAN_GXWX(plan, ly - plan_halfwidth);
xmax = PLAN_GXWX(plan, lx + plan_halfwidth);
ymax = PLAN_GXWX(plan, ly + plan_halfwidth);
plan_set_bounds(plan, xmin, ymin, xmax, ymax);
// Reset plan costs (within the local patch)
plan_reset(plan);
// Find a local goal to pursue
if(_plan_find_local_goal(plan, &gx, &gy, lx, ly) != 0)
{
puts("no local goal");
return(-1);
}
//printf("local goal: %.3lf, %.3lf\n", gx, gy);
plan->lpath_count = 0;
if(_plan_update_plan(plan, lx, ly, gx, gy) != 0)
{
puts("local plan update failed");
return(-1);
}
li = PLAN_GXWX(plan, lx);
lj = PLAN_GYWY(plan, ly);
// Cache the path
for(cell = plan->cells + PLAN_INDEX(plan,li,lj);
cell;
cell = cell->plan_next)
{
if(plan->lpath_count >= plan->lpath_size)
{
plan->lpath_size *= 2;
plan->lpath = (plan_cell_t**)realloc(plan->lpath,
plan->lpath_size * sizeof(plan_cell_t*));
assert(plan->lpath);
}
plan->lpath[plan->lpath_count++] = cell;
}
t1 = get_time();
printf("computed local path: %.6lf\n", t1-t0);
return(0);
}
int
plan_do_global(plan_t *plan, double lx, double ly, double gx, double gy)
{
plan_cell_t* cell;
int li, lj;
double t0,t1;
t0 = get_time();
// Set bounds to look over the entire grid
plan_set_bounds(plan, 0, 0, plan->size_x - 1, plan->size_y - 1);
// Reset plan costs
plan_reset(plan);
plan->path_count = 0;
if(_plan_update_plan(plan, lx, ly, gx, gy) < 0)
{
// no path
return(-1);
}
li = PLAN_GXWX(plan, lx);
lj = PLAN_GYWY(plan, ly);
// Cache the path
for(cell = plan->cells + PLAN_INDEX(plan,li,lj);
cell;
cell = cell->plan_next)
{
if(plan->path_count >= plan->path_size)
{
plan->path_size *= 2;
plan->path = (plan_cell_t**)realloc(plan->path,
plan->path_size * sizeof(plan_cell_t*));
assert(plan->path);
}
plan->path[plan->path_count++] = cell;
}
t1 = get_time();
//printf("computed global path: %.6lf\n", t1-t0);
return(0);
}
int
_plan_find_local_goal(plan_t *plan, double* gx, double* gy,
double lx, double ly)
{
int c;
int c_min;
double squared_d;
double squared_d_min;
int li,lj;
plan_cell_t* cell;
// Must already have computed a global goal
if(plan->path_count == 0)
{
//puts("no global path");
return(-1);
}
li = PLAN_GXWX(plan, lx);
lj = PLAN_GYWY(plan, ly);
assert(PLAN_VALID_BOUNDS(plan,li,lj));
// Find the closest place to jump on the global path
squared_d_min = DBL_MAX;
c_min = -1;
for(c=0;c<plan->path_count;c++)
{
cell = plan->path[c];
squared_d = ((cell->ci - li) * (cell->ci - li) +
(cell->cj - lj) * (cell->cj - lj));
if(squared_d < squared_d_min)
{
squared_d_min = squared_d;
c_min = c;
}
}
assert(c_min > -1);
// Follow the path to find the last cell that's inside the local planning
// area
for(c=c_min; c<plan->path_count; c++)
{
cell = plan->path[c];
//printf("step %d: (%d,%d)\n", c, cell->ci, cell->cj);
if((cell->ci < plan->min_x) || (cell->ci > plan->max_x) ||
(cell->cj < plan->min_y) || (cell->cj > plan->max_y))
{
// Did we move at least one cell along the path?
if(c == c_min)
{
// nope; the entire global path is outside the local region; can't
// fix that here
puts("global path not in local region");
return(-1);
}
else
break;
}
}
assert(c > c_min);
cell = plan->path[c-1];
//printf("ci: %d cj: %d\n", cell->ci, cell->cj);
*gx = PLAN_WXGX(plan, cell->ci);
*gy = PLAN_WYGY(plan, cell->cj);
return(0);
}
// Generate the plan
int
_plan_update_plan(plan_t *plan, double lx, double ly, double gx, double gy)
{
int oi, oj, di, dj, ni, nj;
int gi, gj, li,lj;
float cost;
plan_cell_t *cell, *ncell;
char old_occ_state;
float old_occ_dist;
// Reset the queue
heap_reset(plan->heap);
// Initialize the goal cell
gi = PLAN_GXWX(plan, gx);
gj = PLAN_GYWY(plan, gy);
// Initialize the start cell
li = PLAN_GXWX(plan, lx);
lj = PLAN_GYWY(plan, ly);
//printf("planning from %d,%d to %d,%d\n", li,lj,gi,gj);
if(!PLAN_VALID_BOUNDS(plan, gi, gj))
{
puts("goal out of bounds");
return(-1);
}
if(!PLAN_VALID_BOUNDS(plan, li, lj))
{
puts("start out of bounds");
return(-1);
}
// Latch and clear the obstacle state for the cell I'm in
cell = plan->cells + PLAN_INDEX(plan, li, lj);
old_occ_state = cell->occ_state_dyn;
old_occ_dist = cell->occ_dist_dyn;
cell->occ_state_dyn = -1;
cell->occ_dist_dyn = (float) plan->max_radius;
cell = plan->cells + PLAN_INDEX(plan, gi, gj);
cell->plan_cost = 0;
// Are we done?
if((li == gi) && (lj == gj))
return(0);
plan_push(plan, cell);
while (1)
{
float * p;
cell = plan_pop(plan);
if (cell == NULL)
break;
oi = cell->ci;
oj = cell->cj;
//printf("pop %d %d %f\n", cell->ci, cell->cj, cell->plan_cost);
p = plan->dist_kernel_3x3;
for (dj = -1; dj <= +1; dj++)
{
ncell = plan->cells + PLAN_INDEX(plan,oi-1,oj+dj);
for (di = -1; di <= +1; di++, p++, ncell++)
{
if (!di && !dj)
continue;
//if (di && dj)
//continue;
ni = oi + di;
nj = oj + dj;
if (!PLAN_VALID_BOUNDS(plan, ni, nj))
continue;
if(ncell->mark)
continue;
if (ncell->occ_dist_dyn < plan->abs_min_radius)
continue;
cost = cell->plan_cost;
if(ncell->lpathmark)
cost += (float) ((*p) * plan->hysteresis_factor);
else
cost += *p;
if(ncell->occ_dist_dyn < plan->max_radius)
cost += (float) (plan->dist_penalty * (plan->max_radius - ncell->occ_dist_dyn));
if(cost < ncell->plan_cost)
{
ncell->plan_cost = cost;
ncell->plan_next = cell;
plan_push(plan, ncell);
}
}
}
}
// Restore the obstacle state for the cell I'm in
cell = plan->cells + PLAN_INDEX(plan, li, lj);
cell->occ_state_dyn = old_occ_state;
cell->occ_dist_dyn = old_occ_dist;
if(!cell->plan_next)
{
//puts("never found start");
return(-1);
}
else
return(0);
}
// Generate the plan
int
_plan_update_plan(plan_t *plan, double lx, double ly, double gx, double gy)
{
int oi, oj, di, dj, ni, nj;
int gi, gj, li,lj;
float cost;
plan_cell_t *cell, *ncell;
// Reset the queue
heap_reset(plan->heap);
// Initialize the goal cell
gi = PLAN_GXWX(plan, gx);
gj = PLAN_GYWY(plan, gy);
// Initialize the start cell
li = PLAN_GXWX(plan, lx);
lj = PLAN_GYWY(plan, ly);
//printf("planning from %d,%d to %d,%d\n", li,lj,gi,gj);
if(!PLAN_VALID_BOUNDS(plan, gi, gj))
{
puts("goal out of bounds");
return(-1);
}
if(!PLAN_VALID_BOUNDS(plan, li, lj))
{
puts("start out of bounds");
return(-1);
}
cell = plan->cells + PLAN_INDEX(plan, gi, gj);
cell->plan_cost = 0;
plan_push(plan, cell);
while (1)
{
float * p;
cell = plan_pop(plan);
if (cell == NULL)
break;
oi = cell->ci;
oj = cell->cj;
//printf("pop %d %d %f\n", cell->ci, cell->cj, cell->plan_cost);
p = plan->dist_kernel_3x3;
for (dj = -1; dj <= +1; dj++)
{
ncell = plan->cells + PLAN_INDEX(plan,oi-1,oj+dj);
for (di = -1; di <= +1; di++, p++, ncell++)
{
if (!di && !dj)
continue;
//if (di && dj)
//continue;
ni = oi + di;
nj = oj + dj;
if (!PLAN_VALID_BOUNDS(plan, ni, nj))
continue;
if(ncell->mark)
continue;
if (ncell->occ_dist_dyn < plan->abs_min_radius)
continue;
cost = cell->plan_cost + *p;
if(ncell->occ_dist_dyn < plan->max_radius)
cost += plan->dist_penalty * (plan->max_radius - ncell->occ_dist_dyn);
if(cost < ncell->plan_cost)
{
ncell->plan_cost = cost;
ncell->plan_next = cell;
plan_push(plan, ncell);
}
}
}
}
cell = plan->cells + PLAN_INDEX(plan, li, lj);
if(!cell->plan_next)
{
puts("never found start");
return(-1);
}
else
return(0);
}
// Generate a path to the goal
void plan_update_waypoints(plan_t *plan, double px, double py)
{
double dist;
int ni, nj;
plan_cell_t *cell, *ncell;
plan->waypoint_count = 0;
ni = PLAN_GXWX(plan, px);
nj = PLAN_GYWY(plan, py);
// Can't plan a path if we're off the map
if(!PLAN_VALID(plan,ni,nj))
return;
cell = plan->cells + PLAN_INDEX(plan, ni, nj);
while (cell != NULL)
{
if (plan->waypoint_count >= plan->waypoint_size)
{
plan->waypoint_size *= 2;
plan->waypoints = realloc(plan->waypoints,
plan->waypoint_size * sizeof(plan->waypoints[0]));
}
plan->waypoints[plan->waypoint_count++] = cell;
if (cell->plan_next == NULL)
{
// done
break;
}
// Find the farthest cell in the path that is reachable from the
// currrent cell.
dist = 0;
for(ncell = cell; ncell->plan_next != NULL; ncell = ncell->plan_next)
{
if(dist > 0.50)
{
if(!plan_test_reachable(plan, cell, ncell->plan_next))
break;
}
dist += plan->scale;
}
if(ncell == cell)
{
break;
}
cell = ncell;
}
if(cell && (cell->plan_cost > 0))
{
// no path
plan->waypoint_count = 0;
}
return;
}
// Test to see if once cell is reachable from another.
// This could be improved.
int plan_test_reachable(plan_t *plan, plan_cell_t *cell_a, plan_cell_t *cell_b)
{
int i, j;
int ai, aj, bi, bj;
double ox, oy, oa;
double dx, dy;
plan_cell_t *cell;
ai = cell_a->ci;
aj = cell_a->cj;
bi = cell_b->ci;
bj = cell_b->cj;
ox = PLAN_WXGX(plan, ai);
oy = PLAN_WYGY(plan, aj);
oa = atan2(bj - aj, bi - ai);
if (fabs(cos(oa)) > fabs(sin(oa)))
{
dy = tan(oa) * plan->scale;
if (ai < bi)
{
for (i = ai; i < bi; i++)
{
j = PLAN_GYWY(plan, oy + (i - ai) * dy);
if (PLAN_VALID(plan, i, j))
{
cell = plan->cells + PLAN_INDEX(plan, i, j);
if (cell->occ_dist < plan->abs_min_radius)
return 0;
}
}
}
else
{
for (i = ai; i > bi; i--)
{
j = PLAN_GYWY(plan, oy + (i - ai) * dy);
if (PLAN_VALID(plan, i, j))
{
cell = plan->cells + PLAN_INDEX(plan, i, j);
if (cell->occ_dist < plan->abs_min_radius)
return 0;
}
}
}
}
else
{
dx = tan(M_PI/2 - oa) * plan->scale;
if (aj < bj)
{
for (j = aj; j < bj; j++)
{
i = PLAN_GXWX(plan, ox + (j - aj) * dx);
if (PLAN_VALID(plan, i, j))
{
cell = plan->cells + PLAN_INDEX(plan, i, j);
if (cell->occ_dist < plan->abs_min_radius)
return 0;
}
}
}
else
{
for (j = aj; j > bj; j--)
{
i = PLAN_GXWX(plan, ox + (j - aj) * dx);
if (PLAN_VALID(plan, i, j))
{
cell = plan->cells + PLAN_INDEX(plan, i, j);
if (cell->occ_dist < plan->abs_min_radius)
return 0;
}
}
}
}
return 1;
}