terrain.cc

来自「机器人人3D仿真工具,可以加入到Simbad仿真环境下应用。」· CC 代码 · 共 801 行 · 第 1/2 页

/*
 *  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: Terrain main class
 * Author: Nate Keonig
 * Date: 05 Nov 2004
 * CVS: $Id: Terrain.cc,v 1.16 2004/12/06 05:57:48 inspectorg Exp $
 */

#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <assert.h>
#include <float.h>
#include <limits.h>
#include <netinet/in.h>

#include "Terrain.hh"
#include "TerrainPatch.hh"

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

//////////////////////////////////////////////////////////////////////////////
// Constructor
Terrain::Terrain()
{
  // Register all known drivers for GDAL
  GDALAllRegister();

  this->dataSet = NULL;
  this->patches = NULL;
  this->indexList = NULL;
  this->odeIndexList = NULL;
  this->vertices = NULL;

  this->patchSize = 17;

  this->utmZone = 11;
  this->easting = 0;
  this->northing = 0;

  this->offsetsX = 0.0;
  this->offsetsY = 0.0;
  this->offsetsZ = 0.0;

  this->horzScale = 1.0;
  this->vertScale = 1.0;

  this->normalizeZ = false;

  this->sTextureSize = 2.0;
  this->tTextureSize = 2.0;

  this->errBound = 0.0;
}

//////////////////////////////////////////////////////////////////////////////
// Destructor
Terrain::~Terrain()
{
}

void Terrain::SetUTMZone(int zone)
{
  this->utmZone = zone;
}

void Terrain::SetXOffset( double x )
{
  this->offsetsX = x;
}

void Terrain::SetYOffset( double y )
{
  this->offsetsY = y;
}

void Terrain::SetZOffset( double z )
{
  this->offsetsZ = z;
}

void Terrain::SetScale( float horzScale, float vertScale )
{
  this->horzScale = horzScale;
  this->vertScale = vertScale;
}

void Terrain::SetHorzScale( float horzScale )
{
  this->horzScale = horzScale;
}

void Terrain::SetVertScale( float vertScale )
{
  this->vertScale = vertScale;
}

void Terrain::SetNormalizeZ( bool normalize )
{
  this->normalizeZ = normalize;
}

void Terrain::SetTextureSize( float s, float t )
{
  this->sTextureSize = s;
  this->tTextureSize = t;
}

void Terrain::SetSTextureSize( float s )
{
  this->sTextureSize = s;
}

void Terrain::SetTTextureSize( float t )
{
  this->tTextureSize = t;
}

void Terrain::SetErrorBound( double err )
{
  this->errBound = err;
}

int Terrain::GetUTMZone()
{
  return this->utmZone;
}

float Terrain::GetEasting()
{
  return this->easting;
}

float Terrain::GetNorthing()
{
  return this->northing;
}

void Terrain::GetOffsets( double &x, double &y, double &z )
{
  x = this->offsetsX;
  y = this->offsetsX;
  z = this->offsetsX;
}

void Terrain::GetScale( float &horzScale, float &vertScale )
{
  horzScale = this->horzScale;
  vertScale = this->vertScale;
}

bool Terrain::GetNormalizeZ()
{
  return this->normalizeZ;
}
void Terrain::GetTextureSize( float &s, float &t )
{
  s = this->sTextureSize;
  t = this->tTextureSize;
}

double Terrain::GetErrorBound()
{
  return this->errBound;
}

void Terrain::GetOrigDataSize(double &width, double &height)
{
  width = this->dataSizeOrigX;
  height = this->dataSizeOrigY;
}

void Terrain::GetModDataSize( double &width, double &height )
{
  width =  this->dataSizeModX;
  height =  this->dataSizeModY;
}

void Terrain::GetMapSize( double &width, double &height )
{
  width = this->mapSizeX;
  height = this->mapSizeY;
}

void Terrain::GetNumPatches( double &xCount, double &yCount )
{
  xCount = this->numPatchesX;
  yCount = this->numPatchesY;
}

int Terrain::GetVertexStride()
{
  return 12;
}

int Terrain::GetReducedVertexCount()
{
  return this->reducedVertexCount;
}

int Terrain::GetReducedIndexCount()
{
  return this->reducedIndexCount;
}

const GLfloat *Terrain::GetReducedVertices()
{
  return this->reducedVertices;
}

const GLuint *Terrain::GetReducedIndices()
{
  return this->reducedIndices;
}



