void inflateBlacklist(GridMap& map, std::set<tf::Vector3> positions, int radius) { CHECK_RADIUS(radius); static GridMask* grid_mask = NULL; static int last_radius = -1; if (!grid_mask || radius != last_radius) { delete grid_mask; grid_mask = makeCircularMask(radius); last_radius = radius; } ApplyMaskOperator op = [](int8_t cell, int8_t mask_cell) { // If the cell isn't unknown, set it as unreachable. if (cell == GridMap::UNKNOWN) return cell; else return std::max(cell, mask_cell); }; for (tf::Vector3 pos : positions) { index_t idx = map.getIdxForPosition(pos); ROS_WARN_STREAM("Applying mask at idx=" << idx); apply_mask_at_idx(map, *grid_mask, op, idx); } }
void nas_start(nas_t *nas) { nas_sta_t *sta; /* reset all STAs */ bzero((void *)nas->sta, sizeof(nas->sta)); bzero((void *)nas->sta_hashed, sizeof(nas->sta_hashed)); nas->MIC_failures = 0; nas->MIC_countermeasures = 0; nas->prev_MIC_error = 0; /* start session count down timer */ if (CHECK_RADIUS(nas->mode)) nas_start_watchdog(nas); /* initiate/request pairwise key exchange */ if (!(nas->flags & NAS_FLAG_WDS)) return; sta = lookup_sta(nas, (struct ether_addr *)nas->remote, SEARCH_ENTER); if (!sta) { #ifdef BCMDBG char eabuf[ETHER_ADDR_STR_LEN]; #endif dbg(nas, "sta %s not available", ether_etoa(nas->remote, eabuf)); return; } /* Assume the peer use the same cipher and akm */ if (CHECK_PSK(nas->mode)) { sta->suppl.pmk_len = nas->wpa->pmk_len; bcopy(nas->wpa->pmk, sta->suppl.pmk, nas->wpa->pmk_len); } /* * There is no beacon/proberesp/assocreq across WDS, therefore * we have no way to know what cipher and akm the peer is configured * for. But assuming the peer uses the same configuration seems * reasonable. No WEP when doing WPA over WDS. */ wpa_set_suppl(nas->wpa, sta, nas->mode, nas->wsec, CRYPTO_ALGO_OFF); #ifdef BCMSUPPL if (nas->flags & NAS_FLAG_SUPPLICANT) wpa_request(nas->wpa, sta); #endif if (nas->flags & NAS_FLAG_AUTHENTICATOR) wpa_start(nas->wpa, sta); }
void freeRobotPose(GridMap& map, index_t idx, int radius) { CHECK_RADIUS(radius); static GridMask* grid_mask = NULL; static int last_radius = -1; if (!grid_mask || radius != last_radius) { delete grid_mask; grid_mask = makeCircularMask(radius); last_radius = radius; } ApplyMaskOperator op = [](int8_t cell, int8_t mask_cell) { // Mark the cell as known & free return 0; }; apply_mask_at_idx(map, *grid_mask, op, idx); }
void inflateObstacles(GridMap& map, int radius, int8_t value) { CHECK_RADIUS(radius); static GridMask* grid_mask = NULL; static int last_radius = -1; if (!grid_mask || radius != last_radius) { delete grid_mask; grid_mask = makeCircularMask(radius, value); last_radius = radius; } ApplyMaskOperator op = [](int8_t cell, int8_t mask_cell) { return std::max(cell, mask_cell); }; GridCellSelectorFunction pred = boost::bind(&GridMap::isObstacle, map, _2); apply_mask_if(map, *grid_mask, op, pred); }
static Bool initlisa(ModeInfo * mi, lisas * loop) { lisacons *lc = &Lisa[MI_SCREEN(mi)]; lisafuncs **lf = loop->function; XPoint *lp; int phase, pctr, fctr, xctr, yctr, extra_points; double xprod, yprod, xsum, ysum; xMaxLines = (XMaxRequestSize(MI_DISPLAY(mi))-3)/2; /* printf("Got xMaxLines = %d\n", xMaxLines); */ loop->nsteps = MI_CYCLES(mi); if (loop->nsteps == 0) loop->nsteps = 1; if (MI_NPIXELS(mi) > 2) { loop->color = STARTCOLOR; loop->cstep = (loop->nsteps > MI_NPIXELS(mi)) ? loop->nsteps / MI_NPIXELS(mi) : 1; } else { loop->color = MI_WHITE_PIXEL(mi); loop->cstep = 0; } extra_points = loop->cstep - (loop->nsteps % loop->cstep); lc->maxcycles = (MAXCYCLES * loop->nsteps) - 1; loop->cstep = ( loop->nsteps > MI_NPIXELS(mi) ) ? loop->nsteps / MI_NPIXELS(mi) : 1; /* printf("Got cstep = %d\n", loop->cstep); */ loop->melting = 0; loop->nfuncs = 1; loop->pistep = 2.0 * M_PI / (double) loop->nsteps; loop->center.x = lc->width / 2; loop->center.y = lc->height / 2; loop->radius = (int) MI_SIZE(mi); CHECK_RADIUS(loop, lc); loop->dx = NRAND(XVMAX); loop->dy = NRAND(YVMAX); loop->dx++; loop->dy++; #if defined STARTFUNC lf[0] = &Function[STARTFUNC]; #else /* STARTFUNC */ lf[0] = &Function[NRAND(NUMSTDFUNCS)]; #endif /* STARTFUNC */ if ((lp = loop->lastpoint = (XPoint *) calloc(loop->nsteps+extra_points, sizeof (XPoint))) == NULL) { free_lisa(lc); return False; } phase = lc->loopcount % loop->nsteps; #if defined DEBUG printf( "nsteps = %d\tcstep = %d\tmrs = %d\textra_points = %d\n", loop->nsteps, loop->cstep, xMaxLines, extra_points ); PRINT_FUNC(lf[0]); #endif /* DEBUG */ for (pctr = 0; pctr < loop->nsteps; pctr++) { loop->phi = (double) (pctr - phase) * loop->pistep; loop->theta = (double) (pctr + phase) * loop->pistep; fctr = loop->nfuncs; xsum = ysum = 0.0; while (fctr--) { xprod = yprod = (double) loop->radius; xctr = lf[fctr]->nx; yctr = lf[fctr]->ny; while (xctr--) xprod *= sin(lf[fctr]->xcoeff[xctr] * loop->theta); while (yctr--) yprod *= sin(lf[fctr]->ycoeff[yctr] * loop->phi); xsum += xprod; ysum += yprod; } if (loop->nfuncs > 1) { xsum /= 2.0; ysum /= 2.0; } xsum += (double) loop->center.x; ysum += (double) loop->center.y; lp[pctr].x = (int) ceil(xsum); lp[pctr].y = (int) ceil(ysum); } /* this fills in the extra points, so we can use segment-drawing calls */ for (pctr = loop->nsteps; pctr < loop->nsteps + extra_points; pctr++) { lp[pctr].x=lp[pctr - loop->nsteps].x; lp[pctr].y=lp[pctr - loop->nsteps].y; } #if defined DRAWLINES loop->linewidth = LINEWIDTH; /* #### make this a resource */ if (loop->linewidth == 0) loop->linewidth = 1; if (loop->linewidth < 0) loop->linewidth = NRAND(-loop->linewidth) + 1; XSetLineAttributes(MI_DISPLAY(mi), MI_GC(mi), loop->linewidth, LINESTYLE, LINECAP, LINEJOIN); #endif /* DRAWLINES */ if ( loop->cstep < xMaxLines ) { /* we can send each color segment in a single request * because the max request length is long enough * and because we have padded out the array to have extra elements * to support calls which would otherwise fall off the end*/ for (pctr = 0; pctr < loop->nsteps; pctr+=loop->cstep) { /* Set the color */ SET_COLOR(); #if defined DRAWLINES XDrawLines(MI_DISPLAY(mi), MI_WINDOW(mi), MI_GC(mi), &lp[pctr], loop->cstep, CoordModeOrigin ); #else /* DRAWLINES */ XDrawPoints(MI_DISPLAY(mi), MI_WINDOW(mi), MI_GC(mi), &lp[pctr], loop->cstep, CoordModeOrigin ); #endif /* DRAWLINES */ } } else { /* do it one by one as before */ for (pctr = 0; pctr < loop->nsteps; pctr++ ) { SET_COLOR(); #if defined DRAWLINES XDrawLine(MI_DISPLAY(mi), MI_WINDOW(mi), MI_GC(mi), lp[pctr].x, lp[pctr].y, lp[pctr+1 % loop->nsteps].x, lp[pctr+1 % loop->nsteps].y); #else /* DRAWLINES */ XDrawPoint(MI_DISPLAY(mi), MI_WINDOW(mi), MI_GC(mi), lp[pctr].x, lp[pctr].y); #endif /* DRAWLINES */ } } #if defined DRAWLINES XSetLineAttributes(MI_DISPLAY(mi), MI_GC(mi), 1, LINESTYLE, LINECAP, LINEJOIN); #endif /* DRAWLINES */ return True; }
static Bool drawlisa(ModeInfo * mi, lisas * loop) { XPoint *np; XPoint *lp = loop->lastpoint; lisacons *lc = &Lisa[MI_SCREEN(mi)]; lisafuncs **lf = loop->function; int phase = lc->loopcount % loop->nsteps; int pctr, fctr, xctr, yctr, extra_points; double xprod, yprod, xsum, ysum; /* why carry this around in the struct when we can calculate it on demand? */ extra_points = loop->cstep - (loop->nsteps % loop->cstep); /* Allocate the np (new point) array (with padding) */ if ((np = (XPoint *) calloc(loop->nsteps+extra_points, sizeof (XPoint))) == NULL) { free_lisa(lc); return False; } /* Update the center */ loop->center.x += loop->dx; loop->center.y += loop->dy; CHECK_RADIUS(loop, lc); /* check for overlaps -- where the figure might go off the screen */ if ((loop->center.x - loop->radius) <= 0) { loop->center.x = loop->radius; loop->dx = NRAND(XVMAX); } else if ((loop->center.x + loop->radius) >= lc->width) { loop->center.x = lc->width - loop->radius; loop->dx = -NRAND(XVMAX); }; if ((loop->center.y - loop->radius) <= 0) { loop->center.y = loop->radius; loop->dy = NRAND(YVMAX); } else if ((loop->center.y + loop->radius) >= lc->height) { loop->center.y = lc->height - loop->radius; loop->dy = -NRAND(YVMAX); }; /* Now draw the points, and erase the ones from the last cycle */ for (pctr = 0; pctr < loop->nsteps; pctr++) { fctr = loop->nfuncs; loop->phi = (double) (pctr - phase) * loop->pistep; loop->theta = (double) (pctr + phase) * loop->pistep; xsum = ysum = 0; while (fctr--) { xctr = lf[fctr]->nx; yctr = lf[fctr]->ny; if (additive) { xprod = yprod = 0.0; while (xctr--) xprod += sin(lf[fctr]->xcoeff[xctr] * loop->theta); while (yctr--) yprod += sin(lf[fctr]->ycoeff[yctr] * loop->phi); if (loop->melting) { if (fctr) { xsum += xprod * (double) (loop->nsteps - loop->melting) / (double) loop->nsteps; ysum += yprod * (double) (loop->nsteps - loop->melting) / (double) loop->nsteps; } else { xsum += xprod * (double) loop->melting / (double) loop->nsteps; ysum += yprod * (double) loop->melting / (double) loop->nsteps; } } else { xsum = xprod; ysum = yprod; } if (!fctr) { xsum = xsum * (double) loop->radius / (double) lf[fctr]->nx; ysum = ysum * (double) loop->radius / (double) lf[fctr]->ny; } } else { if (loop->melting) { if (fctr) { yprod = xprod = (double) loop->radius * (double) (loop->nsteps - loop->melting) / (double) (loop->nsteps); } else { yprod = xprod = (double) loop->radius * (double) (loop->melting) / (double) (loop->nsteps); } } else { xprod = yprod = (double) loop->radius; } while (xctr--) xprod *= sin(lf[fctr]->xcoeff[xctr] * loop->theta); while (yctr--) yprod *= sin(lf[fctr]->ycoeff[yctr] * loop->phi); xsum += xprod; ysum += yprod; } } if ((loop->nfuncs > 1) && (!loop->melting)) { xsum /= (double) loop->nfuncs; ysum /= (double) loop->nfuncs; } xsum += (double) loop->center.x; ysum += (double) loop->center.y; np[pctr].x = (int) ceil(xsum); np[pctr].y = (int) ceil(ysum); } /* fill in extra points */ for (pctr=loop->nsteps; pctr < loop->nsteps+extra_points; pctr++) { np[pctr].x = np[pctr - loop->nsteps].x; np[pctr].y = np[pctr - loop->nsteps].y; } if (loop->melting) { if (!--loop->melting) { loop->nfuncs = 1; loop->function[0] = loop->function[1]; } } /* reset starting color each time to prevent ass-like appearance */ loop->color = STARTCOLOR; if (loop->cstep < xMaxLines) { /* printf("Drawing dashes\n"); */ for (pctr = 0; pctr < loop->nsteps; pctr+=loop->cstep) { #if defined DRAWLINES XSetLineAttributes(MI_DISPLAY(mi), MI_GC(mi), loop->linewidth, LINESTYLE, LINECAP, LINEJOIN); /* erase the last cycle's point */ XSetForeground(MI_DISPLAY(mi), MI_GC(mi), MI_BLACK_PIXEL(mi)); XDrawLines(MI_DISPLAY(mi), MI_WINDOW(mi), MI_GC(mi), &lp[pctr], loop->cstep, CoordModeOrigin); /* Set the new color */ SET_COLOR(); /* plot this cycle's point */ XDrawLines(MI_DISPLAY(mi), MI_WINDOW(mi), MI_GC(mi), &np[pctr], loop->cstep, CoordModeOrigin); XSetLineAttributes(MI_DISPLAY(mi), MI_GC(mi), 1, LINESTYLE, LINECAP, LINEJOIN); #else /* erase the last cycle's point */ XSetForeground(MI_DISPLAY(mi), MI_GC(mi), MI_BLACK_PIXEL(mi)); XDrawPoints(MI_DISPLAY(mi), MI_WINDOW(mi), MI_GC(mi), &lp[pctr], loop->cstep, CoordModeOrigin); /* Set the new color */ SET_COLOR(); /* plot this cycle's point */ XDrawPoints(MI_DISPLAY(mi), MI_WINDOW(mi), MI_GC(mi), &np[pctr], loop->cstep, CoordModeOrigin); #endif } } else { /* on my system, cstep is larger than 65532/2 if we get here */ for (pctr = 0; pctr < loop->nsteps; pctr++) { #if defined DRAWLINES XSetLineAttributes(MI_DISPLAY(mi), MI_GC(mi), loop->linewidth, LINESTYLE, LINECAP, LINEJOIN); /* erase the last cycle's point */ XSetForeground(MI_DISPLAY(mi), MI_GC(mi), MI_BLACK_PIXEL(mi)); XDrawLine(MI_DISPLAY(mi), MI_WINDOW(mi), MI_GC(mi), lp[pctr].x, lp[pctr].y, lp[(pctr + 1) % loop->nsteps].x, lp[(pctr + 1) % loop->nsteps].y); /* Set the new color */ SET_COLOR(); /* plot this cycle's point */ XDrawLine(MI_DISPLAY(mi), MI_WINDOW(mi), MI_GC(mi), np[pctr].x, np[pctr].y, np[(pctr + 1) % loop->nsteps].x, np[(pctr + 1) % loop->nsteps].y); XSetLineAttributes(MI_DISPLAY(mi), MI_GC(mi), 1, LINESTYLE, LINECAP, LINEJOIN); #else /* DRAWLINES */ /* erase the last cycle's point */ XSetForeground(MI_DISPLAY(mi), MI_GC(mi), MI_BLACK_PIXEL(mi)); XDrawPoint(MI_DISPLAY(mi), MI_WINDOW(mi), MI_GC(mi), lp[pctr].x, lp[pctr].y); /* Set the new color */ SET_COLOR(); /* plot this cycle's point */ XDrawPoint(MI_DISPLAY(mi), MI_WINDOW(mi), MI_GC(mi), np[pctr].x, np[pctr].y); #endif /* DRAWLINES */ } } (void) free((void *) lp); loop->lastpoint = np; return True; }