I have spent quite some time going through the Road model, and thought I
would post the results for everyone else.
The road file contains two types of lines, waypoint and track. The
format is as follows:
waypoint [display distance] [waypoint id] [X Position] [Y Position] [Z
Position]
track [display distance] [waypoint id] [X Position] [Y Position] [Z
Position] [No of Profile Points] [Profile Height 1] [Profile Height
2] ...
The "waypoints" create a road sign inside the simulation, which displays
the [waypoint id] and the [display distance]. The "tracks" add a new
point in the road path; a spline is used to connect sequential track
points. In the track data line, the [display distance] and [waypoint id]
have no effect.
Additionally, inside the world file, the parameter <roadSpacing> is
somewhat misleading. The first value is the number of intermediate
spline points that will be added between sequential track points. Note
that the total number of points (~= track Points * roadSpacing) must
equal or exceed 10,000 for the the native Gazebo road to work properly.
The second value is the lateral distance in meters between sequential
profile points.
Once I adjusted the <roadSpacing> value correctly, the road would
render. However, for short roads, the hard-coded 10,000 spline points
was causing serious CPU issues. Also, when large changes in the Z
Position were present, the road seemed to narrow considerably and
occasionally invert.
To fix the above issues, as well as some data redundancy in the road
file, I have created a gazebo plugin called (imaginatively) "Road2". The
code is attached, or available at
http://swilliams.homelinux.net/websvn/listing.php?repname=Gazebo+Road
+Plugin&path=%2Ftrunk%2Fsrc%2F&rev=0&sc=0
Instructions on how to compile a plugin are available on the main Gazebo
site. Additionally, I had to copy the Image.hh file from the Gazebo
source to /usr/local/include/gazebo and include the path
path /usr/include/libxml2 in the compile statement. Notes on how to use
the plugin are written the comments of Road2.cc
Hopefully some of this will help someone,
Stephen Williams
/*
* 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 road
* Author: Andrew Howard
* Date: 23 Oct 2004
* CVS: $Id: Road.cc,v 1.17 2005/05/06 17:09:30 natepak Exp $
*/
/* Desc: Road2 geometry
* Modified By: Stephen Williams
* Date: 3 July 2007
* CVS: ???
* Notes: Modification of the original RoadGeom body. Changes the syntax of the road file, the model parameters, the tangent calculation, and the way the spline is done
*/
/*
Generates a road surface using a Catmull-Rom spline based on points from a descriptor file.
@par libgazebo interfaces
This model has no libgazebo interfaces.
@par Player drivers
No Player drivers are available.
@par Attributes
Road models can be instantiated using the <tt><model:Road2></tt> tag.
The following attributes are supported.
@htmlinclude default_attr_include.html
- hardness (float tuple)
- Set spring-damper parameters for contact joints
- Default: dInfinity 0
- friction (float tuple)
- Set friction coefficients
- Default: 1 1
- color (float tuple)
- RGB color.
- Default: 0 0 0
- textureFile (string, filename)
- Texture image file.
- Default: empty
- textureSize (float tuple, meters)
- Size of the texture image when projected onto the ground plane (m).
- Default: 2.0 2.0
- profileSpacing (float, meters)
- Lateral distance between sequential points for the profile defined in the roadFile.
- Default: 0.10
- profileIntermediatePoints (int)
- The number of spline interpolated points to be added between sequential profile points
- Default: 1
- trackIntermediatePoints (int)
- The number of spline interpolated points to be added between sequential track points
- Default: 10
- roadFile (string, filename)
- Road data filename. A road file is a plain text file with a specific format.
- Default: empty
- The first word of each line specifies the type of data on the line. Subsequent information is type specific. The following types are supported
- waypoint (draws a road sign in the simulation)
X Position
Y Position
Z Position
Waypoint ID
Distance to Display (meters)
Example: waypoint 10.0 12.0 0.10 1 5.50
- trackpoint (adds a point to the raod path)
X position
Y Position
Z Position
Example: trackpoint 10.0 12.0 0.05
- profile (specifies the track profile by listing a sequences of heights/Z Positions. The lateral distance between profile points is controlled by profileSpacing.)
Z Position 1
Z Position 2
.
.
.
@par Bodies
The following bodies are created by this model.
@htmlinclude default_body_include.html
@par Example
@verbatim
<model:Road2>
<plugin>road2.so</plugin>
<xyz>0 0 0</xyz>
</model:Road2>
@endverbatim
@par Authors
Andrew Howard
@par Modified By
Stephen Williams
*/
/// @}
#if HAVE_CONFIG_H
#include <config.h>
#endif
#include <assert.h>
#include <errno.h>
#include <string.h>
#include <stdio.h>
#include <gazebo.h>
#include <gazebo/Error.hh>
#include <gazebo/World.hh>
#include <gazebo/WorldFile.hh>
#include <gazebo/ModelFactory.hh>
#include <gazebo/Body.hh>
#include "Road2Geom.hh"
#include "Road2.hh"
//////////////////////////////////////////////////////////////////////////////
// Register this model
GZ_REGISTER_PLUGIN("Road2", Road2);
//////////////////////////////////////////////////////////////////////////////
// Constructor
Road2::Road2( World *world )
: Model( world )
{
this->body = NULL;
this->roadGeom = NULL;
return;
}
//////////////////////////////////////////////////////////////////////////////
// Destructor
Road2::~Road2()
{
if (this->body)
delete this->body;
if (this->roadGeom)
delete this->roadGeom;
this->body = NULL;
this->roadGeom = NULL;
return;
}
//////////////////////////////////////////////////////////////////////////////
// Load the model
int Road2::Load(WorldFile *file, WorldFileNode *node)
{
// Create the ODE objects
if (this->LoadODE(file, node) != 0)
return -1;
// Load the road description
if (this->LoadRoad(file, node) != 0)
return -1;
return 0;
}
//////////////////////////////////////////////////////////////////////////////
// Load ODE objects
int Road2::LoadODE(WorldFile *file, WorldFileNode *node)
{
const char *textureFile;
this->body = new Body(this->world, NULL, true);
this->AddBody(this->body, true);
this->roadGeom = new Road2Geom(this->body, this->modelSpaceId);
// Surface hardness
this->roadGeom->SetHardness(node->GetTupleDouble("surfaceHardness", 0, dInfinity),
node->GetTupleDouble("surfaceHardness", 1, 0));
// Surface friction
this->roadGeom->SetFriction(node->GetTupleDouble("surfaceFriction", 0, 1.0),
node->GetTupleDouble("surfaceFriction", 1, 1.0));
// Color
this->roadGeom->SetColor(node->GetColor("color", GzColor(0.7, 0.7, 0)));
// Apply a texture
textureFile = node->SearchFilename("textureFile", GAZEBO_TEXTURES_PATH, NULL);
if (textureFile)
{
PRINT_MSG1(1, "loading texture file [%s]", textureFile);
if (this->roadGeom->SetTexture2DFile(textureFile) != 0)
{
PRINT_ERR("unable to load texture file");
return -1;
}
}
this->roadGeom->SetTexture2DSize(node->GetPosition("textureSize", GzVectorSet(2.0, 2.0, 0.0)));
// Set Road construction parameters
this->roadGeom->SetTrackIntermediatePoints(node->GetInt("trackIntermediatePoints", 10));
this->roadGeom->SetProfileIntermediatePoints(node->GetInt("profileIntermediatePoints", 1));
this->roadGeom->SetProfileSpacing(node->GetDouble("profileSpacing", 0.10));
dGeomSetCategoryBits((dGeomID) this->modelSpaceId, GZ_FIXED_COLLIDE);
dGeomSetCollideBits((dGeomID) this->modelSpaceId, ~GZ_FIXED_COLLIDE);
return 0;
}
//////////////////////////////////////////////////////////////////////////////
// Load the road description
int Road2::LoadRoad(WorldFile *file, WorldFileNode *node)
{
const char *filename;
FILE *rfile;
char line[1024];
char *tokens[1024];
int lineCount, tokenCount;
int i, wp;
double x, y, z;
GzVector p;
char *type;
char text[128];
double dist;
// Get the road file
filename = node->SearchFilename("roadFile", NULL, NULL);
if (!filename)
{
PRINT_ERR("no road file specified");
return -1;
}
// Open file
rfile = fopen(filename, "r");
if (rfile == NULL)
{
PRINT_ERR2("unable to open [%s] : %s", filename, strerror(errno));
return -1;
}
lineCount = 0;
// Parse the file
while (true)
{
if (fgets(line, sizeof(line), rfile) == NULL)
break;
lineCount++;
// Break line into tokens
tokenCount = 0;
tokens[tokenCount] = strtok(line, " ");
while (tokens[tokenCount])
{
tokenCount++;
assert(tokenCount < (int) (sizeof(tokens) / sizeof(tokens[0])));
tokens[tokenCount] = strtok(NULL, " ");
}
// if (tokenCount < 5)
// {
// PRINT_ERR1("syntax error in road file, line [%d]", lineCount);
// break;
// }
// Get type from the first token, then load the rest of the line accordingly
type = tokens[0];
// If this is a waypoint entry
if (strcmp(type, "waypoint") == 0)
{
// Get data from tokens
x = atof(tokens[1]);
y = atof(tokens[2]);
z = atof(tokens[3]);
wp = atoi(tokens[4]);
dist = atof(tokens[5]);
// Compute position in global cs
p = GzVectorSet(x, y, z);
p = GzVectorSub(p, this->world->utmOffset);
snprintf(text, sizeof(text), "WP %d -- %8.1f m", wp, dist);
this->roadGeom->AddSign(p, text);
}
// If this is a track entry
else if (strcmp(type, "trackpoint") == 0)
{
// Get data from tokens
x = atof(tokens[1]);
y = atof(tokens[2]);
z = atof(tokens[3]);
// Compute track point in global cs
p = GzVectorSet(x, y, z);
p = GzVectorSub(p, this->world->utmOffset);
this->roadGeom->AddTrackPoint(p);
}
// If this is a profile entry
else if (strcmp(type, "profile") == 0)
{
// Extract profile points
for (i = 1 ; i < tokenCount ; i++)
{
z = atof(tokens[i]);
this->roadGeom->AddProfilePoint( z );
}
}
}
// See if we got to the end
if (!feof(rfile))
{
fclose(rfile);
return -1;
}
fclose(rfile);
this->roadGeom->InitMesh();
return 0;
}
//////////////////////////////////////////////////////////////////////////////
// Initialize the model
int Road2::Init(WorldFile *file, WorldFileNode *node)
{
//int i;
//Model *model;
/* TODO
this->trackModel = NULL;
for (i = 0; i < this->world->GetNumModels(); i++)
{
model = this->world->GetModels()[i];
if (strcmp(model->GetId(), "car1") == 0) // HACK
this->trackModel = model;
}
assert(this->trackModel);
this->trackPose = this->trackModel->GetPose();
*/
this->roadGeom->UpdateMesh(this->trackPose.pos);
return 0;
}
//////////////////////////////////////////////////////////////////////////////
// Finalize the model
int Road2::Fini()
{
return 0;
}
//////////////////////////////////////////////////////////////////////////////
// Update model
void Road2::Update(double step)
{
/* TODO
GzPose pose;
GzVector d;
if (!this->trackModel)
return;
pose = this->trackModel->GetPose();
d = GzVectorSub(pose.pos, this->trackPose.pos);
if (fabs(d.x) > 1.0 || fabs(d.y) > 1.0) // HACK
{
//this->roadGeom->UpdateMesh(pose.pos);
this->trackPose = pose;
}
*/
return;
}
/*
* 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 road
* Author: Andrew Howard
* Date: 23 Oct 2004
* CVS: $Id: Road.hh,v 1.3 2004/11/17 03:53:43 natepak Exp $
*/
/* Desc: Road2 geometry
* Modified By: Stephen Williams
* Date: 3 July 2007
* CVS: ???
* Notes: Modification of the original RoadGeom body. Changes the syntax of the road file, the model parameters, the tangent calculation, and the way the spline is done
*/
#ifndef ROAD2_HH
#define ROAD2_HH
// Gazebo includes
#include <gazebo/Model.hh>
#include <gazebo/Body.hh>
// Local Includes
#include "Road2Geom.hh"
// Forward declarations
//class Road2Geom;
// Definitions
#define GAZEBO_TEXTURES_PATH "./"
/// @brief A linear road model
class Road2 : public Model
{
/// @brief Construct
public: Road2(World *world);
/// @brief Destructor
public: virtual ~Road2();
/// @brief Load the model
public: virtual int Load(WorldFile *file, WorldFileNode *node);
/// @brief Load ODE stuff
private: int LoadODE(WorldFile *file, WorldFileNode *node);
/// @brief Load road description
private: int LoadRoad(WorldFile *file, WorldFileNode *node);
/// @brief Initialize the model
public: virtual int Init(WorldFile *file, WorldFileNode *node);
/// @brief Finalize the model
public: virtual int Fini();
/// @brief Update the model state
public: virtual void Update(double step);
// Components
private: Body *body;
private: Road2Geom *roadGeom;
/// Model we are tracking
private: Model *trackModel;
/// Current track pose
private: GzPose trackPose;
};
#endif
/*
* 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: Andrew Howard
* Date: 23 Oct 2004
* CVS: $Id: Road.hh,v 1.3 2004/11/17 03:53:43 natepak Exp $
*/
/* Desc: Road2 geometry
* Modified By: Stephen Williams
* Date: 3 July 2007
* CVS: ???
* Notes: Modification of the original RoadGeom body. Changes the syntax of the road file, the way the spline is done, and the scaling parameters
* Road File Details:
*/
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
//#ifdef HAVE_TRIMESH
#include <assert.h>
#include <GL/glu.h>
#include <GL/glut.h>
#include <gazebo/Image.hh>
#include <gazebo/Error.hh> // allows message printing
#include "Road2Geom.hh"
//////////////////////////////////////////////////////////////////////////////
// Constructor
Road2Geom::Road2Geom( Body *body, dSpaceID spaceId )
: Geom( spaceId )
{
this->trackIntermediatePoints = 10;
this->trackPointCount = 0;
this->trackPointMaxCount = 0;
this->trackPoints = NULL;
this->track = NULL;
this->profileSpacing = 1.0;
this->profileIntermediatePoints = 10;
this->profilePointCount = 0;
this->profilePointMaxCount = 0;
this->profilePoints = NULL;
this->profile = 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;
// 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
Road2Geom::~Road2Geom()
{
int i;
// Clean up
dGeomDestroy(this->trimesh);
dGeomTriMeshDataDestroy(this->tridata);
free(this->indices);
free(this->vertices);
free(this->trackPoints);
free(this->track);
free(this->profilePoints);
free(this->profile);
for (i = 0; i < this->signCount; i++)
free(this->signs[i].text);
free(this->signs);
return;
}
//////////////////////////////////////////////////////////////////////////////
// Add a point to the track
void Road2Geom::AddTrackPoint(GzVector pos)
{
size_t size;
if (this->trackPointCount >= this->trackPointMaxCount)
{
this->trackPointMaxCount += 128;
size = sizeof(this->trackPoints[0]) * this->trackPointMaxCount;
this->trackPoints = (GzVector*) realloc(this->trackPoints, size);
}
this->trackPoints[trackPointCount++] = pos;
return;
}
//////////////////////////////////////////////////////////////////////////////
// Add a point to the profile
void Road2Geom::AddProfilePoint(double pz)
{
size_t size;
if (this->profilePointCount >= this->profilePointMaxCount)
{
this->profilePointMaxCount += 8;
size = sizeof(this->profilePoints[0]) * this->profilePointMaxCount;
this->profilePoints = (GzVector*) realloc(this->profilePoints, size);
}
this->profilePoints[profilePointCount].x = this->profilePointCount * this->profileSpacing;
this->profilePoints[profilePointCount].y = 0;
this->profilePoints[profilePointCount++].z = pz;
return;
}
//////////////////////////////////////////////////////////////////////////////
// Add a sign.
void Road2Geom::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 (this is now done after the road file is loaded)
void Road2Geom::InitMesh()
{
int i, j;
int n0, n1, n2, n3;
double t;
dReal* vertex;
size_t size;
// Calculate the total number (specified plus intermediate spline) of profile points, and the number of track points
int totalTrackPoints = (this->trackPointCount - 1) * this->trackIntermediatePoints + 1;
int totalProfilePoints = (this->profilePointCount - 1) * this->profileIntermediatePoints + 1;
// Allocate vertex storage
this->vertexCount = 0;
this->vertexMaxCount = totalTrackPoints * totalProfilePoints;
this->vertices = (dVector3*) realloc(this->vertices,
sizeof(this->vertices[0]) * this->vertexMaxCount);
this->normals = (GzVector*) realloc(this->normals,
sizeof(this->normals[0]) * this->vertexMaxCount);
// Allocate memory for the profile
size = sizeof(this->profile[0]) * totalProfilePoints;
this->profile = (GzVector*) realloc(this->profile, size);
// Calculate and store the profile spline (this only needs to be done once)
for( i = 0 ; i < this->profilePointCount-1 ; i++ )
{
for( j = 0 ; j <= this->profileIntermediatePoints ; j++ )
{
// Spline control point indexes
n0 = i - 1;
n1 = i + 0;
n2 = i + 1;
n3 = i + 2;
// Handle boundary conditions for spline fit
n0 = Min(Max(0, n0), this->profilePointCount - 1);
n1 = Min(Max(0, n1), this->profilePointCount - 1);
n2 = Min(Max(0, n2), this->profilePointCount - 1);
n3 = Min(Max(0, n3), this->profilePointCount - 1);
// Error Checking / Debugging
assert(n0 >= 0 && n0 < this->profilePointCount);
assert(n1 >= 0 && n1 < this->profilePointCount);
assert(n2 >= 0 && n2 < this->profilePointCount);
assert(n3 >= 0 && n3 < this->profilePointCount);
// Calculate the percentage between user defined points
t = (double)j / ( (double)this->profileIntermediatePoints + 1 );
// Use a spline to interpolate the profile altitude
this->profile[i*this->profileIntermediatePoints + j] = this->SplineCR(t, this->profilePoints[n0], this->profilePoints[n1], this->profilePoints[n2], this->profilePoints[n3] );
// Translate the profile to have a center at 0
this->profile[i*this->profileIntermediatePoints + j].x -= ( ((double)totalProfilePoints - 1) * this->profileSpacing )/2.0;
}
}
// Add the last point
this->profile[totalProfilePoints - 1] = this->profilePoints[this->profilePointCount - 1];
this->profile[totalProfilePoints - 1].x -= ( ((double)totalProfilePoints - 1) * this->profileSpacing )/2.0;
// Allocate memory for the track
size = sizeof(this->track[0]) * totalTrackPoints;
this->track = (GzVector*) realloc(this->track, size);
// Calculate and store the track spline (this only needs to be done once)
for( i = 0 ; i < this->trackPointCount-1 ; i++ )
{
for( j = 0 ; j <= this->trackIntermediatePoints ; j++ )
{
// Spline control point indexes
n0 = i - 1;
n1 = i + 0;
n2 = i + 1;
n3 = i + 2;
// Handle boundary conditions for spline fit
n0 = Min(Max(0, n0), this->trackPointCount - 1);
n1 = Min(Max(0, n1), this->trackPointCount - 1);
n2 = Min(Max(0, n2), this->trackPointCount - 1);
n3 = Min(Max(0, n3), this->trackPointCount - 1);
// Error Checking / Debugging
assert(n0 >= 0 && n0 < this->trackPointCount);
assert(n1 >= 0 && n1 < this->trackPointCount);
assert(n2 >= 0 && n2 < this->trackPointCount);
assert(n3 >= 0 && n3 < this->trackPointCount);
// Calculate the percentage between user defined points
t = (double)j / ( (double)this->trackIntermediatePoints + 1 );
// Use a spline to interpolate between track points
this->track[i*this->trackIntermediatePoints + j] = this->SplineCR(t, this->trackPoints[n0], this->trackPoints[n1], this->trackPoints[n2], this->trackPoints[n3] );
}
}
// Add the last point
this->track[totalTrackPoints - 1] = this->trackPoints[this->trackPointCount - 1];
// Initialize vertices
for (i = 0 ; i < totalTrackPoints ; i++)
{
for (j = 0 ; j < totalProfilePoints ; j++)
{
vertex = this->vertices[this->vertexCount++];
vertex[0] = i;
vertex[1] = j;
vertex[2] = 0.05;
}
}
// Allocate index storage
this->indexCount = 0;
this->indexMaxCount = totalTrackPoints * totalProfilePoints * 6;
this->indices = (int*) realloc(this->indices,
sizeof(this->indices[0]) * this->indexMaxCount);
// Construct triangle strips
for (i = 0; i < totalTrackPoints - 1; i++)
{
for (j = 0; j < totalProfilePoints - 1; j++)
{
assert(this->indexCount + 6 <= this->indexMaxCount);
this->indices[this->indexCount++] = (i + 0) * totalProfilePoints + j;
this->indices[this->indexCount++] = (i + 1) * totalProfilePoints + j;
this->indices[this->indexCount++] = (i + 0) * totalProfilePoints + j + 1;
this->indices[this->indexCount++] = (i + 0) * totalProfilePoints + j + 1;
this->indices[this->indexCount++] = (i + 1) * totalProfilePoints + j;
this->indices[this->indexCount++] = (i + 1) * totalProfilePoints + j + 1;
}
}
return;
}
//////////////////////////////////////////////////////////////////////////////
// Regenerate the mesh
void Road2Geom::UpdateMesh(GzVector pos)
{
int i, j;
int n0, n1, n2;
GzVector tangent;
dReal* vertex;
// Calculate total number of track and profile points
int totalTrackPoints = (this->trackPointCount - 1) * this->trackIntermediatePoints + 1;
int totalProfilePoints = (this->profilePointCount - 1) * this->profileIntermediatePoints + 1;
// Rotate the the profile to be perpendicular to the track tangent
for( i = 0 ; i < totalTrackPoints ; i++ )
{
// Tangent point indexes
// Handle boundary conditions for the central difference
if( i <= 0 )
{
n0 = 0;
n1 = 1;
n2 = 2;
}
else if( i >= totalTrackPoints-1 )
{
n0 = totalTrackPoints-3;
n1 = totalTrackPoints-2;
n2 = totalTrackPoints-1;
}
else
{
n0 = i - 1;
n1 = i - 0;
n2 = i + 1;
}
// Error Checking / Debugging
assert(n0 >= 0 && n0 < totalTrackPoints);
assert(n1 >= 0 && n1 < totalTrackPoints);
assert(n2 >= 0 && n2 < totalTrackPoints);
// Use a Central Differencing scheme to compute the tangent
tangent = this->SplineCDtan(this->track[n0], this->track[n1], this->track[n2]);
// Update Vertex points
// Rotate the profile about the center to be perpendicular to the tangent, then translate it to the track point
for( j = 0 ; j < totalProfilePoints ; j++ )
{
// move to the correct vertex
vertex = this->vertices[i * totalProfilePoints + j];
// Calculate the vertex position by rotating the profile point to be perpendicular to the tangent line, and translating it to the track point position
vertex[0] = this->track[i].x + profile[j].x*tangent.y;
vertex[1] = this->track[i].y + profile[j].x*-tangent.x;
vertex[2] = this->track[i].z + profile[j].z;
}
}
// Compute normal vectors
this->UpdateNormals();
// Construct the trimesh data
dGeomTriMeshDataBuildSimple(this->tridata,
(dReal*) this->vertices, this->vertexCount,
this->indices, this->indexCount);
dGeomTriMeshSetData(this->trimesh, this->tridata);
return;
}
//////////////////////////////////////////////////////////////////////////////
// Compute normal vectors on vertices
void Road2Geom::UpdateNormals()
{
int i, a, b, c;
dVector3 *v;
GzVector v0, v1, v2, n;
v = this->vertices;
// Reset the normals
for (i = 0; i < this->vertexCount; i++)
this->normals[i] = GzVectorZero();
// Consider each triangle
for (i = 0; i < this->indexCount; i += 3)
{
a = this->indices[i + 0];
b = this->indices[i + 1];
c = this->indices[i + 2];
// Compute the normal
v0 = GzVectorSet(v[a][0], v[a][1], v[a][2]);
v1 = GzVectorSet(v[b][0], v[b][1], v[b][2]);
v2 = GzVectorSet(v[c][0], v[c][1], v[c][2]);
n = GzVectorCross(GzVectorSub(v1, v0), GzVectorSub(v2, v1));
n = GzVectorUnit(n);
// Accumate normals across triangles
// We assume normals are being normalized by OpenGL
this->normals[a] = GzVectorAdd(this->normals[a], n);
this->normals[b] = GzVectorAdd(this->normals[b], n);
this->normals[c] = GzVectorAdd(this->normals[c], n);
}
return;
}
//////////////////////////////////////////////////////////////////////////////
// Compute a Catmull-Rom spline
GzVector Road2Geom::SplineCR(double t, GzVector p0, GzVector p1,
GzVector p2, GzVector p3)
{
GzVector q;
q.x = p1.x +
(-0.5*p0.x + 0.5*p2.x) * t +
(1*p0.x - 2.5*p1.x + 2*p2.x - 0.5*p3.x) * t*t +
(-0.5*p0.x + 1.5*p1.x - 1.5*p2.x + 0.5*p3.x) * t*t*t;
q.y = p1.y +
(-0.5*p0.y + 0.5*p2.y) * t +
(1*p0.y - 2.5*p1.y + 2*p2.y - 0.5*p3.y) * t*t +
(-0.5*p0.y + 1.5*p1.y - 1.5*p2.y + 0.5*p3.y) * t*t*t;
q.z = p1.z +
(-0.5*p0.z + 0.5*p2.z) * t +
(1*p0.z - 2.5*p1.z + 2*p2.z - 0.5*p3.z) * t*t +
(-0.5*p0.z + 1.5*p1.z - 1.5*p2.z + 0.5*p3.z) * t*t*t;
return q;
}
//////////////////////////////////////////////////////////////////////////////
// Compute a 3-Point Central Difference tangent (note: p0 -- point immediately before the point in question, p1 -- point in question, p2 -- point immediately after point in question
GzVector Road2Geom::SplineCDtan(GzVector p0, GzVector p1, GzVector p2)
{
GzVector q;
double dx1, dx2, dy1, dy2;
dx1 = p1.x - p0.x;
dy1 = p1.y - p0.y;
dx2 = p2.x - p1.x;
dy2 = p2.y - p1.y;
q.x = (dx2*dx2)*p0.x + (dx1*dx1 - dx2*dx2)*p1.x - (dx1*dx1)*p2.x;
q.y = (dy2*dy2)*p0.y + (dy1*dy1 - dy2*dy2)*p1.y - (dy1*dy1)*p2.y;
q.z = 0; // ignore elevation changes
q = GzVectorUnit(q);
return q;
}
//////////////////////////////////////////////////////////////////////////////
// Default render routine
void Road2Geom::Render(RenderOptions *opt)
{
int i, j;
int ni, nj, mi, mj;
GLuint listId;
RenderOptions listOpt;
// Recover stored display list for this camera
this->GetList(opt->cameraIndex, &listId, &listOpt);
// See if the current display list is dirty
this->dirty |= (listId == 0);
this->dirty |= (opt->displayMaterials != listOpt.displayMaterials);
this->dirty |= (opt->displayTextures != listOpt.displayTextures);
// Compute offset based on old camera pose
mi = (int) floor(listOpt.cameraPose.pos.x / this->patchSize + 0.5);
mj = (int) floor(listOpt.cameraPose.pos.y / this->patchSize + 0.5);
// Compute offset based on new camera pose
ni = (int) floor(opt->cameraPose.pos.x / this->patchSize + 0.5);
nj = (int) floor(opt->cameraPose.pos.y / this->patchSize + 0.5);
// If the offset has changed, we need to redraw
this->dirty |= (ni != mi);
this->dirty |= (nj != mj);
// Generate the display list
if (this->dirty)
{
if (listId == 0)
listId = glGenLists(1);
glNewList(listId, GL_COMPILE);
// Set material properties
if (opt->displayMaterials)
{
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, this->colorAmbient);
glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, this->colorDiffuse);
glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, this->colorSpecular);
glMaterialf(GL_FRONT_AND_BACK, GL_SHININESS, this->shininess);
}
if (opt->displayTextures && this->textureImage)
{
// Set up textures
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE);
// Build the mipmaps
// Assume image data is *not* aligned
glPixelStorei( GL_UNPACK_ALIGNMENT, 1);
gluBuild2DMipmaps(GL_TEXTURE_2D, this->textureImage->components,
this->textureImage->width, this->textureImage->height,
this->textureImage->format, this->textureImage->type,
this->textureImage->data);
glEnable(GL_TEXTURE_2D);
}
dReal *v;
GzVector n;
double tx, ty;
// Texture scale; each texture image is scaled such that it maps
// to a road patch of size textureSize
tx = 0.5 / this->textureSize.x;
ty = 0.5 / this->textureSize.y;
// Calculate total number of track and profile points
int totalTrackPoints = (this->trackPointCount - 1) * this->trackIntermediatePoints + 1;
int totalProfilePoints = (this->profilePointCount - 1) * this->profileIntermediatePoints + 1;
// Construct triangle strips (skipping the first two and last two points, as these cause problems)
for (i = 2 ; i < totalTrackPoints-2 ; i++)
{
// Test vertex to see if it is near the current camera position
v = this->vertices[totalProfilePoints * i + totalProfilePoints / 2];
if (fabs(v[0] - opt->cameraPose.pos.x) > opt->farClip)
continue;
if (fabs(v[1] - opt->cameraPose.pos.y) > opt->farClip)
continue;
glBegin(GL_TRIANGLE_STRIP);
// Construct triangle strips
for (j = 0 ; j < totalProfilePoints ; j++)
{
glTexCoord2f(i * tx, j * tx);
n = this->normals[totalProfilePoints * (i + 0) + j];
v = this->vertices[totalProfilePoints * (i + 0) + j];
glNormal3f(n.x, n.y, n.z);
glVertex3f(v[0], v[1], v[2]);
glTexCoord2f((i + 1) * tx, j * tx);
n = this->normals[totalProfilePoints * (i + 1) + j];
v = this->vertices[totalProfilePoints * (i + 1) + j];
glNormal3f(n.x, n.y, n.z);
glVertex3f(v[0], v[1], v[2]);
}
glEnd();
}
if (opt->displayTextures && this->textureImage)
glDisable(GL_TEXTURE_2D);
// Display signs
this->RenderSigns(opt, listOpt);
// Store list options
this->SetList(opt->cameraIndex, listId, *opt);
glEndList();
}
// Call the display list
if (listId)
glCallList(listId);
return;
}
//////////////////////////////////////////////////////////////////////////////
// Render signs
void Road2Geom::RenderSigns(RenderOptions *opt, RenderOptions listOpt)
{
int i, j;
double size;
GzVector p;
SignData *sign;
// Set material properties
if (opt->displayMaterials)
{
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, GzColor(0, 0, 0));
glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, GzColor(0, 0, 0));
glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, GzColor(0, 0, 0));
}
for (i = 0; i < this->signCount; i++)
{
sign = this->signs + i;
p = sign->pos;
// Test sign to see if it is near the current camera position
if (fabs(p.x - opt->cameraPose.pos.x) > opt->farClip)
continue;
if (fabs(p.y - opt->cameraPose.pos.y) > opt->farClip)
continue;
// Set sign hight
size = 0.80;
// Print the sign
glPushMatrix();
glTranslatef(p.x, p.y, p.z + 1.0); // HACK
glRotatef(90, 1, 0, 0);
glScalef(size / 150, size / 150, size / 150);
for (j = 0; j < (int) strlen(sign->text); j++)
glutStrokeCharacter(GLUT_STROKE_MONO_ROMAN, sign->text[j]);
glPopMatrix();
// Print a little stand
glBegin(GL_LINES);
glVertex3f(p.x, p.y, p.z - size / 2);
glVertex3f(p.x, p.y, 0.0);
glEnd();
}
return;
}
//#endif
/*
* 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: Andrew Howard
* Date: 23 Oct 2004
* CVS: $Id: Road.hh,v 1.3 2004/11/17 03:53:43 natepak Exp $
*/
/* Desc: Road2 geometry
* Modified By: Stephen Williams
* Date: 3 July 2007
* CVS: ???
* Notes: Modification of the original RoadGeom body. Changes the syntax of the road file, the model parameters, the tangent calculation, and the way the spline is done
*/
#ifndef ROAD2GEOM_HH
#define ROAD2GEOM_HH
#include <gazebo/Geom.hh>
class Road2Geom : public Geom
{
/// @brief Constructor
public: Road2Geom(Body *body, dSpaceID spaceId);
/// @brief Destructor
public: virtual ~Road2Geom();
/// @brief Set the number of intermediate spline points to use between defined track points
public: void SetTrackIntermediatePoints(int points) {this->trackIntermediatePoints = points;}
/// @brief Set the number of intermediate spline points to use between defined profile points
public: void SetProfileIntermediatePoints(int points) {this->profileIntermediatePoints = points;}
/// @brief Set the nominal spacing between profile points (m)
public: void SetProfileSpacing(double spacing) {this->profileSpacing = spacing;}
/// @brief Add a point to the track
/// @param pos Point position (relative to road).
public: void AddTrackPoint(GzVector pos);
/// @brief Add a point to the profile
/// @param pz Height above or below the nominal road level.
public: void AddProfilePoint(double pz);
/// @brief Add a sign.
/// @param pos Sign position (relative to road).
/// @param text Sign text (the value will be copied).
public: void AddSign(GzVector pos, const char *text);
/// @brief Initialize the mesh
public: void InitMesh();
/// @brief Update the mesh
public: void UpdateMesh(GzVector pos);
/// @brief Compute normal vectors on vertices
private: void UpdateNormals();
/// @brief Compute a Catmull-Rom spline
private: GzVector SplineCR(double t, GzVector p0, GzVector p1,
GzVector p2, GzVector p3);
/// @brief Compute a 3-point Central Difference spline tangent
private: GzVector SplineCDtan(GzVector p0, GzVector p1, GzVector p2);
/// @brief Render the geom (GL)
public: virtual void Render(RenderOptions *opt);
/// @brief Render signs
private: void RenderSigns(RenderOptions *opt, RenderOptions listOpt);
/// Data manager for trimesh
private: dTriMeshDataID tridata;
/// ODE trimesh geom
private: dGeomID trimesh;
/// Track data
private: int trackIntermediatePoints;
private: int trackPointCount, trackPointMaxCount;
private: GzVector* trackPoints;
private: GzVector* track;
// Profile Data
private: double profileSpacing;
private: int profileIntermediatePoints;
private: int profilePointCount, profilePointMaxCount;
private: GzVector* profilePoints;
private: GzVector* profile;
/// Description for a single sign
private: struct SignData
{
/// Position
GzVector pos;
/// Text
char *text;
};
/// Sign data
private: int signCount, signMaxCount;
private: SignData *signs;
/// Vertex data
private: int vertexCount, vertexMaxCount;
private: dVector3 *vertices;
private: GzVector *normals;
/// Triangle data
private: int indexCount, indexMaxCount;
private: int *indices;
/// Maximum patch size; road will be re-rendered when the camera is
/// outside the current patch.
private: double patchSize;
};
#endif
-------------------------------------------------------------------------
This SF.net email is sponsored by DB2 Express
Download DB2 Express C - the FREE version of DB2 express and take
control of your XML. No limits. Just data. Click to get it now.
http://sourceforge.net/powerbar/db2/
_______________________________________________
Playerstage-gazebo mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-gazebo