//////////////////////////////////////////////////////////////////////////////
// Load the GDAL data
int Terrain::Load( const char *filename)
{
  int result = -1;

  this->dataSet = (GDALDataset *)GDALOpen( filename, GA_ReadOnly );
  if (!this->dataSet)
    return -1;

  // Make sure we have a good dataset
  if (this->dataSet)
  {

    // Cleanup all the old stuff
    this->Cleanup();

    // Convert to UTM coordinates
    this->Warp(filename);

    // Initialize all the varables
    this->Initialize();

    // Load the data
    this->LoadData();

    // Reduce the data to it finall patchified form
    this->Reduce();

    unsigned int i,j;
    unsigned int totVerts = 0;

    unsigned int *uniqueMapping = new unsigned int[this->indexList->max+1];
    IndexList uniqueIndexList;
    IndexNode *node = this->indexList->head->next;

    for (i=0; i<=this->indexList->max; i++)
      uniqueMapping[i] = UINT_MAX;

    // Get all the unique indices
    while (node!=NULL)
    {
      if (uniqueMapping[node->index] == UINT_MAX)
      {
        uniqueMapping[node->index] = totVerts++;
        uniqueIndexList.Push(node->index);
      }
      node = node->next;
    }

    this->reducedVertexCount = uniqueIndexList.size;
    this->reducedVertices = new GLfloat[this->reducedVertexCount*12];

    j = 0;
    node = uniqueIndexList.head->next;
    while (node!=NULL)
    {
      for (i=0; i<12; i++)
        this->reducedVertices[j*12+i] = this->vertices[node->index*12+i];
      node = node->next;
      j++;
    }

    this->reducedIndexCount = this->indexList->size;
    this->reducedIndices = new GLuint[this->reducedIndexCount];

    j = 0;
    node = this->indexList->head->next;
    while (node!=NULL)
    {
      this->reducedIndices[j] = uniqueMapping[node->index];
      node = node->next;
      j++;
    }

    this->reducedODEIndexCount = this->odeIndexList->size;
    this->reducedODEIndices = new GLuint[this->reducedODEIndexCount];

    j = 0;
    node = this->odeIndexList->head->next;
    while (node!=NULL)
    {
      this->reducedODEIndices[j] = uniqueMapping[node->index];
      node = node->next;
      j++;
    }

    delete [] uniqueMapping;

    result = 0;
  }
  else
  {
    fprintf(stderr,"GDAL Loader: Failed to load data file[%s]\n",filename);
  }

  return result;
}

//////////////////////////////////////////////////////////////////////////////
// Output a Gazebo friendly terrain file
void Terrain::Write(const char *outputFilename)
{
  FILE *outFile = fopen(outputFilename,"w");
  unsigned int i,j;
 
  int32_t tmp;
  uint32_t utmp;

  // Version number
  utmp = htonl((int)(atof(VERSION)*1e3));
  fwrite(&utmp, sizeof(uint32_t),1,outFile);

  // Output the x-offset (easting)
  utmp = htonl((int)this->easting);
  fwrite(&utmp,sizeof(uint32_t),1,outFile);
  utmp = htonl((int)((this->easting-(int)this->easting)*1e6));
  fwrite(&utmp,sizeof(uint32_t),1,outFile);

  // Output the y-offset (northing)
  utmp = htonl((int)this->northing);
  fwrite(&utmp,sizeof(uint32_t),1,outFile);
  utmp = htonl((int)((this->northing - (int)this->northing)*1e6));
  fwrite(&utmp,sizeof(uint32_t),1,outFile);

  // Output the zoffset
  utmp = htonl((int)this->offsetsZ);
  fwrite(&utmp,sizeof(uint32_t),1,outFile);
  utmp = htonl((int)((this->offsetsZ - (int)this->offsetsZ)*1e6));
  fwrite(&utmp,sizeof(uint32_t),1,outFile);


  // Output number of vertices and indices
  utmp = htonl(this->reducedVertexCount);
  fwrite(&utmp,sizeof(uint32_t),1,outFile);
  utmp = htonl(this->reducedIndexCount);
  fwrite(&utmp,sizeof(uint32_t),1,outFile);
  utmp = htonl(this->reducedODEIndexCount);
  fwrite(&utmp,sizeof(uint32_t),1,outFile); 

  // Output all the unique vertices
  for (i=0; i<this->reducedVertexCount; i++)
  {

    // Fix up the texture coordinates
    this->reducedVertices[i * 12 + 0] = this->reducedVertices[i * 12 + 9] / this->sTextureSize;
    this->reducedVertices[i * 12 + 1] = this->reducedVertices[i * 12 + 10] / this->tTextureSize;

    
    for (j=0; j<12; j++)
    {
      tmp = (int32_t)htonl((int)(this->reducedVertices[i*12+j]*1e3));
      fwrite(&tmp, sizeof(int32_t),1, outFile);
    }
  }

  // Output the indices
  for (i=0; i<this->reducedIndexCount; i++)
  {
    utmp = (uint32_t)htonl((unsigned int)this->reducedIndices[i]);
    fwrite(&utmp, sizeof(uint32_t),1,outFile);
  }

  // Output the ODE indices