terrain.cc

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

/*
 *  Gazebo - Outdoor Multi-Robot Simulator
 *  Copyright (C) 2003  
 *     Nate Koenig & Andrew Howard
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */
/* Desc: Model for a Terrain generator
 * Author: Nate Koenig
 * Date: 02 Sept 2003
 * CVS: $Id: Terrain.cc,v 1.49 2006/03/03 21:11:04 natepak Exp $
 */


/// @addtogroup models 
/// @{ 
/** @defgroup Terrain Terrain


The Terrain model creates a triangle mesh surface representing fixed
features such as walls, ramps, hills and valleys.  The model reads an
elevation map produced by the @ref gzbuilder "gzbuilder utility", and
is restricted to 2.5D structures (i.e., no tunnels).


@par libgazebo interfaces

This model has no libgazebo interface.

@par Player drivers

There are no Player drivers.

@par Attributes

The following attributes are supported.

@htmlinclude default_attr_include.html

- terrainFile (string, filename)
  - File containing terrain data (required).  Use gzbuilder to generate terrain files.
  - Default: empty

- color (float tuple)
  - RGB color.
  - Default: 0.7 0.7 0.7

- surfaceHardness (float tuple)
  - Set spring-damper parameters for contact joints.
  - Default: dInfinity 0

- surfaceFriction (float tuple)
  - Set friction coefficients
  - Default: 1 1

@par Bodies

The following bodies are created by this model.

@htmlinclude default_body_include.html

@par Example

@verbatim
<model:Terrain>
  <terrainFile>terrain1.gzb</terrainFile>
</model:Terrain>
@endverbatim


@par Authors

Nate Koenig

*/
/// @} 


#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <sys/types.h>
#include <netinet/in.h>

#include "gazebo.h"
#include "Error.hh"
#include "World.hh"
#include "WorldFile.hh"
#include "ModelFactory.hh"
#include "Body.hh"
#include "TerrainGeom.hh"

#include "Terrain.hh"



//////////////////////////////////////////////////////////////////////////////
// Register this model
GZ_REGISTER_STATIC("Terrain", Terrain);


//////////////////////////////////////////////////////////////////////////////
// Constructor
Terrain::Terrain( World *world )
  : Model( world )
{
  this->body = NULL;
  this->vertices = NULL;
  this->indices = NULL;
  this->odeIndices = NULL;
  this->terrainGeom = NULL;
}


//////////////////////////////////////////////////////////////////////////////
// Destructor
Terrain::~Terrain()
{
  if (this->body)
    delete this->body;

  if (this->vertices)
    delete this->vertices;

  if (this->indices)
    delete this->indices;

  if (this->odeIndices)
    delete this->odeIndices;

  if (this->terrainGeom)
    delete this->terrainGeom;

  this->body = NULL;
  this->vertices = NULL;
  this->indices = NULL;
  this->odeIndices = NULL;
  this->terrainGeom = NULL;
}


//////////////////////////////////////////////////////////////////////////////
// Load the model
int Terrain::Load( WorldFile *file, WorldFileNode *node )
{
  const char *textureFile;

  // Create a dummy body to add the terrain to
  this->body = new Body(this->world, NULL, true);
  this->AddBody(this->body, true);
  
  if (this->OdeLoad( file, node ) != 0)
    return -1;

  // Apply a texture
  textureFile = node->SearchFilename("textureFile", GAZEBO_TEXTURES_PATH, NULL);
  if (textureFile)
  {
    PRINT_MSG1(1, "loading texture file [%s]", textureFile);  
    if (this->terrainGeom->SetTexture2DFile(textureFile) != 0)
    {
      PRINT_ERR("unable to load texture file");
      return -1;
    }
  }

  // This a fixed model, and should not collide with other fixed models
  dGeomSetCategoryBits( (dGeomID) this->modelSpaceId, GZ_FIXED_COLLIDE );
  dGeomSetCollideBits( (dGeomID) this->modelSpaceId, ~GZ_FIXED_COLLIDE );

  return 0;
}

