worldfile.cc

机器人人3D仿真工具,可以加入到Simbad仿真环境下应用。

  else if( !value )
    return def;

  // TODO: cache the value somewhere (currently leaks)
  return (const char*) value;
}


///////////////////////////////////////////////////////////////////////////
// Set an attribute string value
void WorldFileNode::SetString( const char *key, const char *v)
{
  this->SetNodeValue(key, v);
  
  return;
}



///////////////////////////////////////////////////////////////////////////
// Get a file name.  Always returns an absolute path.  If the filename
// is entered as a relative path, we prepend the world file path.
const char *WorldFileNode::GetFilename( const char *key, const char *def, int require)
{
  const char *filename = this->GetString( key, def, require );
  if (filename == NULL)
    return NULL;

  if (filename[0] == '/' || filename[0] == '~')
    return filename;
  
  else if (this->file->filename[0] == '/' || this->file->filename[0] == '~')
  {
    // Note that dirname() modifies the contents, so
    // we need to make a copy of the filename.
    // There's no bounds-checking, but what the heck.
    char *tmp = strdup( this->file->filename );
    char *fullpath = (char*) malloc( PATH_MAX );
    memset( fullpath, 0, PATH_MAX );
    strcat( fullpath, dirname(tmp));
    strcat( fullpath, "/" ); 
    strcat( fullpath, filename );
    assert( strlen(fullpath) + 1 < PATH_MAX );
    free( tmp );
    return fullpath;
  }
  else
  {
    // Prepend the path
    // Note that dirname() modifies the contents, so
    // we need to make a copy of the filename.
    // There's no bounds-checking, but what the heck.
    char *tmp = strdup( this->file->filename );
    char *fullpath = (char*) malloc(PATH_MAX);
    getcwd( fullpath, PATH_MAX );
    strcat( fullpath, "/" ); 
    strcat( fullpath, dirname(tmp) );
    strcat( fullpath, "/" ); 
    strcat( fullpath, filename );
    assert( strlen(fullpath) + 1 < PATH_MAX );
    free(tmp);
    return fullpath;
  }
  assert(false);
  return NULL;
}


///////////////////////////////////////////////////////////////////////////
// Search for a filename across multiple paths.  
const char *WorldFileNode::SearchFilename( const char *key, const char *path, const char *def,
                                           int require)
{
  struct stat stats;
  char *fullname, *tmp, *dir;
  
  // Get the filename
  const char *filename = this->GetString( key, def, require );
  if (filename == NULL)
    return NULL;

  // Absolute filename; do nothing
  if (filename[0] == '/' || filename[0] == '~')
    return filename;

  fullname = (char*) malloc(PATH_MAX);

  // Look for file using current dir
  tmp = (char*) malloc(PATH_MAX);
  getcwd(tmp, PATH_MAX);
  snprintf(fullname, PATH_MAX, "%s/%s", tmp, filename);
  free(tmp);
  
  if (stat(filename, &stats) == 0)
    return fullname;

  // Look for file using world file dir
  tmp = strdup( this->file->filename );
  dir = dirname(tmp);
  snprintf(fullname, PATH_MAX, "%s/%s", dir, filename);
  free(tmp);
  
  if (stat(fullname, &stats) == 0)
    return fullname;

  if (path)
  {
    // Look for file using given path
    snprintf(fullname, PATH_MAX, "%s/%s", path, filename);  
    if (stat(fullname, &stats) == 0)
      return fullname;
  }

  free(fullname);

  // Couldnt find the file
  PRINT_WARN1("file not found [%s]", filename);
  
  return def;
}


////////////////////////////////////////////////////////////////////////////
// Get an integer
int WorldFileNode::GetInt( const char *key, int def, int require )
{
  char *value = this->GetNodeValue( key );

  //printf("node [%s] [%s]\n", key, value);
  
  if (!value && require)
  {
    PRINT_ERR3( "in %s:%d missing attribute [%s]",
        this->file->filename, (int) xmlGetLineNo(this->xmlNode), key );
    return -1;
  }
  else if( !value )
    return def;

  return atoi((const char*) value);
}


