// also reflectors.		  //break;		  		case IDARReflect: // it's a reflector (ie. an obstacle)		  //printf( "REFLECT from %p type %s idar_return %d\n",		  //	ent, ent->token, ent->idar_return );					  // try poking this message into my own receiver		  if( (intensity = LookupIntensity(0,range,true)) > 0 )		    ReceiveMessage( (CEntity*)this, 				    transmit->mesg,				    transmit->len,				    intensity, 				    true );		  break;					default:		  printf( "STAGE WARNING: UNKNOWN IDAR VALUE %d\n", 			  ent->idar_return );		}		  	      break; // out of the while loop because we hit something	    }	}#ifdef INCLUDE_RTK2#ifdef RENDER_SCANLINES      rtk_fig_color_rgb32(this->rays_fig, RGB(220,220,220) );	      rtk_fig_arrow(this->rays_fig, 0,0, scanline_bearing-oth, range, 0.03);#endif#endif        }}bool CIdarDevice::ReceiveMessage( CEntity* sender,				  unsigned char* mesg, int len, 				  uint8_t intensity,				  bool reflection ){  //PRINT_DEBUG( "RECEIVE MESSAGE" );  //printf( "mesg recv - sensor: %d  len: %d  intensity: %d  refl: %d\n",  //  sensor, len, intensity, reflection );  // TEST INTENSITY  // we only accept this message if it's the most intense thing we've seen  if( intensity < recv.intensity )    return false;    // TEST REFLECTION AND  ANGLE OF INCIDENCE  // we accept reflections without checking angle  if( !reflection )     { // it's not a reflection, so we check the angle of incidence       double sx, sy, sa; // the sender's pose      double lx, ly, la; // my pose            sender->GetGlobalPose( sx, sy, sa );      this->GetGlobalPose( lx, ly, la );            double incidence = atan2( sy-ly, sx-lx ) - la;      NORMALIZE(incidence);            //printf( "%p G: %.2f,%.2f,%.2f   L: %.2f,%.2f,%.2f inc: %.2f\n",       //      this, sx, sy, sa, lx, ly, la, incidence );            // if it's out of our receptive angle, we reject it and bail.      if( fabs(incidence) > IDAR_RECEIVE_ANGLE/2.0 )	return false;    }      // looks good  - accept the message  // copy the message into the data buffer  memcpy( &recv.mesg, mesg, len );    recv.len = len;  recv.intensity = intensity;  recv.reflection = (uint8_t)reflection;    // record the time we received this message  recv.timestamp_sec = m_world->m_sim_timeval.tv_sec;  recv.timestamp_usec = m_world->m_sim_timeval.tv_usec;  #ifdef INCLUDE_RTK2        // reflections are green, remote messages are red  rtk_fig_color_rgb32(this->data_fig, 		      reflection ? RGB(0,200,0) : RGB(200,0,0) );    rtk_fig_arrow(this->data_fig, 0,0,0, 1.5*size_x, size_y );  #ifdef SHOW_MSGS    // room for the message in hex text  char message[ 3 * IDARBUFLEN + 6];    // print the message in hex, with a space between each char  for( int c=0; c<recv.len; c++ )    sprintf( message + 3*c, "%2X ", recv.mesg[c] );  message[ 3 * recv.len ] = 0; // terminate    // add the intensity  sprintf( message, "%s (%d)", message, recv.intensity );      rtk_fig_text(this->data_fig, 0,0,0, message);#endif#endif  return true; // we accepted the message}// HERE ARE A BUNCH OF RANGE/INTENSITY CONVERSION TABLES DETERMINED// EMPIRICALLY BY EXPERIMENTS WITH THE PHERBOTS. WE USE ONE OF THESE// FOR ROBOT-TO-ROBOT TRANSMISSIONS AND ONE FOR REFLECTIONS// ranges are in INCHES// (BASED ON 3g32)int reflection_intensity[] = { 150,129,117,115,112,104,103,102,101,100};int reflection_range[]     = {   0,  1,  7, 10, 14, 26, 28, 31, 35, 39, 99};int reflection_len = 10;// (BASED ON HERE_I_AM)int direct_intensity[] = { 130,126,125,123,122,121,120,119,118,117,115,114,			   112,112,111,109,108,107,106,105,104,103,101, 97,			   96, 95, 94, 93, 90 };int direct_range[]     = {   6,  9, 12, 15, 18, 21, 24, 28, 32, 36, 40, 44, 			     48, 52, 56, 60, 64, 68, 72, 76, 80, 84, 88, 92, 			     96,100,104,108,120,1200};int direct_len = 30;// RTV - reverse lookup distance to intensityuint8_t CIdarDevice::LookupIntensity( uint8_t transmit_intensity,				      double trans_range, 				      bool reflection ){  // transmit_intensity isn't used for now  // subtract our radius from the range reading  trans_range -= size_x / 2.0;  // convert range from m to inches (yuk!)  int irange = (int)(trans_range * 39.37); // m to inches     uint8_t result = 255; // make sure we got a result  int* dist_table = 0;  int* sigi_table = 0;  int table_len = 0;  // choose the tables to work from  if( reflection )    {      dist_table = reflection_range;      sigi_table = reflection_intensity;      table_len = reflection_len;    }  else    {      dist_table = direct_range;      sigi_table = direct_intensity;      table_len = direct_len;    }        // now do the lookup  // if we're below the minimum distance, return the maximum intensity  if( irange < dist_table[0] )    {      //printf( "vrange %d below minimum %d\n", vrange, dist_table[0] );      result = sigi_table[0];    }  // if we're above the max distance, return zero intensity  else if( irange > dist_table[ table_len-1 ] )    {      //printf( "irange %d is above maximum %d %s\n",       //      irange, dist_table[ table_len-1],      //      reflection ? "reflect" : "direct" );            result = 0;//sigi_table[ table_len-1 ];    }  // otherwise, interpolate a result from the table  else    for (int j = 1; j < table_len; j++)	      {	if( irange == dist_table[j] )	  {	    result = sigi_table[j];	    break;	  }	else if (irange < dist_table[j])		  {	    //printf( "irange %d is between %d and %d %s\n",	    //    irange, dist_table[j-1], dist_table[j],	    //    reflection ? "reflect" : "direct" );	    	    //# we're in the right range; calc ans;	    result =  (uint8_t)((irange - dist_table[j-1])				* (sigi_table[j] - sigi_table[j-1])				/ (dist_table[j] - dist_table[j-1])				+ sigi_table[j-1] );	    break;	  }      }      assert( result != 255 );    if( result > 200 )    printf( "WARNING: intensity suspicious!" );    //printf( "RANGE: %umm %u' %uv  INT: %u\n",   //  trans_range, irange, vrange, result );    return result;} #ifdef INCLUDE_RTK2///////////////////////////////////////////////////////////////////////////// Initialise the rtk guivoid CIdarDevice::RtkStartup(){  CPlayerEntity::RtkStartup();    // Create a figure representing this object  this->data_fig = rtk_fig_create(m_world->canvas, this->fig, 49);  this->rays_fig = rtk_fig_create(m_world->canvas, this->fig, 48);    rtk_fig_origin(this->data_fig, 0,0,0 );  rtk_fig_origin(this->rays_fig, 0,0,0 );   // show our orientation in the body figure  rtk_fig_line( this->fig, 0,0, size_x/2.0, 0);  // Set the color  rtk_fig_color_rgb32(this->data_fig, this->color);  rtk_fig_color_rgb32(this->rays_fig, RGB(200,200,200) );  #ifdef RENDER_INDEX  // render our index number  char buf[16];  sprintf( buf,"%d", m_player.index );  rtk_fig_text(this->fig, 2.0 * size_x, 0, 0, buf );#endif}///////////////////////////////////////////////////////////////////////////// Finalise the rtk guivoid CIdarDevice::RtkShutdown(){  // Clean up the figure we created  if(this->data_fig) rtk_fig_destroy(this->data_fig);  if(this->rays_fig) rtk_fig_destroy(this->rays_fig);    CPlayerEntity::RtkShutdown();} ///////////////////////////////////////////////////////////////////////////// Update the rtk guivoid CIdarDevice::RtkUpdate(){  CPlayerEntity::RtkUpdate();     // Get global pose  //double gx, gy, gth;  //GetGlobalPose(gx, gy, gth);  //rtk_fig_origin(this->data_fig, gx, gy, gth );  //  if( m_world->ShowDeviceData( this->type_num) )  //{  //  rtk_fig_show( this->data_fig, true );  //  rtk_fig_show( this->rays_fig, true );  // }  //else  //{  //  rtk_fig_show( this->data_fig, false );  //  rtk_fig_show( this->rays_fig, false );  // }    //if( Subscribed() < 1 )  //{  //rtk_fig_clear( this->rays_fig );  //rtk_fig_clear( this->data_fig );  // }}#endif