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