////////////////////////////////////////////////////////////////////////////
// Get a double
double WorldFileNode::GetDouble( const char *key, double def, int require )
{
  char *value = this->GetNodeValue( key );

  //printf("node [%s] [%s]\n", key, value);
  
  if (!value && require)
  {
    PRINT_ERR3( "in %s:%d missing attribute [%s]",
        this->file->filename, (int) xmlGetLineNo(this->xmlNode), key );
    return -1;
  }
  else if( !value )
    return def;

  return atof((const char*) value);
}

////////////////////////////////////////////////////////////////////////////
// Get a boolean
bool WorldFileNode::GetBool( const char *key, bool def, int require )
{
  char *value = this->GetNodeValue( key );

  //printf("node [%s] [%s]\n", key, value);
  
  if (!value && require)
  {
    PRINT_ERR3( "in %s:%d missing attribute [%s]",
        this->file->filename, (int) xmlGetLineNo(this->xmlNode), key );
    return -1;
  }
  else if( !value )
    return def;

  if (strcmp((const char*) value, "true") == 0)
    return 1;
  
  return atoi((const char*) value);
}


////////////////////////////////////////////////////////////////////////////
// Get a length
double WorldFileNode::GetLength( const char *key, double def, int require )
{
  double length = this->GetDouble(key, def, require);

  // Do unit conversion here
  
  return length;
}


////////////////////////////////////////////////////////////////////////////
// Get an attribute angle value (return value in radians)
double WorldFileNode::GetAngle( const char *key, double def, int require )
{
  return this->GetDouble(key, def * 180 / M_PI, require) * M_PI / 180;
}


////////////////////////////////////////////////////////////////////////////
// Get a time
double WorldFileNode::GetTime( const char *key, double def, int require )
{
  double time = this->GetDouble(key, def, require);

  // Do unit conversion here
  
  return time;
}


////////////////////////////////////////////////////////////////////////////
// Read a position
GzVector WorldFileNode::GetPosition( const char *key, GzVector def )
{
  GzVector v;

  if (this->GetTupleString(key, 0, NULL) == NULL)
    return def;

  v.x = this->GetTupleLength( key, 0, 0.0 );
  v.y = this->GetTupleLength( key, 1, 0.0 );
  v.z = this->GetTupleLength( key, 2, 0.0 );

  return v;
}


////////////////////////////////////////////////////////////////////////////
// Write a position
void WorldFileNode::SetPosition( const char *key, GzVector v )
{
  char buffer[256];
  
  snprintf(buffer, sizeof(buffer), "%.3f %.3f %.3f", v.x, v.y, v.z);
  this->SetString(key, buffer);
  
  return;
}


////////////////////////////////////////////////////////////////////////////
// Read a rotation
GzQuatern WorldFileNode::GetRotation( const char *key, GzQuatern def )
{
  double px, py, pz;

  if (this->GetTupleString(key, 0, NULL) == NULL)
    return def;

  px = this->GetTupleAngle( key, 0, 0.0 );
  py = this->GetTupleAngle( key, 1, 0.0 );
  pz = this->GetTupleAngle( key, 2, 0.0 );
  GzQuatern q = GzQuaternFromEuler( px, py, pz );

  return q;
}


////////////////////////////////////////////////////////////////////////////
// Write a rotation
void WorldFileNode::SetRotation( const char *key, GzQuatern v )
{
  char buffer[256];
  GzVector e;

  e = GzQuaternToEuler(v);
  e.x *= 180 / M_PI;
  e.y *= 180 / M_PI;
  e.z *= 180 / M_PI;
  
  snprintf(buffer, sizeof(buffer), "%.0f %.0f %.0f", e.x, e.y, e.z);
  this->SetString(key, buffer);
  
  return;
}


