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);
}
// Test to see if once cell is reachable from another.
int plan_test_reachable(plan_t *plan, plan_cell_t *cell_a, plan_cell_t *cell_b)
{
double theta;
double sinth, costh;
double i,j;
int lasti, lastj;
theta = atan2((double)(cell_b->cj - cell_a->cj),
(double)(cell_b->ci - cell_a->ci));
sinth = sin(theta);
costh = cos(theta);
lasti = lastj = -1;
i = (double)cell_a->ci;
j = (double)cell_a->cj;
while((lasti != cell_b->ci) || (lastj != cell_b->cj))
{
if((lasti != (int)floor(i)) || (lastj != (int)floor(j)))
{
lasti = (int)floor(i);
lastj = (int)floor(j);
if(!PLAN_VALID(plan,lasti,lastj))
{
//PLAYER_WARN("stepped off the map!");
return(0);
}
if(plan->cells[PLAN_INDEX(plan,lasti,lastj)].occ_dist <
plan->abs_min_radius)
return(0);
}
if(lasti != cell_b->ci)
i += costh;
if(lastj != cell_b->cj)
j += sinth;
}
return(1);
}
// 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);
}
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;
int i;
t0 = get_time();
// Set bounds as directed
xmin = PLAN_GXWX(plan, lx - plan_halfwidth);
ymin = PLAN_GYWY(plan, ly - plan_halfwidth);
xmax = PLAN_GXWX(plan, lx + plan_halfwidth);
ymax = PLAN_GYWY(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);
// Reset path marks (TODO: find a smarter place to do this)
cell = plan->cells;
for(i=0;i<plan->size_x*plan->size_y;i++,cell++)
cell->lpathmark = 0;
// 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;
cell->lpathmark = 1;
}
t1 = get_time();
//printf("computed local path: %.6lf\n", t1-t0);
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;
}
