// Compute the distance between the camera and the centroid float compute_distance_from_kinect(Eigen::Matrix<float, 4, 1> p_matrix) { pcl::PointXYZ camera_origin(0,0,0); pcl::PointXYZ object_position(p_matrix(0,0), p_matrix(1,0), p_matrix(3,0)); float distance = pcl::euclideanDistance(camera_origin, object_position); return distance; }
void Scene::draw_object(Object *o) { Vec3<float> object_position((o->modelview_matrix()).val[3], (o->modelview_matrix()).val[7], (o->modelview_matrix()).val[11]); Vec3<float> objminuscam = object_position - camera_pos; globalvalues->set_value(o->modelview_matrix(),"modelview"); globalvalues->set_value(o->projection_modelview_matrix(),"projection_modelview"); globalvalues->set_value(o->normal_matrix(),"normal_matrix"); disp->use_program(o->get_program()); o->draw(objminuscam.norm()); }
void sensor_sense(Sensor s) { Map m = s->map; mapVec pos=s->borig, sz=s->bsz; memset(s->vistiles, 0, sz.x*sz.y*sz.z*sizeof(perception)); map_get_visible_tiles(m, s->vistiles, s->volume, pos, sz); Stimulus vistiles = stimulus_init_tile_vis_change(stimulus_new(), s->vistiles, pos, sz); // TCOD_console_print_left(NULL, 0, 20, "p {%f, %f, %f}, s {%f, %f, %f}", pos.x, pos.y, pos.z, sz.x, sz.y, sz.z); TCOD_list_push(s->stimuli, vistiles); TCOD_list_t oldObjList = s->visObjects; //one ago s->visObjects = s->oldVisObjects; //two ago TCOD_list_clear(s->visObjects); s->oldVisObjects = oldObjList; map_get_visible_objects(m, s->visObjects, s->vistiles, pos, sz); //remove the old objects Object o; mapVec pt; int index; Stimulus visobj; for(int i = 0; i < TCOD_list_size(s->oldVisObjects); i++) { o = TCOD_list_get(s->oldVisObjects, i); if(!TCOD_list_contains(s->visObjects, o)) { //not visible anymore //this isn't quite right, really, using percept_none here visobj = stimulus_init_obj_vis_change(stimulus_new(), o, percept_none, object_context(o)); TCOD_list_push(s->stimuli, visobj); } } //add the new objects for(int i = 0; i < TCOD_list_size(s->visObjects); i++) { o = TCOD_list_get(s->visObjects, i); if(!TCOD_list_contains(s->oldVisObjects, o)) { //not visible before pt = object_position(o); index = tile_index(pt.x, pt.y, pt.z, map_size(m), pos, sz); visobj = stimulus_init_obj_vis_change(stimulus_new(), o, s->vistiles[index], object_context(o)); TCOD_list_push(s->stimuli, visobj); } } }
static SANE_Status do_cancel (Ibm_Scanner * s) { SANE_Status status; DBG (11, ">> do_cancel\n"); DBG (3, "cancel: sending OBJECT POSITION\n"); status = object_position (s->fd, OBJECT_POSITION_UNLOAD); if (status != SANE_STATUS_GOOD) { DBG (1, "cancel: OBJECT POSTITION failed\n"); } s->scanning = SANE_FALSE; if (s->fd >= 0) { sanei_scsi_close (s->fd); s->fd = -1; } DBG (11, "<< do_cancel\n"); return (SANE_STATUS_CANCELLED); }
static SANE_Status attach (const char *devnam, Ibm_Device ** devp) { SANE_Status status; Ibm_Device *dev; int fd; struct inquiry_data ibuf; struct measurements_units_page mup; struct ibm_window_data wbuf; size_t buf_size; char *str; DBG (11, ">> attach\n"); for (dev = first_dev; dev; dev = dev->next) { if (strcmp (dev->sane.name, devnam) == 0) { if (devp) *devp = dev; return (SANE_STATUS_GOOD); } } DBG (3, "attach: opening %s\n", devnam); status = sanei_scsi_open (devnam, &fd, NULL, NULL); if (status != SANE_STATUS_GOOD) { DBG (1, "attach: open failed: %s\n", sane_strstatus (status)); return (status); } DBG (3, "attach: sending INQUIRY\n"); memset (&ibuf, 0, sizeof (ibuf)); buf_size = sizeof(ibuf); /* next line by mf */ ibuf.byte2 = 2; status = inquiry (fd, &ibuf, &buf_size); if (status != SANE_STATUS_GOOD) { DBG (1, "attach: inquiry failed: %s\n", sane_strstatus (status)); sanei_scsi_close (fd); return (status); } if (ibuf.devtype != 6) { DBG (1, "attach: device \"%s\" is not a scanner\n", devnam); sanei_scsi_close (fd); return (SANE_STATUS_INVAL); } if (!( (strncmp ((char *)ibuf.vendor, "IBM", 3) ==0 && strncmp ((char *)ibuf.product, "2456", 4) == 0) || (strncmp ((char *)ibuf.vendor, "RICOH", 5) == 0 && strncmp ((char *)ibuf.product, "IS420", 5) == 0) || (strncmp ((char *)ibuf.vendor, "RICOH", 5) == 0 && strncmp ((char *)ibuf.product, "IS410", 5) == 0) || (strncmp ((char *)ibuf.vendor, "RICOH", 5) == 0 && strncmp ((char *)ibuf.product, "IS430", 5) == 0) )) { DBG (1, "attach: device \"%s\" doesn't look like a scanner I know\n", devnam); sanei_scsi_close (fd); return (SANE_STATUS_INVAL); } DBG (3, "attach: sending TEST_UNIT_READY\n"); status = test_unit_ready (fd); if (status != SANE_STATUS_GOOD) { DBG (1, "attach: test unit ready failed (%s)\n", sane_strstatus (status)); sanei_scsi_close (fd); return (status); } /* * Causes a problem with RICOH IS420 * Ignore this function ... seems to work ok * Suggested to George Murphy [email protected] by henning */ if (strncmp((char *)ibuf.vendor, "RICOH", 5) != 0 && strncmp((char *)ibuf.product, "IS420", 5) != 0) { DBG (3, "attach: sending OBJECT POSITION\n"); status = object_position (fd, OBJECT_POSITION_UNLOAD); if (status != SANE_STATUS_GOOD) { DBG (1, "attach: OBJECT POSTITION failed\n"); sanei_scsi_close (fd); return (SANE_STATUS_INVAL); } } memset (&mup, 0, sizeof (mup)); mup.page_code = MEASUREMENTS_PAGE; mup.parameter_length = 0x06; mup.bmu = INCHES; mup.mud[0] = (DEFAULT_MUD >> 8) & 0xff; mup.mud[1] = (DEFAULT_MUD & 0xff); #if 0 DBG (3, "attach: sending MODE SELECT\n"); status = mode_select (fd, (struct mode_pages *) &mup); if (status != SANE_STATUS_GOOD) { DBG (1, "attach: MODE_SELECT failed\n"); sanei_scsi_close (fd); return (SANE_STATUS_INVAL); } #endif #if 0 DBG (3, "attach: sending MODE SENSE\n"); memset (&mup, 0, sizeof (mup)); status = mode_sense (fd, (struct mode_pages *) &mup, PC_CURRENT | MEASUREMENTS_PAGE); if (status != SANE_STATUS_GOOD) { DBG (1, "attach: MODE_SENSE failed\n"); sanei_scsi_close (fd); return (SANE_STATUS_INVAL); } #endif DBG (3, "attach: sending GET WINDOW\n"); memset (&wbuf, 0, sizeof (wbuf)); status = get_window (fd, &wbuf); if (status != SANE_STATUS_GOOD) { DBG (1, "attach: GET_WINDOW failed %d\n", status); sanei_scsi_close (fd); DBG (11, "<< attach\n"); return (SANE_STATUS_INVAL); } sanei_scsi_close (fd); dev = malloc (sizeof (*dev)); if (!dev) return (SANE_STATUS_NO_MEM); memset (dev, 0, sizeof (*dev)); dev->sane.name = strdup (devnam); dev->sane.vendor = "IBM"; str = malloc (16 + 1); memset (str, 0, sizeof (str)); strncpy (str, (char *)ibuf.product, sizeof(ibuf.product)); strncpy (str + sizeof(ibuf.revision), (char *)ibuf.revision, sizeof(ibuf.revision)); str[sizeof(ibuf.product) + sizeof(ibuf.revision)] = '\0'; dev->sane.model = str; dev->sane.type = "flatbed scanner"; DBG (5, "dev->sane.name = %s\n", dev->sane.name); DBG (5, "dev->sane.vendor = %s\n", dev->sane.vendor); DBG (5, "dev->sane.model = %s\n", dev->sane.model); DBG (5, "dev->sane.type = %s\n", dev->sane.type); dev->info.xres_default = _2btol(wbuf.x_res); dev->info.yres_default = _2btol(wbuf.y_res); dev->info.image_mode_default = wbuf.image_comp; /* if you throw the MRIF bit the brighness control reverses too */ /* so I reverse the reversal in software for symmetry's sake */ /* I should make this into an option */ if (wbuf.image_comp == IBM_GRAYSCALE || wbuf.image_comp == IBM_DITHERED_MONOCHROME) { dev->info.brightness_default = 256 - wbuf.brightness; /* if (is50) dev->info.contrast_default = wbuf.contrast; else */ dev->info.contrast_default = 256 - wbuf.contrast; } else /* wbuf.image_comp == IBM_BINARY_MONOCHROME */ { dev->info.brightness_default = wbuf.brightness; dev->info.contrast_default = wbuf.contrast; } /* da rivedere dev->info.adf_default = wbuf.adf_state; */ dev->info.adf_default = ADF_UNUSED; dev->info.adf_default = IBM_PAPER_USER_DEFINED; #if 1 dev->info.bmu = mup.bmu; dev->info.mud = _2btol(mup.mud); if (dev->info.mud == 0) { /* The Ricoh says it uses points as default Basic Measurement Unit */ /* but gives a Measurement Unit Divisor of zero */ /* So, we set it to the default (SCSI-standard) of 1200 */ /* with BMU in inches, i.e. 1200 points equal 1 inch */ dev->info.bmu = INCHES; dev->info.mud = DEFAULT_MUD; } #else dev->info.bmu = INCHES; dev->info.mud = DEFAULT_MUD; #endif DBG (5, "xres_default=%d\n", dev->info.xres_default); DBG (5, "xres_range.max=%d\n", dev->info.xres_range.max); DBG (5, "xres_range.min=%d\n", dev->info.xres_range.min); DBG (5, "yres_default=%d\n", dev->info.yres_default); DBG (5, "yres_range.max=%d\n", dev->info.yres_range.max); DBG (5, "yres_range.min=%d\n", dev->info.yres_range.min); DBG (5, "x_range.max=%d\n", dev->info.x_range.max); DBG (5, "y_range.max=%d\n", dev->info.y_range.max); DBG (5, "image_mode=%d\n", dev->info.image_mode_default); DBG (5, "brightness=%d\n", dev->info.brightness_default); DBG (5, "contrast=%d\n", dev->info.contrast_default); DBG (5, "adf_state=%d\n", dev->info.adf_default); DBG (5, "bmu=%d\n", dev->info.bmu); DBG (5, "mud=%d\n", dev->info.mud); ++num_devices; dev->next = first_dev; first_dev = dev; if (devp) *devp = dev; DBG (11, "<< attach\n"); return (SANE_STATUS_GOOD); }
int main( int argc, char *argv[] ) { char *font="tilesense/libtcod/fonts/courier12x12_aa_tc.png"; int nb_char_horiz=0,nb_char_vertic=0; int font_flags=TCOD_FONT_TYPE_GREYSCALE|TCOD_FONT_LAYOUT_TCOD; TCOD_console_set_custom_font(font,font_flags,nb_char_horiz,nb_char_vertic); TCOD_console_init_root(80,40,"salamandeRL",false); Map room = initRoom(); Object player = initPlayer(room); object_sense(player); TCOD_sys_set_fps(30); float elapsed = 0; char finished = 0; TCOD_key_t key = {TCODK_NONE,0}; TCOD_console_set_foreground_color(NULL,TCOD_white); do { key = TCOD_console_check_for_keypress(TCOD_KEY_PRESSED); if(key.vk != TCODK_NONE) { TCOD_console_clear(NULL); } TCOD_console_print_right(NULL,79,26,TCOD_BKGND_NONE,"last frame : %3d ms (%3d fps)", (int)(TCOD_sys_get_last_frame_length()*1000), TCOD_sys_get_fps()); TCOD_console_print_right(NULL,79,27,TCOD_BKGND_NONE,"elapsed : %8dms %4.2fs", TCOD_sys_elapsed_milli(),TCOD_sys_elapsed_seconds()); TCOD_console_print_left(NULL,0,27,TCOD_BKGND_NONE,"other stat stuff can go here"); //map drawmap(room, player); TCOD_console_print_left(NULL, object_position(player).x*2, object_position(player).y, TCOD_BKGND_NONE,"@"); //divider TCOD_console_print_left(NULL,0,28,TCOD_BKGND_NONE, "--------------------------------------------------------------------------------" ); //text display TCOD_console_print_left(NULL,2,29,TCOD_BKGND_NONE,"we'll probably put text down here."); /* update the game screen */ TCOD_console_flush(); if(key.vk == TCODK_RIGHT) { map_turn_object(room, "@", 1); } else if(key.vk == TCODK_LEFT) { map_turn_object(room, "@", -1); } else if(key.vk == TCODK_CHAR) { switch(key.c) { case 'w': map_move_object(room, "@", (mapVec){0, -1, 0}); break; case 'a': map_move_object(room, "@", (mapVec){-1, 0, 0}); break; case 's': map_move_object(room, "@", (mapVec){0, 1, 0}); break; case 'd': map_move_object(room, "@", (mapVec){1, 0, 0}); break; case 'q': finished = 1; break; //next, handle chomping default: break; } } } while (!finished && !TCOD_console_is_window_closed()); return 0; }