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