m_sim_timeval.tv_sec = (long)floor(m_sim_time);  m_sim_timeval.tv_usec = (long)((m_sim_time-floor(m_sim_time)) * MILLION);   return m_sim_time;}///////////////////////////////////////////////////////////////////////////// Get the sim time// Returns time in sec since simulation starteddouble CWorld::GetTime(){  return m_sim_time;}///////////////////////////////////////////////////////////////////////////// Get the real time// Returns time in sec since simulation starteddouble CWorld::GetRealTime(){  struct timeval tv;  gettimeofday( &tv, NULL );  double time = tv.tv_sec + (tv.tv_usec / 1000000.0);  return time - m_start_time;}///////////////////////////////////////////////////////////////////////////// Set a rectangle in the world gridvoid CWorld::SetRectangle(double px, double py, double pth,                          double dx, double dy, CEntity* ent, bool add){  Rect rect;  dx /= 2.0;  dy /= 2.0;  double cx = dx * cos(pth);  double cy = dy * cos(pth);  double sx = dx * sin(pth);  double sy = dy * sin(pth);      rect.toplx = (int) ((px + cx - sy) * ppm);  rect.toply = (int) ((py + sx + cy) * ppm);  rect.toprx = (int) ((px + cx + sy) * ppm);  rect.topry = (int) ((py + sx - cy) * ppm);  rect.botlx = (int) ((px - cx - sy) * ppm);  rect.botly = (int) ((py - sx + cy) * ppm);  rect.botrx = (int) ((px - cx + sy) * ppm);  rect.botry = (int) ((py - sx - cy) * ppm);      //printf( "draw_rect %d,%d %d,%d %d,%d %d,%d\n",  //  rect.toplx, rect.toply,  //  rect.toprx, rect.topry,  //  rect.botlx, rect.botly,  //  rect.botrx, rect.botry );  matrix->draw_rect( rect, ent, add );}///////////////////////////////////////////////////////////////////////////// Set a circle in the world gridvoid CWorld::SetCircle(double px, double py, double pr, CEntity* ent,                       bool add ){  // Convert from world to image coords  int x = (int) (px * ppm);  int y = (int) (py * ppm);  int r = (int) (pr * ppm);      matrix->draw_circle( x,y,r,ent, add);}///////////////////////////////////////////////////////////////////////////// Add an entity to the array// returns its array index which the entity uses as a unique IDstage_id_t CWorld::RegisterEntity( CEntity* ent ){  // if we have no space left in the array, allocate another chunk  if (this->entity_count >= this->entities_size)    {       this->entities_size += 100; // a good chunk - one realloc will do for most people      this->entities = (CEntity**)	realloc(this->entities, this->entities_size * sizeof(this->entities[0]));    }    stage_id_t id = entity_count++; // record the id BEFORE incrementing it    this->entities[id] = ent; // store the pointer    return id; // return the unique id/index}// return the entity with matching id/index, or NULL if no matchCEntity* CWorld::GetEntity( int id ){   // bounds check  if( id < 0 || id >= this->entity_count )    return NULL;    return( this->entities[id] );}// returns true if the given hostname matches our hostname, false otherwise//  bool CWorld::CheckHostname(char* host)//  {//    //printf( "checking %s against (%s and %s) ", //    //  host, m_hostname, m_hostname_short ); //    if(!strcmp(m_hostname,host) || !strcmp(m_hostname_short,host))//    {//      //PRINT_DEBUG( "TRUE" );//      return true;//    }//    else//    {//      //PRINT_DEBUG( "FALSE" );//      return false;//    }//  }void CWorld::Output(){  // comms used  static unsigned long last_input = 0;  static unsigned long last_output = 0;  unsigned int bytes_in = g_bytes_input - last_input;  unsigned int bytes_out = g_bytes_output - last_output;  static int bytes_accumulator = 0;    // count the data  bytes_accumulator += bytes_in + bytes_out;  // measure frequency & bandwidth  static double freq = 0.0;  static double bandw = 0.0;  static int updates = 0;  static double lasttime = GetRealTime();  double interval = GetRealTime() - lasttime;  // count this update  updates++;    if( interval > 2.0 ) // measure freq + bandwidth every 2 seconds    {      lasttime += interval;      bandw = (double)bytes_accumulator / interval;      bytes_accumulator = 0;            freq = (double)updates / interval;      updates = 0;        }    if( m_console_output )    ConsoleOutput( freq, bytes_in, bytes_out, bandw );      if( m_log_output )     LogOutput( freq, bytes_in, bytes_out, g_bytes_input, g_bytes_output );    last_input = g_bytes_input;  last_output = g_bytes_output; }void CWorld::ConsoleOutput( double freq, 			    unsigned int bytes_in, unsigned int bytes_out,			    double avg_data){  char lineend = '\r';  //char lineend = '\n';  printf( " Step: %u Time: %8.1f - %7.1fHz - [%4u/%4u] %8.2f b/sec%c", 	  m_step_num,	  m_sim_time,           freq,	  bytes_in, bytes_out,           avg_data,	  lineend );    fflush( stdout );  }bool CWorld::Load( void ){  ///////////////////////////////////////////////////////////////////////  // LOAD THE CONFIGURATION FOR THE GUI   // we call this *after* the world has loaded, so we can configure the menus  // correctly  PRINT_DEBUG( "WORLD LOAD" );  if(this->enable_gui ) GuiLoad( this );  else    PRINT_DEBUG( "NOT LOADING GUI" );    return true; // success}bool CWorld::Save( ){  if( this->enable_gui ) GuiSave( this );  return true; // success}void CWorld::LogOutput( double freq,			unsigned int bytes_in, unsigned int bytes_out, 			unsigned int total_bytes_in, 			unsigned int total_bytes_out ){    assert( m_log_fd > 0 );    char line[512];  sprintf( line,           "%u\t\t%.3f\t\t%u\t%u\t%u\t%u\n",            m_step_num, m_sim_time, // step and time           //loop_duration, // real cycle time in ms           //sleep_duration, // real sleep time in ms           //m_sim_timestep / sleep_duration, // ratio           bytes_in, // bytes in this cycle           bytes_out, // bytes out this cycle           total_bytes_in,  // total bytes in           total_bytes_out); // total bytes out    write( m_log_fd, line, strlen(line) );}void CWorld::LogOutputHeader( void )  {  int log_instance = 0;  while( m_log_fd < 0 )    {      char fname[256];      sprintf( fname, "%s.%d", m_log_filename, log_instance++ );      m_log_fd = open( fname, O_CREAT | O_EXCL | O_WRONLY, 		       S_IREAD | S_IWRITE );    }  struct timeval t;  gettimeofday( &t, 0 );        // count the locally managed entities  int m=0;        char* tmstr = ctime((const time_t*)&t.tv_sec);  tmstr[ strlen(tmstr)-1 ] = 0; // delete the newline        char line[512];  sprintf( line,           "# Stage output log\n#\n"           "# Command:\t%s\n"           "# Date:\t\t%s\n"           "# Host:\t\t%s\n"           //"# Bitmap:\t%s\n"           "# Timestep(ms):\t%d\n"           "# Entities:\t%d of %d\n#\n"           "#STEP\t\tSIMTIME(s)"	   //"\tINTERVAL(s)\tSLEEP(s)\tRATIO\t"           "\tINPUT\tOUTPUT\tITOTAL\tOTOTAL\n",           m_cmdline,            tmstr,            m_hostname,            //worldfilename,           (int)(m_sim_timestep * 1000.0),           m,            this->entity_count );        write( m_log_fd, line, strlen(line) );}// return the entity nearest the specified point, but not more than range m away,// that has the specified parent CEntity* CWorld::GetNearestChildWithinRange( double x, double y, double range, 					     CEntity* parent ){  //printf( "Searching from %.2f,%.2f for children of %p\n", x, y, parent );  CEntity* nearest = NULL;    double px, py, pth;  double d;  for( int c=0; c<this->entity_count; c++ )    {      CEntity* ent = this->entities[c];            // we can only select items with root a parent      if( ent->m_parent_entity != parent )	continue;            ent->GetGlobalPose( px, py, pth );            d = hypot( py - y, px - x );            //printf( "Entity type %s is %.2fm away at  %.2f,%.2f\n",       //      ent->GetToken(), d, px, py );            if( d < range )	{	  range = d;	  nearest = ent;	}    }    //if( nearest )  //printf ( "Nearest is type %s\n", nearest->GetToken() );  //else  //puts( "no entity within range" );  return nearest;}// return the entity nearest the specified point, but not more than range m away,// that has the specified parent CEntity* CWorld::GetNearestEntityWithinRange( double x, double y, double range )		{  CEntity* nearest = NULL;  double px, py, pth;  double d;    for( int c=0; c<this->entity_count; c++ )    {      CEntity* ent = this->entities[c];            ent->GetGlobalPose( px, py, pth );            d = hypot( py - y, px - x );            //printf( "Entity type %s is %.2fm away at  %.2f,%.2f\n",       //      ent->token, d, px, py );            if( d < range )	{	  range = d;	  nearest = ent;	}    }    if( nearest )    printf ( "Nearest is %s\n", nearest->lib_entry->token );  else    puts( "no entity within range" );    return nearest;}///////////////////////////////////////////////////////////////////////////// lock the shared mem//bool CWorld::LockByte( int offset ){  return true; // success}///////////////////////////////////////////////////////////////////////////// unlock the shared mem//bool CWorld::UnlockByte( int offset ){  return true; // success}