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);
}
