case PropSonarReturn:      memcpy( &sonar_return, (bool*)value, sizeof(sonar_return) );      break;    case PropObstacleReturn:      memcpy( &obstacle_return, (bool*)value, sizeof(obstacle_return) );      break;    case PropVisionReturn:      memcpy( &vision_return, (bool*)value, sizeof(vision_return));      break;    case PropPuckReturn:      memcpy( &puck_return, (bool*)value, sizeof(puck_return) );      break;    default:      //printf( "Stage Warning: attempting to set unknown property %d\n",       //      property );      break;  }    // indicate that the property is dirty on all _but_ the connection  // it came from - that way it gets propogated onto to other clients  // and everyone stays in sync. (assuming no recursive connections...)    this->SetDirty( property, 1 ); // dirty on all cons    if( con != -1 ) // unless this was a local change     this->SetDirty( con, property, 0 ); // clean on this con  if( refresh_figure )    {#ifdef INCLUDE_RTK2       if( this->fig )	{	  RtkShutdown();	  RtkStartup();	}#endif    }    if( move_figure )    {      //printf( "-- moving figure to %.2f %.2f %.2f\n",      //      local_px, local_py, local_pth );#ifdef INCLUDE_RTK2          if( this->fig )	rtk_fig_origin(this->fig, local_px, local_py, local_pth );#endif             //#ifdef USE_GNOME2       //this->GuiMove();      //#endif    }  // update the GUI with the new property  if( m_world->enable_gui )    GuiEntityPropertyChange( this, property );    return 0;}int CEntity::GetProperty( EntityProperty property, void* value ){  //PRINT_DEBUG1( "finding property %d", property );  //printf( "finding property %d", property );  assert( value );  // indicate no data - this should be overridden below  int retval = 0;  switch( property )  {    case PropParent:      // TODO - fix      // find the parent's position in the world's entity array      // if parent pointer is null or otherwise invalid, index is -1       //{ int parent_index = m_world->GetEntityIndex( m_parent_entity );      { 	int parent_index = -1;	if( m_parent_entity )	  parent_index = m_parent_entity->stage_id ;		memcpy( value, &parent_index, sizeof(parent_index) );	retval = sizeof(parent_index);       }    break;    case PropSizeX:      memcpy( value, &size_x, sizeof(size_x) );      retval = sizeof(size_x);      break;    case PropSizeY:      memcpy( value, &size_y, sizeof(size_y) );      retval = sizeof(size_y);      break;    case PropPoseX:      memcpy( value, &local_px, sizeof(local_px) );      retval = sizeof(local_px);      break;    case PropPoseY:      memcpy( value, &local_py, sizeof(local_py) );      retval = sizeof(local_py);      break;    case PropPoseTh:      memcpy( value, &local_pth, sizeof(local_pth) );      retval = sizeof(local_pth);      break;    case PropOriginX:      memcpy( value, &origin_x, sizeof(origin_x) );      retval = sizeof(origin_x);      break;      break;    case PropOriginY:      memcpy( value, &origin_y, sizeof(origin_y) );      retval = sizeof(origin_y);      break;    case PropName:      strcpy( (char*)value, name );      retval = strlen(name);      break;    case PropColor:      memcpy( value, &color, sizeof(color) );      retval = sizeof(color);      break;    case PropShape:      memcpy( value, &shape, sizeof(shape) );      retval = sizeof(shape);      break;    case PropLaserReturn:      memcpy( value, &laser_return, sizeof(laser_return) );      retval = sizeof(laser_return);      break;    case PropSonarReturn:      memcpy( value, &sonar_return, sizeof(sonar_return) );      retval = sizeof(sonar_return);      break;    case PropIdarReturn:      memcpy( value, &idar_return, sizeof(idar_return) );      retval = sizeof(idar_return);      break;    case PropObstacleReturn:      memcpy( value, &obstacle_return, sizeof(obstacle_return) );      retval = sizeof(obstacle_return);      break;    case PropVisionReturn:      memcpy( value, &vision_return, sizeof(vision_return) );      retval = sizeof(vision_return);      break;    case PropPuckReturn:      memcpy( value, &puck_return, sizeof(puck_return) );      retval = sizeof(puck_return);      break;    default:      // printf( "Stage Warning: attempting to get unknown property %d\n",       //      property );    break;  }  return retval;}// write the entity tree onto the consolevoid CEntity::Print( char* prefix ){  double ox, oy, oth;  this->GetGlobalPose( ox, oy, oth );  printf( "%s type: %s global: [%.2f,%.2f,%.2f]"	  " local: [%.2f,%.2f,%.2f] vision_return %d )", 	  prefix,	  this->lib_entry->token,	  ox, oy, oth,	  local_px, local_py, local_pth,	  this->vision_return );	    if( this->m_parent_entity == NULL )    puts( " - ROOT" );  else    puts( "" );  // add an indent to the prefix    char* buf = new char[ strlen(prefix) + 1 ];  sprintf( buf, "\t%s", prefix );  CHILDLOOP( ch )    ch->Print( buf );}// subscribe to / unsubscribe from the device// these don't do anything by default, but are overridden by CPlayerEntityvoid CEntity::Subscribe(){   //puts( "SUB" );};void CEntity::Unsubscribe(){   //puts( "UNSUB" );};  int CEntity::Subscribed(){   return 0;};// these versions sub/unsub to this device and all its decendantsvoid CEntity::FamilySubscribe(){   CHILDLOOP( ch ) ch->FamilySubscribe(); };void CEntity::FamilyUnsubscribe(){   CHILDLOOP( ch ) ch->FamilyUnsubscribe(); };void CEntity::GuiStartup( void ){  // use the interface library hook  GuiEntityStartup( this );    CHILDLOOP( ch )    ch->GuiStartup();}void CEntity::GetStatusString( char* buf, int buflen ){  double x, y, th;  this->GetGlobalPose( x, y, th );    // check for overflow  assert( -1 != 	  snprintf( buf, buflen, 		    "Pose(%.2f,%.2f,%.2f) Stage(%d:%d(%s))",		    x, y, th, 		    this->stage_id,		    this->lib_entry->type_num,		    this->lib_entry->token ) );}  #ifdef INCLUDE_RTK2///////////////////////////////////////////////////////////////////////////// Initialise the rtk guivoid CEntity::RtkStartup(){  assert( m_world );  PRINT_DEBUG2("RTK STARTUP %s %s",               this->lib_entry->token,               m_parent_entity ? "" : " - ROOT" );  // Create a figure representing this entity  if( m_parent_entity == NULL )    this->fig = rtk_fig_create(m_world->canvas, NULL, 50);  else    this->fig = rtk_fig_create(m_world->canvas, m_parent_entity->fig, 50);  assert( this->fig );  /* REMOVE  this->fig->thing = (void*)this;  this->fig->origin_callback = staticSetGlobalPose;  this->fig->select_callback = NULL;  this->fig->unselect_callback = NULL;  */  // Set the mouse handler  this->fig->userdata = this;  rtk_fig_add_mouse_handler(this->fig, StaticRtkOnMouse);  // add this device to the world's device menu   this->m_world->AddToDeviceMenu( this, true);       // visible by default  rtk_fig_show( this->fig, true );  // Set the color  rtk_fig_color_rgb32(this->fig, this->color);  // put the figure's origin at the entity's position  rtk_fig_origin( this->fig, local_px, local_py, local_pth );#ifdef RENDER_INITIAL_BOUNDING_BOXES  double xmin, ymin, xmax, ymax;  xmin = ymin = 999999.9;  xmax = ymax = 0.0;  this->GetBoundingBox( xmin, ymin, xmax, ymax );    rtk_fig_t* boundaries = rtk_fig_create( m_world->canvas, NULL, 99);  double width = xmax - xmin;  double height = ymax - ymin;  double xcenter = xmin + width/2.0;  double ycenter = ymin + height/2.0;  rtk_fig_rectangle( boundaries, xcenter, ycenter, 0, width, height, 0 );    #endif     // draw the shape using the center of rotation offsets  switch (this->shape)  {    case ShapeRect:      rtk_fig_rectangle(this->fig,                         this->origin_x, this->origin_y, 0,                         this->size_x, this->size_y, false);      break;    case ShapeCircle:      rtk_fig_ellipse(this->fig,                       this->origin_x, this->origin_y, 0,                        this->size_x, this->size_y, false);      break;    case ShapeNone: // no shape      break;  }    // everything except the root object has a label  if( m_parent_entity )  {    // Create the label    // By default, the label is not shown    this->fig_label = rtk_fig_create(m_world->canvas, this->fig, 51);    rtk_fig_show(this->fig_label, false);        rtk_fig_movemask(this->fig_label, 0);          char label[1024];    char tmp[1024];          label[0] = 0;    snprintf(tmp, sizeof(tmp), "%s %s",              this->name, this->lib_entry->token );    strncat(label, tmp, sizeof(label));          rtk_fig_color_rgb32(this->fig, this->color);    rtk_fig_text(this->fig_label,  0.75 * size_x,  0.75 * size_y, 0, label);          // attach the label to the main figure    // rtk will draw the label when the mouse goes over the figure    // TODO: FIX    //this->fig->mouseover_fig = fig_label;          // we can be moved only if we are on the root node    if (m_parent_entity != this->m_world->GetRoot() )      rtk_fig_movemask(this->fig, 0);    else      rtk_fig_movemask(this->fig, this->movemask);    }  // do our children after we're set  CHILDLOOP( child ) child->RtkStartup();  PRINT_DEBUG( "RTK STARTUP DONE" );}///////////////////////////////////////////////////////////////////////////// Finalise the rtk guivoid CEntity::RtkShutdown(){  // Clean up the figure we created  rtk_fig_destroy(this->fig);  rtk_fig_destroy(this->fig_label);} ///////////////////////////////////////////////////////////////////////////// Update the rtk guivoid CEntity::RtkUpdate(){  CHILDLOOP( child ) child->RtkUpdate();  // TODO this is nasty and inefficient - figure out a better way to  // do this    // if we're not looking at this device, hide it   if( !m_world->ShowDeviceBody( this->lib_entry->type_num ) )  {    rtk_fig_show(this->fig, false);  }  else // we need to show and update this figure  {    rtk_fig_show( this->fig, true );  }}///////////////////////////////////////////////////////////////////////////// Process mouse eventsvoid CEntity::RtkOnMouse(rtk_fig_t *fig, int event, int mode){  double px, py, pth;  switch (event)  {    case RTK_EVENT_PRESS:    case RTK_EVENT_MOTION:    case RTK_EVENT_RELEASE:      rtk_fig_get_origin(fig, &px, &py, &pth);      this->SetGlobalPose(px, py, pth);      break;    default:      break;  }  return;}///////////////////////////////////////////////////////////////////////////// Process mouse events (static callback)void CEntity::StaticRtkOnMouse(rtk_fig_t *fig, int event, int mode){  CEntity *entity;  entity = (CEntity*) fig->userdata;  entity->RtkOnMouse(fig, event, mode);  return;}#endif