/* REMOVE
////////////////////////////////////////////////////////////////////////////
// Read a pose
GzPose WorldFileNode::GetPose( const char *key, GzPose def )
{
  GzVector pos, at, up;
  GzPose p;
  
  if (this->GetTupleString(key, 0, NULL) == NULL)
    return def;

  // Determine pose using GLU lookat method
  pos = GzVectorSet(this->GetTupleLength(key, 0, 0.0),
                    this->GetTupleLength(key, 1, 0.0),
                    this->GetTupleLength(key, 2, 0.0));
  at = GzVectorSet(this->GetTupleLength(key, 3, 0.0),
                   this->GetTupleLength(key, 4, 0.0),
                   this->GetTupleLength(key, 5, 0.0));
  up = GzVectorSet(this->GetTupleLength(key, 6, 0.0),
                   this->GetTupleLength(key, 7, 0.0),
                   this->GetTupleLength(key, 8, 1.0));

  p = GzPosePointAt(pos, at, up);
  
  return p;
}
*/


////////////////////////////////////////////////////////////////////////////
// Read a color (returns color as a 3-vector)
GzColor WorldFileNode::GetColor( const char *key, GzColor def )
{
  GzColor c;

  if (this->GetString( key, NULL ) == NULL)
    return def;
  
  c.r = this->GetTupleDouble( key, 0, 0.0 );
  c.g = this->GetTupleDouble( key, 1, 0.0 );
  c.b = this->GetTupleDouble( key, 2, 0.0 );
  c.a = this->GetTupleDouble( key, 3, 1.0 );
  
  return c;
}


////////////////////////////////////////////////////////////////////////////
// Get a tuple string value.
const char *WorldFileNode::GetTupleString( const char *key, int index, 
                                           const char *def)
{
  char *value;
  char *nvalue = NULL;
  int i, a, b, state, count;

  value = this->GetNodeValue( key );

  if (value == NULL)
    return def;

  state = 0;
  count = 0;
  a = b = 0;

  for (i = 0; i < (int) strlen((const char*) value); i++)
  {
    // Look for start of element
    if (state == 0)
    {
      if (!isspace( value[i] ))
      {
        a = i;
        state = 1;
      }
    }

    // Look for end of element
    else if (state == 1)
    {
      if (isspace( value[i] ))
      {
        state = 0;
        b = i - 1;
        count++;
        if (count > index)
          break;
      }
    }
  }

  if (state == 1)
  {
    b = i - 1;
    count++;
  }

  if (count == index + 1)
    nvalue = strndup( (const char*) value + a, b - a + 2 );
  else
    nvalue = NULL;

  //printf( "tuple [%s] [%d] [%s] [%s] [%d %d]\n", key, index, value, nvalue, a, b );

  xmlFree( value );

  return nvalue;
}


////////////////////////////////////////////////////////////////////////////
// Get an attribute tuple double value
double WorldFileNode::GetTupleDouble( const char *key, int index, double def )
{
  const char *svalue;

  svalue = this->GetTupleString( key, index, NULL );
  if (svalue == NULL)
    return def;

  return atof(svalue);
}

////////////////////////////////////////////////////////////////////////////
// Get an attribute tuple int value
int WorldFileNode::GetTupleInt( const char *key, int index, int def )
{
  const char *svalue;

  svalue = this->GetTupleString( key, index, NULL );
  if (svalue == NULL)
    return def;

  return atoi(svalue);
}

////////////////////////////////////////////////////////////////////////////
// Get an tuple length value (return value in meters)
double WorldFileNode::GetTupleLength( const char *key, int index, double def )
{
  const char *svalue;

  svalue = this->GetTupleString( key, index, NULL );
  if (svalue == NULL)
    return def;

  // TODO: unit conversion here
  
  return atof(svalue);
}


////////////////////////////////////////////////////////////////////////////
// Get an tuple angle value (return value in radians)
double WorldFileNode::GetTupleAngle( const char *key, int index, double def )
{
  const char *svalue;

  svalue = this->GetTupleString( key, index, NULL );
  if (svalue == NULL)
    return def;

  return atof(svalue) * M_PI / 180;
}