roadgeom.cc

来自「机器人人3D仿真工具,可以加入到Simbad仿真环境下应用。」· CC 代码 · 共 630 行 · 第 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: Road geometry
 * Author: Nate Keonig, Andrew Howard
 * Date: 8 May 2003
 * CVS: $Id: RoadGeom.cc,v 1.12 2004/12/07 00:15:06 natepak Exp $
 */


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

#ifdef HAVE_TRIMESH

#include <assert.h>
#include <GL/glu.h>
#include <GL/glut.h>
#include "Image.hh"
#include "RoadGeom.hh"


//////////////////////////////////////////////////////////////////////////////
// Constructor
RoadGeom::RoadGeom( Body *body, dSpaceID spaceId )
    : Geom( spaceId )
{

  this->scale = 0.50;
  this->stepsX = 20000;
  this->stepsY = 33;

  this->trackScale = 10.0;
  this->profileScale = 1.0;

  this->trackPointCount = 0;
  this->trackPointMaxCount = 0;
  this->trackPoints = NULL;

  this->signCount = 0;
  this->signMaxCount = 0;
  this->signs = NULL;

  this->vertexCount = 0;
  this->vertexMaxCount = 0;
  this->vertices = NULL;
  this->normals = NULL;

  this->indexCount = 0;
  this->indexMaxCount = 0;
  this->indices = NULL;

  this->patchSize = 10.0;

  // Initialize the mesh
  this->InitMesh();

  // Create an object for managing trimesh data
  this->tridata = dGeomTriMeshDataCreate();

  // Construct the trimesh data
  dGeomTriMeshDataBuildSimple(this->tridata,
                              (dReal*) this->vertices, this->vertexCount,
                              this->indices, this->indexCount);
  
  // Create and ODE trimesh geom
  this->trimesh = dCreateTriMesh(spaceId, this->tridata, NULL, NULL, NULL);

  // Initialize underlying geom
  this->SetGeom(body, this->trimesh, NULL, false);
  
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Destructor
RoadGeom::~RoadGeom()
{
  int i;
  
  // Clean up
  dGeomDestroy(this->trimesh);
  dGeomTriMeshDataDestroy(this->tridata);

  free(this->indices);
  free(this->vertices);

  for (i = 0; i < this->trackPointCount; i++)
    free(this->trackPoints[i].profilePoints);
  free(this->trackPoints);  

  for (i = 0; i < this->signCount; i++)
    free(this->signs[i].text);
  free(this->signs);  

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Add a point to the track
int RoadGeom::AddTrackPoint(GzVector pos)
{
  size_t size;
  TrackPoint *point;
  
  if (this->trackPointCount >= this->trackPointMaxCount)
  {
    this->trackPointMaxCount += 1024;
    size = sizeof(this->trackPoints[0]) * this->trackPointMaxCount;
    this->trackPoints = (TrackPoint*) realloc(this->trackPoints, size);
  }  

  point = this->trackPoints + this->trackPointCount++;
  point->pos = pos;
  point->profilePointCount = 0;
  point->profilePointMaxCount = 0;
  point->profilePoints = NULL;

  return this->trackPointCount - 1;
}


//////////////////////////////////////////////////////////////////////////////
// Add a point to the profile
void RoadGeom::AddProfilePoint(int index, double pz)
{
  size_t size;
  TrackPoint *point;

  assert(index >= 0 && index < this->trackPointCount);
  point = this->trackPoints + index;
  
  if (point->profilePointCount >= point->profilePointMaxCount)
  {
    point->profilePointMaxCount += 8;
    size = sizeof(point->profilePoints[0]) * point->profilePointMaxCount;
    point->profilePoints = (GzVector*) realloc(point->profilePoints, size);
  }  
  point->profilePoints[point->profilePointCount++] = GzVectorSet(0, 0, pz);
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Add a sign.
void RoadGeom::AddSign(GzVector pos, const char *text)
{
  size_t size;
  SignData *sign;
  
  if (this->signCount >= this->signMaxCount)
  {
    this->signMaxCount += 254;
    size = sizeof(this->signs[0]) * this->signMaxCount;
    this->signs = (SignData*) realloc(this->signs, size);
  }  

  sign = this->signs + this->signCount++;
  sign->pos = pos;
  sign->text = strdup(text);

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Initialize the mesh
void RoadGeom::InitMesh()
{
  int i, j;
  dReal *vertex;

  // Allocate vertex storage
  this->vertexCount = 0;
  this->vertexMaxCount = this->stepsX * this->stepsY;
  this->vertices = (dVector3*) realloc(this->vertices,
                                       sizeof(this->vertices[0]) * this->vertexMaxCount);
  this->normals = (GzVector*) realloc(this->normals,
                                      sizeof(this->normals[0]) * this->vertexMaxCount);
    
  // Initialize vertices
  for (i = 0; i < this->stepsX; i++)
  {
    for (j = 0; j < this->stepsY; j++)
    {
      vertex = this->vertices[this->vertexCount++];
      vertex[0] = i * this->scale;
      vertex[1] = j * this->scale;
      vertex[2] = i * 0.05;
    }
  }

  // Allocate index storage
  this->indexCount = 0;
  this->indexMaxCount = this->stepsX * this->stepsY * 6;
  this->indices = (int*) realloc(this->indices,
                                 sizeof(this->indices[0]) * this->indexMaxCount);

  // Construct triangle strips
  for (i = 0; i < this->stepsX - 1; i++)
  {
    for (j = 0; j < this->stepsY - 1; j++)
    { 
      assert(this->indexCount + 6 <= this->indexMaxCount);
      this->indices[this->indexCount++] = (i + 0) * this->stepsY + j;
      this->indices[this->indexCount++] = (i + 1) * this->stepsY + j;
      this->indices[this->indexCount++] = (i + 0) * this->stepsY + j + 1;     
      this->indices[this->indexCount++] = (i + 0) * this->stepsY + j + 1;
      this->indices[this->indexCount++] = (i + 1) * this->stepsY + j;
      this->indices[this->indexCount++] = (i + 1) * this->stepsY + j + 1;
    }
  }
  
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Regenerate the mesh
void RoadGeom::UpdateMesh(GzVector pos)
{
  int i, j;
  int si, ti;
  int n0, n1, n2, n3;
  double s, sm, t, tm;
  TrackPoint *point;
  GzVector o, ot, p;
  dReal *vertex;

  /*
  // HACK
  // Find the nearest point in the track
  trackMin = 0;
  distMin = DBL_MAX;
  
  for (i = 0; i < this->trackPointCount; i++)
  {
  p = this->trackPoints[i];
  p = GzVectorSub(pos, p);
  s = GzVectorDot(p, p);

  if (s < distMin)
  {
  distMin = s;
  trackMin = i;
  }
  }
  */
  
  // Consider points along the track
  for (i = 0; i < this->stepsX; i++)
  {
    //s = (i - this->stepsX / 2) * this->scale;
    s = i * this->scale;

    // Figure out where we are in the track
    si = (int) (floor(s / this->trackScale));
    sm = s / this->trackScale - si;

    si += 1;
        
    //printf("s si sm %f %d %f\n", s, si, sm);
    assert(si >= 1 && si + 2 < this->trackPointCount);
      
    // Use a spline to interpolate our position
    o = this->SplineCR(sm,
                       this->trackPoints[si - 1].pos,
                       this->trackPoints[si + 0].pos,
                       this->trackPoints[si + 1].pos,
                       this->trackPoints[si + 2].pos);
    ot = this->SplineCRtan(sm,
                           this->trackPoints[si - 1].pos,
                           this->trackPoints[si + 0].pos,
                           this->trackPoints[si + 1].pos,
                           this->trackPoints[si + 2].pos);
    //printf("%f %f %f\n", o.x, o.y, o.z);

    assert(si >= 0 && si < this->trackPointCount);
    point = this->trackPoints + si;      

    // TESTING HACK
    if (point->profilePointCount >= 4)
    {
      point->profilePoints[0].z = -o.z - 0.10;
      point->profilePoints[1].z = -o.z - 0.10;
      point->profilePoints[point->profilePointCount - 2].z = -o.z - 0.10;    
      point->profilePoints[point->profilePointCount - 1].z = -o.z - 0.10;
    }

    // Consider points along the profile
    for (j = 0; j < this->stepsY; j++)
    {
      vertex = this->vertices[i * this->stepsY + j];

      t = (j - this->stepsY / 2) * this->scale;