}///////////////////////////////////////////////////////////////////////////// Shutdown routinevoid CEntity::Shutdown(){  //PRINT_DEBUG( "entity shutting down" );    // recursively shutdown our children  CHILDLOOP( ch ) ch->Shutdown();  if( m_world->enable_gui )    GuiEntityShutdown( this );    return;}///////////////////////////////////////////////////////////////////////////// Update the entity's representationvoid CEntity::Update( double sim_time ){  //PRINT_DEBUG( "UPDATE" );  // recursively update our children  CHILDLOOP( ch ) ch->Update( sim_time );      GuiEntityUpdate( this );}///////////////////////////////////////////////////////////////////////////// Render the entity into the worldvoid CEntity::Map(double px, double py, double pth){  // get the pose in local coords  this->GlobalToLocal( px, py, pth );  // add our center of rotation offsets  px += origin_x;  py += origin_y;   // convert back to global coords  this->LocalToGlobal( px, py, pth );  this->map_px = px;  this->map_py = py;  this->map_pth = pth;  MapEx(map_px, map_py, map_pth, true);}///////////////////////////////////////////////////////////////////////////// Remove the entity from the worldvoid CEntity::UnMap(){  MapEx(this->map_px, this->map_py, this->map_pth, false);}///////////////////////////////////////////////////////////////////////////// Remap ourself if we have movedvoid CEntity::ReMap(double px, double py, double pth){  // if we haven't moved, do nothing  if (fabs(px - this->map_px) < 1 / m_world->ppm &&      fabs(py - this->map_py) < 1 / m_world->ppm &&      pth == this->map_pth)    return;    // otherwise erase the old render and draw a new one  UnMap();  Map(px, py, pth);}///////////////////////////////////////////////////////////////////////////// Primitive rendering functionvoid CEntity::MapEx(double px, double py, double pth, bool render){  switch (this->shape)    {    case ShapeRect:      m_world->SetRectangle(px, py, pth, 			    this->size_x, this->size_y, this, render);      break;    case ShapeCircle:      m_world->SetCircle(px, py, this->size_x / 2, this, render);      break;    case ShapeNone:      break;    }}///////////////////////////////////////////////////////////////////////////// Check to see if the given pose will yield a collision with obstacles.// Returns a pointer to the first entity we are in collision with, and stores// the location of the hit in hitx,hity// Returns NULL if not collisions.// This function is useful for writing position devices.CEntity *CEntity::TestCollision(double px, double py, double pth){  double qx = px + this->origin_x * cos(pth) - this->origin_y * sin(pth);  double qy = py + this->origin_y * sin(pth) + this->origin_y * cos(pth);  double qth = pth;  double sx = this->size_x;  double sy = this->size_y;  switch( this->shape )   {    case ShapeRect:    {      CRectangleIterator rit( qx, qy, qth, sx, sy, m_world->ppm, m_world->matrix );      CEntity* ent;      while( (ent = rit.GetNextEntity()) )      {        if( ent != this && ent->obstacle_return && !IsDescendent(ent) )	    return ent;      }      return NULL;    }    case ShapeCircle:    {      CCircleIterator rit( px, py, sx / 2, m_world->ppm, m_world->matrix );      CEntity* ent;      while( (ent = rit.GetNextEntity()) )      {        if( ent != this && ent->obstacle_return && !IsDescendent(ent))	    return ent;      }      return NULL;    }  case ShapeNone:    break;  }  return NULL;}///////////////////////////////////////////////////////////////////////////// same as the above method, but stores the hit location in hitx, hityCEntity *CEntity::TestCollision(double px, double py, double pth, 				double &hitx, double &hity ){  double qx = px + this->origin_x * cos(pth) - this->origin_y * sin(pth);  double qy = py + this->origin_y * sin(pth) + this->origin_y * cos(pth);  double qth = pth;  double sx = this->size_x;  double sy = this->size_y;  switch( this->shape )   {    case ShapeRect:    {      CRectangleIterator rit( qx, qy, qth, sx, sy, m_world->ppm, m_world->matrix );      CEntity* ent;      while( (ent = rit.GetNextEntity()) )      {        if( ent != this && ent->obstacle_return && !IsDescendent(ent))        {          rit.GetPos( hitx, hity );          return ent;        }      }      return NULL;    }    case ShapeCircle:    {      CCircleIterator rit( px, py, sx / 2, m_world->ppm, m_world->matrix );      CEntity* ent;      while( (ent = rit.GetNextEntity()) )      {        if( ent != this && ent->obstacle_return && !IsDescendent(ent))        {          rit.GetPos( hitx, hity );          return ent;        }      }      return NULL;    }  case ShapeNone: // handle the null case      break;  }  return NULL;}///////////////////////////////////////////////////////////////////////////// Convert local to global coordsvoid CEntity::LocalToGlobal(double &px, double &py, double &pth){  // Get the pose of our origin wrt global cs  double ox, oy, oth;  GetGlobalPose(ox, oy, oth);  // Compute pose based on the parent's pose  double sx = ox + px * cos(oth) - py * sin(oth);  double sy = oy + px * sin(oth) + py * cos(oth);  double sth = oth + pth;  px = sx;  py = sy;  pth = sth;}///////////////////////////////////////////////////////////////////////////// Convert global to local coords//void CEntity::GlobalToLocal(double &px, double &py, double &pth){  // Get the pose of our origin wrt global cs  double ox, oy, oth;  GetGlobalPose(ox, oy, oth);  // Compute pose based on the parent's pose  double sx =  (px - ox) * cos(oth) + (py - oy) * sin(oth);  double sy = -(px - ox) * sin(oth) + (py - oy) * cos(oth);  double sth = pth - oth;  px = sx;  py = sy;  pth = sth;}///////////////////////////////////////////////////////////////////////////// Set the entitys pose in the parent csvoid CEntity::SetPose(double px, double py, double pth){  // if the new position is different, call SetProperty to make the change.  // the -1 indicates that this change is dirty on all connections    if( this->local_px != px )     SetProperty( -1, PropPoseX, &px, sizeof(px) );    if( this->local_py != py )     SetProperty( -1, PropPoseY, &py, sizeof(py) );    if( this->local_pth != pth )     SetProperty( -1, PropPoseTh, &pth, sizeof(pth) );  }///////////////////////////////////////////////////////////////////////////// Get the entitys pose in the parent csvoid CEntity::GetPose(double &px, double &py, double &pth){  px = this->local_px;  py = this->local_py;  pth = this->local_pth;}///////////////////////////////////////////////////////////////////////////// Set the entitys pose in the global csvoid CEntity::SetGlobalPose(double px, double py, double pth){  // Get the pose of our parent in the global cs  double ox = 0;  double oy = 0;  double oth = 0;    if (m_parent_entity) m_parent_entity->GetGlobalPose(ox, oy, oth);    // Compute our pose in the local cs  double new_x  =  (px - ox) * cos(oth) + (py - oy) * sin(oth);  double new_y  = -(px - ox) * sin(oth) + (py - oy) * cos(oth);  double new_th = pth - oth;    SetPose( new_x, new_y, new_th );}///////////////////////////////////////////////////////////////////////////// Get the entitys pose in the global csvoid CEntity::GetGlobalPose(double &px, double &py, double &pth){  // Get the pose of our parent in the global cs  double ox = 0;  double oy = 0;  double oth = 0;  if (m_parent_entity)    m_parent_entity->GetGlobalPose(ox, oy, oth);      // Compute our pose in the global cs  px = ox + this->local_px * cos(oth) - this->local_py * sin(oth);  py = oy + this->local_px * sin(oth) + this->local_py * cos(oth);  pth = oth + this->local_pth;}////////////////////////////////////////////////////////////////////////////// Set the entitys velocity in the global csvoid CEntity::SetGlobalVel(double vx, double vy, double vth){  this->vx = vx;  this->vy = vy;  this->vth = vth;}////////////////////////////////////////////////////////////////////////////// Get the entitys velocity in the global csvoid CEntity::GetGlobalVel(double &vx, double &vy, double &vth){  vx = this->vx;  vy = this->vy;  vth = this->vth;}////////////////////////////////////////////////////////////////////////////// See if the given entity is one of our descendentsbool CEntity::IsDescendent(CEntity *entity){  while (entity)  {    entity = entity->m_parent_entity;    if (entity == this)      return true;  }  return false;}void CEntity::SetDirty( int con, char v ){  for( int i=0; i< ENTITY_LAST_PROPERTY; i++ )    SetDirty( con, (EntityProperty)i, v );}void CEntity::SetDirty( EntityProperty prop, char v ){  for( int i=0; i<MAX_POSE_CONNECTIONS; i++ )    SetDirty( i, prop, v );};void CEntity::SetDirty( int con, EntityProperty prop, char v ){  m_dirty[con][prop] = v;};void CEntity::SetDirty( char v ){  memset( m_dirty, v, 	  sizeof(char) * MAX_POSE_CONNECTIONS * ENTITY_LAST_PROPERTY );};///////////////////////////////////////////////////////////////////////////// change the parent void CEntity::SetParent(CEntity* new_parent) {   m_parent_entity = new_parent; };int CEntity::SetProperty( int con, EntityProperty property, 			  void* value, size_t len ){  assert( value );  assert( len > 0 );  assert( (int)len < MAX_PROPERTY_DATA_LEN );  bool refresh_figure = false;  bool move_figure = false;    switch( property )  {    case PropParent:      // TODO - fix this      // get a pointer to the (*value)'th entity - that's our new parent      //this->m_parent_entity = m_world->GetEntity( *(int*)value );           break;    case PropSizeX:      memcpy( &size_x, (double*)value, sizeof(size_x) );      // force the device to re-render itself      refresh_figure = true;      break;    case PropSizeY:      memcpy( &size_y, (double*)value, sizeof(size_y) );      // force the device to re-render itself      refresh_figure = true;      break;    case PropPoseX:      memcpy( &local_px, (double*)value, sizeof(local_px) );      move_figure = true;      break;    case PropPoseY:      memcpy( &local_py, (double*)value, sizeof(local_py) );      move_figure = true;      break;    case PropPoseTh:      // normalize theta      local_pth = atan2( sin(*(double*)value), cos(*(double*)value));      //memcpy( &local_pth, (double*)value, sizeof(local_pth) );      move_figure = true;      break;    case PropOriginX:      memcpy( &origin_x, (double*)value, sizeof(origin_x) );      refresh_figure = true;      break;    case PropOriginY:      memcpy( &origin_y, (double*)value, sizeof(origin_y) );      refresh_figure = true;      break;    case PropName:      strcpy( name, (char*)value );      // force the device to re-render itself      refresh_figure = true;      break;    case PropColor:      memcpy( &color, (StageColor*)value, sizeof(color) );      // force the device to re-render itself      refresh_figure = true;      break;    case PropShape:      memcpy( &shape, (StageShape*)value, sizeof(shape) );      // force the device to re-render itself      refresh_figure = true;      break;    case PropLaserReturn:      memcpy( &laser_return, (LaserReturn*)value, sizeof(laser_return) );      break;    case PropIdarReturn:      memcpy( &idar_return, (IDARReturn*)value, sizeof(idar_return) );      break;