entity.cc

一个机器人平台

}


///////////////////////////////////////////////////////////////////////////
// Shutdown routine
void 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 representation
void 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 world
void 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 world
void CEntity::UnMap()
{
  MapEx(this->map_px, this->map_py, this->map_pth, false);
}


///////////////////////////////////////////////////////////////////////////
// Remap ourself if we have moved
void 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 function
void 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, hity
CEntity *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 coords
void 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 cs
void 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 cs
void 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 cs
void 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 cs
void 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 cs
void CEntity::SetGlobalVel(double vx, double vy, double vth)
{
  this->vx = vx;
  this->vy = vy;
  this->vth = vth;
}


////////////////////////////////////////////////////////////////////////////
// Get the entitys velocity in the global cs
void 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 descendents
bool 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;