terrainpatch.cc

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

/*
 *  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: A single patch of terrain
 * Author: Nate Keonig
 * Date: 30 April 2004
 * CVS: $Id: TerrainPatch.cc,v 1.9 2004/11/15 17:11:45 natepak Exp $
 */

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

#include <float.h>

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

//////////////////////////////////////////////////////////////////////////////
// Constructor
TerrainPatch::TerrainPatch( unsigned int size, double err, GLfloat *vertices )
  : errBound(err), tessLevel(0), vertices(vertices)
{
  int i=0;

  // Size of the square patch (just on side)
  this->size = size;

  this->allIndices = new int[this->size*this->size];

  // Indices used for rendering
  this->numFinalIndices = 0;
  this->maxFinalIndices = this->size;
  this->finalIndices = (GLuint*) malloc( this->maxFinalIndices * sizeof(this->finalIndices[0]));

  for (i=0; i<4; i++)
    this->neighbors[i] = NULL;

  this->minCoordsX = FLT_MAX;
  this->minCoordsY = FLT_MAX;
  this->minCoordsZ = FLT_MAX;
  this->maxCoordsX = -FLT_MAX;
  this->maxCoordsY = -FLT_MAX;
  this->maxCoordsZ = -FLT_MAX;
}


//////////////////////////////////////////////////////////////////////////////
// Destructor
TerrainPatch::~TerrainPatch()
{
  if (this->finalIndices)
    free(this->finalIndices);

  delete [] this->allIndices;
}

//////////////////////////////////////////////////////////////////////////////
// Initialize the patch.
void TerrainPatch::Init()
{
  float heightTopLeft, heightTopRight, heightBottomRight, heightBottomLeft;
  float tessHeight;
  unsigned int x0,y0,xi,yi,x,y, totErr, skip;
  float distFromLeft, distFromRight, distFromTop, distFromBottom;
  float sumErr;

  int numErrors = (int)sqrt(this->size)+1;
  float *errors = new float[numErrors];

  for (int i=0; i<numErrors; i++)
    errors[i] = 0;

  // Calculate height errors for each tesselation level
  for (int i=1; i<numErrors; i++)
  {
    tessHeight =0;
    skip = (int)pow(2,i);

    sumErr = 0;
    totErr = 0;

    for (y0=0; y0<this->size-skip; y0+=skip)
    {
      for (x0=0; x0<this->size-skip; x0+=skip)
      {
        for (yi=1; yi<skip; yi++)
        {
          for (xi=1; xi<skip; xi++)
          {
            x = xi + x0;
            y = yi + y0;

            // Precentage distances from each side of the enclosing box
            distFromLeft = (float)xi / (float)skip;
            distFromRight = 1-distFromLeft;
            distFromTop = (float)yi / (float)skip;
            distFromBottom = 1-distFromTop;

            // Heights, moving clockwise around the square starting from the
            // top left.
            heightTopLeft = this->GetVertexPosZ(
                 this->allIndices[y0*this->size+x0]);
            heightTopRight = this->GetVertexPosZ(
                this->allIndices[y0*this->size+x0+skip]);
            heightBottomRight = this->GetVertexPosZ(
                this->allIndices[(y0+skip)*this->size+x0+skip]);
            heightBottomLeft = this->GetVertexPosZ(
                this->allIndices[(y0+skip)*this->size+x0]);

            // The height of the triangle above/below this vertex(x,y) 
            // when rendered at this (i) tesselation level.
            tessHeight = distFromLeft * distFromTop * heightTopLeft + 
              distFromRight * distFromTop * heightTopRight +
              distFromRight * distFromBottom * heightBottomRight +
              distFromLeft * distFromBottom * heightBottomLeft;

            sumErr += fabs(this->GetVertexPosZ(this->allIndices[y*this->size+x]) - tessHeight);
            totErr++;

          }
        }
      }
    }

    errors[i] = sumErr / totErr;
  }

  // Find the appropriate tesselation level
  for (int i=0; i<numErrors; i++)
  {
    if (errors[i] < this->errBound)
      this->tessLevel=i;
  }

  delete [] errors;
}


//////////////////////////////////////////////////////////////////////////////
// Attach a neighbor to a direction
void TerrainPatch::SetNeighbor( TerrainPatch *neigh, Direction dir)
{
  this->neighbors[dir] = neigh;
}


//////////////////////////////////////////////////////////////////////////////
// Set the vertex index for the local x,y coordinate inside the patch
void TerrainPatch::SetVertex(short x, short y, unsigned int vertex)
{
  this->allIndices[y*this->size+x] = vertex;

  double coordX = this->GetVertexPosX(vertex);
  double coordY = this->GetVertexPosY(vertex);
  double coordZ = this->GetVertexPosZ(vertex);

  // Min and max vertex coordinates
  if (coordX < this->minCoordsX)
    this->minCoordsX = coordX;
  if (coordX > this->maxCoordsX)
    this->maxCoordsX = coordX;

  if (coordY < this->minCoordsY)
    this->minCoordsY = coordY;
  if (coordY > this->maxCoordsY)
    this->maxCoordsY = coordY;

  if (coordZ < this->minCoordsZ)
    this->minCoordsZ = coordZ;
  if (coordZ > this->maxCoordsZ)
    this->maxCoordsZ = coordZ;
}

