// Get the ith waypoint; returns non-zero of there are no more waypoints
int plan_get_waypoint(plan_t *plan, int i, double *px, double *py)
{
if (i < 0 || i >= plan->waypoint_count)
return 0;
*px = PLAN_WXGX(plan, plan->waypoints[i]->ci);
*py = PLAN_WYGY(plan, plan->waypoints[i]->cj);
return 1;
}
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);
}
// 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;
}
// Convert given waypoint cell to global x,y
void plan_convert_waypoint(plan_t* plan,
plan_cell_t *waypoint, double *px, double *py)
{
*px = PLAN_WXGX(plan, waypoint->ci);
*py = PLAN_WYGY(plan, waypoint->cj);
}
