terrain.cc

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

  for (i=0; i<this->reducedODEIndexCount; i++)
  {
    utmp = (uint32_t)htonl((int)this->reducedODEIndices[i]);
    fwrite(&utmp, sizeof(uint32_t), 1, outFile);
  }

  fclose(outFile);
}

void Terrain::PrintInfo()
{
  printf("Data Size [%f %f] (points)\n", this->dataSizeOrigX, 
      this->dataSizeOrigY);
  printf("Modified Data Size [%f %f] (points)\n", this->dataSizeModX, 
      this->dataSizeModY);
  printf("Num Patches [%f %f]\n",this->numPatchesX, this->numPatchesY);
  printf("Pixel Size [%f %f] (meters)\n", this->pixelSizeX, this->pixelSizeY);
  printf("Map Size [%f %f] (meters)]\n", this->mapSizeX, this->mapSizeY);
  printf("UTM [%f %f]\n",this->easting, this->northing);
}

///////////////////////////////////////////////////////////////////////////////
// Cleanup our mess
void Terrain::Cleanup()
{
  // Cleanup old vertices
  if (this->vertices)
    delete [] this->vertices;

  // Cleanup old patches
  if (this->patches)
  {
    for (int i=0; i<this->numPatchesX*this->numPatchesY; i++)
    {
      delete this->patches[i];
    }
    delete [] this->patches;
  }

  if (this->indexList)
    delete this->indexList;
  if (this->odeIndexList)
    delete this->odeIndexList;

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

///////////////////////////////////////////////////////////////////////////////
// Warp the input file to guarantee it's in UTM coordinates
int Terrain::Warp( const char *filename )
{
  const char *projRef = strdup(this->dataSet->GetProjectionRef());

  if ( strcmp(projRef, "") != 0)
  {
    char cmd[512];
    sprintf(cmd, "gdalwarp -q -t_srs \"+proj=utm +zone=%d +data=WGS84\" %s .gzBuilder.tmp", this->utmZone, filename);

    system("rm .gzBuilder.tmp 2>/dev/null");
    system(cmd);

    // Reload the dataset with the new warpped version
    this->dataSet = (GDALDataset *)GDALOpen( ".gzBuilder.tmp", GA_ReadOnly );
    if (!this->dataSet)
      return -1;
  }

  return 0;
}


///////////////////////////////////////////////////////////////////////////////
// Initialize the various attributes
void Terrain::Initialize()
{
  double gt[6];

  this->Cleanup();

  this->indexList = new IndexList();
  this->odeIndexList = new IndexList();

  // Get the affine transform info
  if (this->dataSet->GetGeoTransform( gt ) == CE_None)
  {
    this->pixelSizeX = fabsf(gt[1]) <= 0.0001 ? 1.0 : fabs(gt[1]);
    this->pixelSizeY = fabsf(gt[5]) <= 0.0001 ? 1.0 : fabs(gt[5]);

    this->pixelSizeX *= this->horzScale;
    this->pixelSizeY *= this->horzScale;

    this->easting  = gt[0] + this->offsetsX;
    this->northing = gt[3] + this->offsetsY;
  }
  else
  {
    this->pixelSizeX = this->horzScale;
    this->pixelSizeY = this->horzScale;
    
    this->easting  = this->offsetsX;
    this->northing = this->offsetsY;
  }
 
  // Grab the original size of the data
  this->dataSizeOrigX = this->dataSet->GetRasterXSize();
  this->dataSizeOrigY = this->dataSet->GetRasterYSize();

  this->dataSizeModX = this->dataSizeOrigX;
  this->dataSizeModY = this->dataSizeOrigY;

  // Find the nearst width and height that is divisible by the patch size
  while ((float)this->dataSizeModX/(this->patchSize-1) - 
         floor((float)this->dataSizeModX/(this->patchSize-1)) != 0)
    this->dataSizeModX++;
  this->dataSizeModX++;

  while ((float)this->dataSizeModY/(this->patchSize-1) - 
         floor((float)this->dataSizeModY/(this->patchSize-1)) != 0)
    this->dataSizeModY++;
  this->dataSizeModY++;

  // The size of the map in meters
  this->mapSizeX = this->dataSizeModX * this->pixelSizeX;
  this->mapSizeY = this->dataSizeModY * this->pixelSizeY;

  // Calc. patch info
  this->numPatchesX = (this->dataSizeModX-1) / (this->patchSize-1);
  this->numPatchesY = (this->dataSizeModY-1) / (this->patchSize-1);

  // Create the array of patches
  this->patches = new TerrainPatch*[(int)(this->numPatchesX * this->numPatchesY)];

  // This is a tightly packed interleaved array. Each vertex contains
  // texture coordinates, color, normal, and vertex position.
  // Format of a single Vertex where each value is a GLfloat:
  //     [Tex, Tex, Clr, Clr, Clr, Clr, Norm, Norm, Norm, X, Y, Z]
  this->vertices = new GLfloat[(int)(this->dataSizeModX*this->dataSizeModY*12)];

}

///////////////////////////////////////////////////////////////////////////////
// Load all the terrain data
void Terrain::LoadData()
{
  float *zData = NULL;
  float noDataValue = -1.0;
  float bandOffset, bandScale;
  GLfloat *vert = NULL;
  unsigned int x,y, index;
  float z, maxZ;


  maxZ = -FLT_MAX;
  index = 0;

  // Get a pointer to the  first raster band of the data
  GDALRasterBand *band = this->dataSet->GetRasterBand(1);

  // Contains all the z-values
  zData = new float[(int)(this->dataSizeOrigX * this->dataSizeOrigY)];

  // Fill zData with all the raster information
  // Handles data replication if we increased the dimensions
  band->RasterIO( GF_Read, 0, 0, 
      (int)this->dataSizeOrigX, (int)this->dataSizeOrigY, zData, 
      (int)this->dataSizeOrigX, (int)this->dataSizeOrigY, 
      GDT_Float32, 0 , 0);

  // This value indicates invalid elevation data
  noDataValue = band->GetNoDataValue();
  bandOffset = band->GetOffset();
  bandScale = band->GetScale();
  maxZ = band->GetMaximum();

  z = 0;
  for (y=0; y < this->dataSizeModY; y++)
  {
    for (x=0; x < this->dataSizeModX; x++)
    {
      index = y * (int)this->dataSizeModX + x;
      vert = this->vertices + (index*12);

      //z = 0;
      if (y < this->dataSizeOrigY && x < this->dataSizeOrigX)
      {
        z = zData[ (int)(this->dataSizeOrigY-y-1) * (int)this->dataSizeOrigX + x ];
      }

      // TODO: The -32766 is a hack. Some SDTS file output this value....
      if (z==noDataValue || z==-32766)
        z=0;
      else
        z = z * bandScale + bandOffset;

      if (this->normalizeZ)
        z /= maxZ;

      // Texture coords
      *(vert++) = (float)x*this->pixelSizeX / this->sTextureSize;
      *(vert++) = (float)(this->dataSizeModY-y-1)*this->pixelSizeY / this->tTextureSize;

      // Color values
      *(vert++) = 1;
      *(vert++) = 0;
      *(vert++) = 0;
      *(vert++) = 1;

      // Normal vector
      *(vert++) = 0;
      *(vert++) = 0;
      *(vert++) = 0;

      // Vertex position
      *(vert++) = (x * this->pixelSizeX);
      *(vert++) = (y * this->pixelSizeY);
      *(vert++) = z * this->vertScale;
    }
  }

   delete [] zData;
}


///////////////////////////////////////////////////////////////////////////////
// Reduce the terrain based on an error bound (in meters)
void Terrain::Reduce()
{
  int i;
  unsigned int x,y;
  int pIndex, index;
  int yPatch, xPatch;
  int yOffset, xOffset;
  float d;

  pIndex = 0;
  index = 0;

  // Create all the patches. Each patch contains an array indexing the
  // vertices it uses from the vertices array.
  for (yPatch=0; yPatch<this->numPatchesY; yPatch++)
  {
    for (xPatch=0; xPatch<this->numPatchesX; xPatch++,pIndex++)
    {

      // The edges of each patch must overlap by one value
      yOffset = yPatch * (this->patchSize-1);
      xOffset = xPatch * (this->patchSize-1);

      this->patches[pIndex] = new TerrainPatch(this->patchSize, errBound, this->vertices);
      
      for (y=0; y<this->patchSize; y++)
      {
        for (x=0; x<this->patchSize; x++)
        {
          // The index into the vertices array
          index = (y+yOffset) * (int)this->dataSizeModX + x + xOffset;
          this->patches[pIndex]->SetVertex(x,y,index);
        }
      }
    }
  }

  pIndex = 0;

  // Initialize the patches. Tell all the patches about their neighbors
  for (yPatch=0; yPatch<this->numPatchesY; yPatch++)
  {
    for (xPatch=0; xPatch<this->numPatchesX; xPatch++,pIndex++)
    {
      // Set left neighbor
      if (yPatch > 0)
      {
        this->patches[pIndex]->SetNeighbor(
          this->patches[(yPatch-1)*(int)this->numPatchesX+xPatch], 
          TerrainPatch::BOTTOM);
      } else {
        this->patches[pIndex]->SetNeighbor(NULL, TerrainPatch::BOTTOM);
      }
      
      // Set right neighbor
      if (yPatch < this->numPatchesY-1)
      {
        this->patches[pIndex]->SetNeighbor(
          this->patches[(yPatch+1)*(int)this->numPatchesX+xPatch], 
          TerrainPatch::TOP);
      } else {
        this->patches[pIndex]->SetNeighbor(NULL, TerrainPatch::TOP);
      }

      // Set top neighbor
      if (xPatch > 0)
      {
        this->patches[pIndex]->SetNeighbor(
          this->patches[yPatch*(int)this->numPatchesX+xPatch-1],
          TerrainPatch::LEFT);
      } else {
        this->patches[pIndex]->SetNeighbor(NULL,TerrainPatch::LEFT);
      }

      // Set bottom neighbor
      if (xPatch < this->numPatchesX-1)
      {
        this->patches[pIndex]->SetNeighbor(
          this->patches[yPatch*(int)this->numPatchesX+xPatch+1],
          TerrainPatch::RIGHT);
      } else {
        this->patches[pIndex]->SetNeighbor(NULL,TerrainPatch::RIGHT);
      }

      this->patches[pIndex]->Init();
    }
  }

  bool facing = false;

  for (i=0; i<this->numPatchesX*this->numPatchesY; i++)
  {
    this->patches[i]->CreateStrip(i, this->indexList, this->odeIndexList,
        facing);
  }


  // Normalize all the normal vectors
  for (i=0; i<this->dataSizeModX * this->dataSizeModY; i++)
  {
    d = sqrt(this->vertices[i*12+6]*this->vertices[i*12+6] +
             this->vertices[i*12+7]*this->vertices[i*12+7] +
             this->vertices[i*12+8]*this->vertices[i*12+8]);

    //if (d!=0)
    //{
      this->vertices[i*12+6] /= d;
      this->vertices[i*12+7] /= d;
      this->vertices[i*12+8] /= d;
   // } else  {
      //this->vertices[i*12+6] = 0;
      //this->vertices[i*12+7] = 0;
      //this->vertices[i*12+8] = 1;
    //}

  }
}






//////////////////////////////////////////////////////////////////////////////
// A really really simple list

IndexNode::IndexNode()
  : index(0)
{
  this->next = NULL;
  this->prev = NULL;
}

IndexList::IndexList()
  : size(0), max(0)
{
  this->head = new IndexNode();
  this->tail = this->head;
}

IndexList::~IndexList()
{
  IndexNode *node;
  node = this->head->next;

  while (node != NULL)
  {
    delete node->prev;
    node = node->next;
  }

  delete this->tail;
}

void IndexList::Push( GLuint index )
{
  IndexNode *newNode = new IndexNode();

  newNode->index = index;

  this->tail->next = newNode;
  newNode->prev = this->tail;
  this->tail = newNode;

  if (index > this->max)
    this->max = index;

  this->size++;
}