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