//////////////////////////////////////////////////////////////////////////////
// Create the triangle strip for this patch
void TerrainPatch::CreateStrip(unsigned int index, IndexList *indexList,
    IndexList *odeIndexList, bool &facing)
{
  unsigned int x,y;
  int i=0;
  int skip = (int)pow(2,this->tessLevel);
  int v[6];

  // Adjust the border allIndices to fix any gaps
  this->FixBorders();

  for (y=0; y< this->size-1; y+=skip)
  {

    // Need to double-up the beginning vertex of each row other than the
    // first and last. This creates degenerate triangles used to stich
    // together the rows of the patch
    if (y>0 || index > 0)
    {
      this->AddStripIndex(this->allIndices[(y+skip)*this->size], indexList);
    }

    // Generate the indices
    for (x=0; x<this->size; x+=skip)
    {
      if (x+skip < this->size)
      {
        v[0] = this->allIndices[(y+skip)*this->size+x];
        v[1] = this->allIndices[y*this->size+x];
        v[2] = this->allIndices[(y+skip)*this->size+x+skip];

        v[3] = this->allIndices[(y+skip)*this->size+x+skip];
        v[4] = this->allIndices[y*this->size+x];
        v[5] = this->allIndices[y*this->size+x+skip];

        if (this->IsValidTriangle(v[0], v[1], v[2]))
          for (i=0;i<3;i++)
            odeIndexList->Push(v[i]);

        if (this->IsValidTriangle(v[3], v[4], v[5]))
          for (i=3;i<6;i++)
            odeIndexList->Push(v[i]);
      }

      this->AddStripIndex(this->allIndices[(y+skip)*this->size+x], indexList);
      this->AddStripIndex(this->allIndices[y*this->size+x], indexList);

      if (x+skip > this->size-1)
        this->AddStripIndex(this->allIndices[y*this->size+x], indexList);
    }
  }

  // Calculate the normals
  for (i=0; i<this->numFinalIndices-3; i++)
  {
    if (this->IsValidTriangle(this->finalIndices[i],
          this->finalIndices[i+1],this->finalIndices[i+2]))
    {
      // Triangles alternate rotation.
      // Calculate normal for each triangle and add the normal to the
      // triangle's allIndices
      if (facing)
        this->CalcNormal(this->finalIndices[i], this->finalIndices[i+2], this->finalIndices[i+1]);
      else
        this->CalcNormal(this->finalIndices[i], this->finalIndices[i+1], this->finalIndices[i+2]);

    } 
      facing = !facing;
  }

  facing = !facing;
}

//////////////////////////////////////////////////////////////////////////////
// Return the tesselation level
unsigned short TerrainPatch::GetTessLevel()
{
  return this->tessLevel;
}