//////////////////////////////////////////////////////////////////////////////
// Load the ODE model
int Terrain::OdeLoad( WorldFile *file, WorldFileNode *node )
{
  int tmp = 0;
  unsigned int utmp, i, j;
  float version = 0.0;
  FILE *terrainFile;
  const char *filename;
  GzVector offset;

  filename = node->SearchFilename("terrainFile", GAZEBO_TERRAINS_PATH, NULL);
  if (filename == NULL)
  {
    PRINT_ERR("No terrain file specified (use the <terrainFile> tag)");
    return -1;
  }

  terrainFile = fopen( filename,"r" );
  if (terrainFile == NULL)
  {
    PRINT_ERR("Unable to read terrain data");
    return -1;
  }

  fread(&utmp, sizeof(uint32_t), 1, terrainFile);
  version = (float)ntohl(utmp)/1e3;

  if (version < 0.5)
  {
    PRINT_ERR1("Invalid version. Terrain file version\n",version);
    return -1;
  }

  // Read in the xoffset (easting)
  fread(&utmp, sizeof(uint32_t), 1, terrainFile);
  offset.x = (float) (int32_t) ntohl(utmp);
  fread(&utmp, sizeof(uint32_t), 1, terrainFile);
  offset.x += (float) (int32_t) ntohl(utmp) / 1e6;

  // Read in the yoffset (northing)
  fread(&utmp, sizeof(uint32_t), 1, terrainFile);
  offset.y = (float) (int32_t) ntohl(utmp);
  fread(&utmp, sizeof(uint32_t), 1, terrainFile);
  offset.y += (float) (int32_t) ntohl(utmp) / 1e6;

  // Read in the zoffset (altitude)
  fread(&utmp, sizeof(uint32_t), 1, terrainFile);
  offset.z = (float) (int32_t) ntohl(utmp);
  fread(&utmp, sizeof(uint32_t), 1, terrainFile);
  offset.z += (float) (int32_t) ntohl(utmp) / 1e6;

  // Work out the terrain offset
  offset = GzVectorAdd(offset, node->GetPosition("xyz", GzVectorZero()));
  offset = GzVectorSub(offset, this->world->utmOffset);
  
  // Read in the number of vertices and indices
  fread(&utmp, sizeof(uint32_t), 1, terrainFile);
  this->vertexCount = ntohl(utmp);
  fread(&utmp, sizeof(uint32_t), 1, terrainFile);
  this->indexCount = ntohl(utmp);
  fread(&utmp, sizeof(uint32_t), 1, terrainFile);
  this->odeIndexCount = ntohl(utmp);

  this->vertices = new GLfloat[this->vertexCount*12];
  this->indices = new GLuint[this->indexCount];
  this->odeIndices = new GLuint[this->odeIndexCount];
  
  // Read in all the vertex information
  for (i=0; i<this->vertexCount; i++)
  {
    for (j=0; j<12; j++)
    {
      fread(&tmp, sizeof(int32_t),1, terrainFile);
      this->vertices[i*12+j] = (float)( ((int32_t)ntohl(tmp)) /1e3 );
      
      // Apply the UTM offset values
      if (j==9)
        this->vertices[i*12+j] += offset.x;
      if (j==10)
        this->vertices[i*12+j] += offset.y;
      if (j==11)
        this->vertices[i*12+j] += offset.z;
    }
  }

  // Read in all the indices
  for (i=0; i<this->indexCount; i++)
  {
    fread(&utmp,sizeof(uint32_t),1, terrainFile);
    this->indices[i] = (GLuint)ntohl(utmp);
  }

  // Read all the ODE indices
  for (i=0; i<this->odeIndexCount; i++)
  {
    fread(&utmp,sizeof(uint32_t),1, terrainFile);
    this->odeIndices[i] = (GLuint)ntohl(utmp);
  }

  // Create terrain geom
  this->terrainGeom = new TerrainGeom(this->body, this->modelSpaceId);
    
  // Load the geometry
  this->terrainGeom->Load(this->vertices, this->vertexCount, 
                          this->indices, this->indexCount,
                          this->odeIndices, this->odeIndexCount);
  
  // Read the terrain color
  this->terrainGeom->SetColor(node->GetColor("color", GzColor(0.7, 0.7, 0.7)));

  // Surface hardness
  this->terrainGeom->SetHardness(node->GetTupleDouble("surfaceHardness", 0, dInfinity),
                                 node->GetTupleDouble("surfaceHardness", 1, 0));

  // Surface friction
  this->terrainGeom->SetFriction(node->GetTupleDouble("surfaceFriction", 0, 1.0),
                                 node->GetTupleDouble("surfaceFriction", 1, 1.0));

  return 0;
}

//////////////////////////////////////////////////////////////////////////////
// Initialize the model
int Terrain::Init( WorldFile *file, WorldFileNode *node )
{  
  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Finalize the model
int Terrain::Fini()
{
  return 0;
}

//////////////////////////////////////////////////////////////////////////////
// Update the model
void Terrain::Update( double /*step*/ )
{
  return;
}