//////////////////////////////////////////////////////////////////////////////
// Adjust the border allIndices to fix any gaps
void TerrainPatch::FixBorders()
{
  int mySkip = (int)pow(2,this->tessLevel);
  int neighSkip = mySkip;
  int x,y;
  int idist;
  float fdist;

  // Fix the TOP border of the top neighbor has coarser tesselation level
  if (this->neighbors[TOP] && 
      this->neighbors[TOP]->GetTessLevel() > this->tessLevel)
  {
    neighSkip = (int)pow(2,this->neighbors[TOP]->GetTessLevel());
    y=this->size-1;

    for (x=this->size-1; x>=0; x-=mySkip)
    {
      fdist = (float)x/neighSkip;
      idist = (int)(x/neighSkip);

      if ( fdist != idist)
      {
        if (fabs(idist-fdist)<=.5)
        {
          this->allIndices[y*this->size+x] = 
            this->allIndices[(y*this->size+(idist*neighSkip - x))+x];
        } else {
          this->allIndices[y*this->size+x] = 
              this->allIndices[(y*this->size+((idist+1)*neighSkip - x))+x];
        }
      }
    }

  }

  // Fix the RIGHT border of the top neighbor has coarser tesselation level
  if (this->neighbors[RIGHT] &&
      this->neighbors[RIGHT]->GetTessLevel() > this->tessLevel)
  {
    neighSkip = (int)pow(2,this->neighbors[RIGHT]->GetTessLevel());
    x=this->size-1;

    for (y=mySkip; y<(int)(this->size); y+=mySkip)
    {
      fdist = (float)y/neighSkip;
      idist = (int)(y/neighSkip);

      if ( fdist != idist)
      {
        if (fabs(idist-fdist)<=.5)
        {
          this->allIndices[y*this->size+x] = 
              this->allIndices[(y+(idist*neighSkip - y))*this->size+x];
        } else {
          this->allIndices[y*this->size+x] = 
              this->allIndices[(y+((idist+1)*neighSkip - y))*this->size+x];
        }
      }
    }
  }

  // Fix the BOTTOM border of the top neighbor has coarser tesselation level
  if (this->neighbors[BOTTOM] &&
      this->neighbors[BOTTOM]->GetTessLevel() > this->tessLevel)
  {
    neighSkip = (int)pow(2,this->neighbors[BOTTOM]->GetTessLevel());
    y=0;

    for (x=this->size-1; x>=0; x-=mySkip)
    {
      fdist = (float)x/neighSkip;
      idist = (int)(x/neighSkip);

      if ( fdist != idist)
      {
        if (fabs(idist-fdist)<=.5)
        {
          this->allIndices[y*this->size+x] = 
            this->allIndices[(y*this->size+(idist*neighSkip - x))+x];
        } else {
          this->allIndices[y*this->size+x] = 
              this->allIndices[(y*this->size+((idist+1)*neighSkip - x))+x];
        }
      }
    }
  }

  // Fix the LEFT border of the top neighbor has coarser tesselation level
  if (this->neighbors[LEFT] &&
      this->neighbors[LEFT]->GetTessLevel() > this->tessLevel)
  {
    neighSkip = (int)pow(2,this->neighbors[LEFT]->GetTessLevel());
    x=0;

    for (y=mySkip; y<(int)(this->size); y+=mySkip)
    {
      fdist = (float)y/neighSkip;
      idist = (int)(y/neighSkip);

      if ( fdist != idist)
      {
        if (fabs(idist-fdist)<=.5)
        {
          this->allIndices[y*this->size+x] =
              this->allIndices[(y+(idist*neighSkip - y))*this->size+x];
        } else {
          this->allIndices[y*this->size+x] =
              this->allIndices[(y+((idist+1)*neighSkip - y))*this->size+x];
        }
      }
    }
  }
}


//////////////////////////////////////////////////////////////////////////////
// Triangle is valid (not degenerate) if the points do not coincide
bool TerrainPatch::IsValidTriangle( int i1, int i2, int i3 )
{
  return i1 != i2 && i1!=i3 && i2!=i3;
}

//////////////////////////////////////////////////////////////////////////////
// Add a vertex to the index array
void TerrainPatch::AddStripIndex(unsigned int index, IndexList *indexList)
{
  if (this->numFinalIndices >= this->maxFinalIndices)
  {
    this->maxFinalIndices += 10;

    this->finalIndices = (GLuint*) realloc( this->finalIndices, 
        this->maxFinalIndices * sizeof(this->finalIndices[0]));
  }

  this->finalIndices[this->numFinalIndices++] = index;
  indexList->Push(index);
}

//////////////////////////////////////////////////////////////////////////////
// Render this patch
void TerrainPatch::Render()
{
  //glDrawElements( GL_TRIANGLE_STRIP, this->numFinalIndices , GL_UNSIGNED_INT, this->finalIndices );
}


//////////////////////////////////////////////////////////////////////////////
// Return the X Coord of the vertex[index]
double TerrainPatch::GetVertexPosX(unsigned int index)
{
  return this->vertices[index*12+9];
}

//////////////////////////////////////////////////////////////////////////////
// Return the Y Coord of the vertex[index]
double TerrainPatch::GetVertexPosY(unsigned int index)
{
  return this->vertices[index*12+10];
}

//////////////////////////////////////////////////////////////////////////////
// Return the Z Coord of the vertex[index]
double TerrainPatch::GetVertexPosZ(unsigned int index)
{
  return this->vertices[index*12+11];
}

//////////////////////////////////////////////////////////////////////////////
// Calcuate the normal for a single triangle
void TerrainPatch::CalcNormal(unsigned int one, unsigned int two, 
    unsigned int three)
{
  double aX, aY, aZ;
  double bX, bY, bZ;
  double normX, normY, normZ;

  aX = this->vertices[one*12+9] - this->vertices[two*12+9];
  aY = this->vertices[one*12+10] - this->vertices[two*12+10];
  aZ = this->vertices[one*12+11] - this->vertices[two*12+11];

  bX = this->vertices[one*12+9] - this->vertices[three*12+9];
  bY = this->vertices[one*12+10] - this->vertices[three*12+10];
  bZ = this->vertices[one*12+11] - this->vertices[three*12+11];

  // Cross Product
  normX =  aY * bZ - aZ * bY;
  normY = -aX * bZ + aZ * bX;
  normZ =  aX * bY - aY * bX;
 
  this->vertices[one*12+6] += normX;
  this->vertices[one*12+7] += normY;
  this->vertices[one*12+8] += normZ;

  this->vertices[two*12+6] += normX;
  this->vertices[two*12+7] += normY;
  this->vertices[two*12+8] += normZ;

  this->vertices[three*12+6] += normX;
  this->vertices[three*12+7] += normY;
  this->vertices[three*12+8] += normZ;
}