p80426359
/
pxmlw6n2f
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
pxmlw6n2f/Gazebo_Distributed_TCP/gazebo/common/MeshManager.cc

1351 lines
37 KiB
C++
Raw Permalink Blame History

/*
* Copyright (C) 2012 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#include <sys/stat.h>
#include <string>
#include <map>
#include "gazebo/math/Plane.hh"
#include "gazebo/math/Matrix3.hh"
#include "gazebo/math/Matrix4.hh"
#include "gazebo/math/Vector2i.hh"
#include "gazebo/common/CommonIface.hh"
#include "gazebo/common/Exception.hh"
#include "gazebo/common/Console.hh"
#include "gazebo/common/Mesh.hh"
#include "gazebo/common/ColladaLoader.hh"
#include "gazebo/common/ColladaExporter.hh"
#include "gazebo/common/STLLoader.hh"
#include "gazebo/common/OBJLoader.hh"
#include "gazebo/gazebo_config.h"
#ifdef HAVE_GTS
#include "gazebo/common/MeshCSG.hh"
#include "gazebo/common/GTSMeshUtils.hh"
#endif
#include "gazebo/common/MeshManager.hh"
using namespace gazebo;
using namespace common;
// added here for ABI compatibility
// TODO move to header / private class when merging forward.
static OBJLoader objLoader;
//////////////////////////////////////////////////
MeshManager::MeshManager()
{
this->colladaLoader = new ColladaLoader();
this->colladaExporter = new ColladaExporter();
this->stlLoader = new STLLoader();
// Create some basic shapes
this->CreatePlane("unit_plane",
ignition::math::Planed(
ignition::math::Vector3d(0, 0, 1), ignition::math::Vector2d(1, 1), 0),
ignition::math::Vector2d(1, 1),
ignition::math::Vector2d(1, 1));
this->CreateSphere("unit_sphere", 0.5, 32, 32);
this->CreateSphere("joint_anchor", 0.01, 32, 32);
this->CreateBox("body_cg", ignition::math::Vector3d(0.014, 0.014, 0.014),
ignition::math::Vector2d(0.014, 0.014));
this->CreateBox("unit_box", ignition::math::Vector3d(1, 1, 1),
ignition::math::Vector2d(1, 1));
// Note: axis_box is added and currently only used by SelectionObj in
// replacement of unit_box to avoid a weird ogre 1.9 problem in
// UNIT_Projection_TEST on OSX
this->CreateBox("axis_box", ignition::math::Vector3d::One,
ignition::math::Vector2d::One);
this->CreateCylinder("unit_cylinder", 0.5, 1.0, 1, 32);
this->CreateCone("unit_cone", 0.5, 1.0, 5, 32);
this->CreateCamera("unit_camera", 0.5);
this->CreateCylinder("axis_shaft", 0.01, 0.2, 1, 16);
this->CreateCone("axis_head", 0.02, 0.08, 1, 16);
this->CreateTube("selection_tube", 1.0, 1.2, 0.01, 1, 64);
this->fileExtensions.push_back("stl");
this->fileExtensions.push_back("dae");
this->fileExtensions.push_back("obj");
}
//////////////////////////////////////////////////
MeshManager::~MeshManager()
{
delete this->colladaLoader;
delete this->colladaExporter;
delete this->stlLoader;
std::map<std::string, Mesh*>::iterator iter;
for (iter = this->meshes.begin(); iter != this->meshes.end(); ++iter)
delete iter->second;
this->meshes.clear();
}
//////////////////////////////////////////////////
const Mesh *MeshManager::Load(const std::string &_filename)
{
if (!this->IsValidFilename(_filename))
{
gzerr << "Invalid mesh filename extension[" << _filename << "]\n";
return NULL;
}
Mesh *mesh = NULL;
std::string extension;
if (this->HasMesh(_filename))
{
return this->meshes[_filename];
// This breaks trimesh geom. Each new trimesh should have a unique name.
/*
// erase mesh from this->meshes. This allows a mesh to be modified and
// inserted into gazebo again without closing gazebo.
std::map<std::string, Mesh*>::iterator iter;
iter = this->meshes.find(_filename);
delete iter->second;
iter->second = NULL;
this->meshes.erase(iter);
*/
}
std::string fullname = common::find_file(_filename);
if (!fullname.empty())
{
extension = fullname.substr(fullname.rfind(".")+1, fullname.size());
std::transform(extension.begin(), extension.end(),
extension.begin(), ::tolower);
MeshLoader *loader = NULL;
if (extension == "stl" || extension == "stlb" || extension == "stla")
loader = this->stlLoader;
else if (extension == "dae")
loader = this->colladaLoader;
else if (extension == "obj")
loader = &objLoader;
else
{
gzerr << "Unsupported mesh format for file[" << _filename << "]\n";
return nullptr;
}
try
{
// This mutex prevents two threads from loading the same mesh at the
// same time.
boost::mutex::scoped_lock lock(this->mutex);
if (!this->HasMesh(_filename))
{
if ((mesh = loader->Load(fullname)) != NULL)
{
mesh->SetName(_filename);
this->meshes.insert(std::make_pair(_filename, mesh));
}
else
gzerr << "Unable to load mesh[" << fullname << "]\n";
}
else
{
mesh = this->meshes[_filename];
}
}
catch(gazebo::common::Exception &e)
{
gzerr << "Error loading mesh[" << fullname << "]\n";
gzerr << e << "\n";
gzthrow(e);
}
}
else
gzerr << "Unable to find file[" << _filename << "]\n";
return mesh;
}
//////////////////////////////////////////////////
void MeshManager::Export(const Mesh *_mesh, const std::string &_filename,
const std::string &_extension, bool _exportTextures)
{
if (_extension == "dae")
{
this->colladaExporter->Export(_mesh, _filename, _exportTextures);
}
else
{
gzerr << "Unsupported mesh format for file[" << _filename << "]\n";
}
}
//////////////////////////////////////////////////
bool MeshManager::IsValidFilename(const std::string &_filename)
{
std::string extension;
extension = _filename.substr(_filename.rfind(".")+1, _filename.size());
if (extension.empty())
return false;
std::transform(extension.begin(), extension.end(),
extension.begin(), ::tolower);
return std::find(this->fileExtensions.begin(), this->fileExtensions.end(),
extension) != this->fileExtensions.end();
}
//////////////////////////////////////////////////
void MeshManager::GetMeshAABB(const Mesh *_mesh,
ignition::math::Vector3d &_center,
ignition::math::Vector3d &_minXYZ, ignition::math::Vector3d &_maxXYZ)
{
if (this->HasMesh(_mesh->GetName()))
this->meshes[_mesh->GetName()]->GetAABB(_center, _minXYZ, _maxXYZ);
}
//////////////////////////////////////////////////
void MeshManager::GenSphericalTexCoord(const Mesh *_mesh,
const ignition::math::Vector3d &_center)
{
if (this->HasMesh(_mesh->GetName()))
this->meshes[_mesh->GetName()]->GenSphericalTexCoord(_center);
}
//////////////////////////////////////////////////
void MeshManager::AddMesh(Mesh *_mesh)
{
if (!this->HasMesh(_mesh->GetName()))
this->meshes[_mesh->GetName()] = _mesh;
}
//////////////////////////////////////////////////
const Mesh *MeshManager::GetMesh(const std::string &_name) const
{
std::map<std::string, Mesh*>::const_iterator iter;
iter = this->meshes.find(_name);
if (iter != this->meshes.end())
return iter->second;
return NULL;
}
//////////////////////////////////////////////////
bool MeshManager::HasMesh(const std::string &_name) const
{
if (_name.empty())
return false;
std::map<std::string, Mesh*>::const_iterator iter;
iter = this->meshes.find(_name);
return iter != this->meshes.end();
}
//////////////////////////////////////////////////
void MeshManager::CreateSphere(const std::string &name, float radius,
int rings, int segments)
{
if (this->HasMesh(name))
{
return;
}
int ring, seg;
float deltaSegAngle = (2.0 * M_PI / segments);
float deltaRingAngle = (M_PI / rings);
ignition::math::Vector3d vert, norm;
unsigned int verticeIndex = 0;
Mesh *mesh = new Mesh();
mesh->SetName(name);
this->meshes.insert(std::make_pair(name, mesh));
SubMesh *subMesh = new SubMesh();
mesh->AddSubMesh(subMesh);
// Generate the group of rings for the sphere
for (ring = 0; ring <= rings; ++ring)
{
float r0 = radius * sinf(ring * deltaRingAngle);
vert.Y() = radius * cosf(ring * deltaRingAngle);
// Generate the group of segments for the current ring
for (seg = 0; seg <= segments; ++seg)
{
vert.X() = r0 * sinf(seg * deltaSegAngle);
vert.Z() = r0 * cosf(seg * deltaSegAngle);
norm = vert;
norm.Normalize();
// Add one vertex to the strip which makes up the sphere
subMesh->AddVertex(vert);
subMesh->AddNormal(norm);
subMesh->AddTexCoord(
static_cast<float>(seg) / static_cast<float>(segments),
static_cast<float>(ring) /static_cast<float>(rings));
if (ring != rings)
{
// each vertex (except the last) has six indices pointing to it
subMesh->AddIndex(verticeIndex + segments + 1);
subMesh->AddIndex(verticeIndex);
subMesh->AddIndex(verticeIndex + segments);
subMesh->AddIndex(verticeIndex + segments + 1);
subMesh->AddIndex(verticeIndex + 1);
subMesh->AddIndex(verticeIndex);
verticeIndex++;
}
}
}
}
//////////////////////////////////////////////////
void MeshManager::CreatePlane(const std::string &_name,
const ignition::math::Planed &_plane,
const ignition::math::Vector2d &_segments,
const ignition::math::Vector2d &_uvTile)
{
this->CreatePlane(_name, _plane.Normal(), _plane.Offset(), _plane.Size(),
_segments, _uvTile);
}
//////////////////////////////////////////////////
void MeshManager::CreatePlane(const std::string &_name,
const ignition::math::Vector3d &_normal,
const double _d,
const ignition::math::Vector2d &_size,
const ignition::math::Vector2d &_segments,
const ignition::math::Vector2d &_uvTile)
{
if (this->HasMesh(_name))
{
return;
}
Mesh *mesh = new Mesh();
mesh->SetName(_name);
this->meshes.insert(std::make_pair(_name, mesh));
SubMesh *subMesh = new SubMesh();
mesh->AddSubMesh(subMesh);
ignition::math::Vector3d zAxis, yAxis, xAxis;
zAxis = _normal;
zAxis.Normalize();
yAxis = zAxis.Perpendicular();
xAxis = yAxis.Cross(zAxis);
ignition::math::Matrix4d xlate, xform, rot;
xlate = rot = ignition::math::Matrix4d::Identity;
ignition::math::Matrix3d rot3;
rot3.Axes(xAxis, yAxis, zAxis);
rot = rot3;
xlate.Translate(_normal * -_d);
xform = xlate * rot;
ignition::math::Vector3d vec;
ignition::math::Vector3d norm(0, 0, 1);
double xSpace = _size.X() / _segments.X();
double ySpace = _size.Y() / _segments.Y();
double halfWidth = _size.X() / 2.0;
double halfHeight = _size.Y() / 2.0;
double xTex = _uvTile.X() / _segments.X();
double yTex = _uvTile.Y() / _segments.Y();
// Give it some thickness to reduce shadow artifacts.
double thickness = 0.01;
for (int i = 0; i <= 1; ++i)
{
double z = i*thickness;
for (int y = 0; y <= _segments.Y(); ++y)
{
for (int x = 0; x <= _segments.X(); ++x)
{
// Compute the position of the vertex
vec.X() = (x * xSpace) - halfWidth;
vec.Y() = (y * ySpace) - halfHeight;
vec.Z() = -z;
vec = xform.TransformAffine(vec);
subMesh->AddVertex(vec);
// Compute the normal
vec = xform.TransformAffine(norm);
subMesh->AddNormal(vec);
// Compute the texture coordinate
subMesh->AddTexCoord(x * xTex, 1 - (y * yTex));
}
}
}
this->Tesselate2DMesh(subMesh, _segments.X() + 1, _segments.Y() + 1, false);
}
//////////////////////////////////////////////////
void MeshManager::CreateBox(const std::string &_name,
const ignition::math::Vector3d &_sides,
const ignition::math::Vector2d &_uvCoords)
{
int i, k;
if (this->HasMesh(_name))
{
return;
}
Mesh *mesh = new Mesh();
mesh->SetName(_name);
this->meshes.insert(std::make_pair(_name, mesh));
SubMesh *subMesh = new SubMesh();
mesh->AddSubMesh(subMesh);
// Vertex values
float v[8][3] =
{
{-1, -1, -1},
{-1, -1, +1},
{+1, -1, +1},
{+1, -1, -1},
{-1, +1, -1},
{-1, +1, +1},
{+1, +1, +1},
{+1, +1, -1}
};
// Normals for each face
float n[6][3]=
{
{+0, -1, +0},
{+0, +1, +0},
{+0, +0, +1},
{-1, +0, +0},
{+0, +0, -1},
{+1, +0, +0},
};
// Texture coords
double t[4][2] =
{
{_uvCoords.X(), 0}, {0, 0}, {0, _uvCoords.Y()},
{_uvCoords.X(), _uvCoords.Y()}
};
// Vertices
int faces[6][4] =
{
{2, 1, 0, 3}, {5, 6, 7, 4},
{2, 6, 5, 1}, {1, 5, 4, 0},
{0, 4, 7, 3}, {6, 2, 3, 7}
};
// Indices
int ind[36] =
{
0, 1, 2,
2, 3, 0,
4, 5, 7,
7, 5, 6,
11, 8, 9,
9, 10, 11,
12, 13, 15,
15, 13, 14,
16, 17, 18,
18, 19, 16,
21, 22, 23,
23, 20, 21,
};
// Compute the vertices
for (i = 0; i < 8; ++i)
{
v[i][0] *= _sides.X() * 0.5;
v[i][1] *= _sides.Y() * 0.5;
v[i][2] *= _sides.Z() * 0.5;
}
// For each face
for (i = 0; i < 6; ++i)
{
// For each vertex in the face
for (k = 0; k < 4; k++)
{
subMesh->AddVertex(v[faces[i][k]][0],
v[faces[i][k]][1],
v[faces[i][k]][2]);
subMesh->AddNormal(n[i][0], n[i][1], n[i][2]);
subMesh->AddTexCoord(t[k][0], t[k][1]);
}
}
// Set the indices
for (i = 0; i < 36; ++i)
subMesh->AddIndex(ind[i]);
}
//////////////////////////////////////////////////
void MeshManager::CreateExtrudedPolyline(const std::string &_name,
const std::vector<std::vector<ignition::math::Vector2d> > &_polys,
double _height)
{
// distance tolerence between 2 points. This is used when creating a list
// of distinct points in the polylines.
double tol = 1e-4;
#if !HAVE_GTS
gzerr << "GTS library not found. Can not extrude polyline" << std::endl;
return;
#endif
auto polys = _polys;
// close all the loops
for (auto &poly : polys)
{
// does the poly ends with the first point?
auto first = poly[0];
auto last = poly[poly.size()-1];
double d = (first.X() - last.X()) * (first.X() - last.X());
d += (first.Y() - last.Y()) * (first.Y() - last.Y());
// within range
if (d > tol * tol)
{
// add the first point at the end
poly.push_back(first);
}
}
if (this->HasMesh(_name))
{
return;
}
Mesh *mesh = new Mesh();
mesh->SetName(_name);
SubMesh *subMesh = new SubMesh();
mesh->AddSubMesh(subMesh);
std::vector<ignition::math::Vector2d> vertices;
std::vector<ignition::math::Vector2i> edges;
MeshManager::ConvertPolylinesToVerticesAndEdges(polys,
tol,
vertices,
edges);
#if HAVE_GTS
if (!GTSMeshUtils::DelaunayTriangulation(vertices, edges, subMesh))
{
gzerr << "Unable to triangulate polyline." << std::endl;
delete mesh;
return;
}
#endif
std::vector<ignition::math::Vector3d> normals;
for (unsigned int i = 0; i < edges.size(); ++i)
{
// we retrieve each edge's coordinates
int i0 = edges[i][0];
int i1 = edges[i][1];
ignition::math::Vector2d edgeV0 = vertices[i0];
ignition::math::Vector2d edgeV1 = vertices[i1];
// we look for those points in the subMesh (where indices may have changed)
for (unsigned int j = 0; j < subMesh->GetIndexCount(); j+=3)
{
ignition::math::Vector3d v0 = subMesh->Vertex(subMesh->GetIndex(j));
ignition::math::Vector3d v1 = subMesh->Vertex(subMesh->GetIndex(j+1));
ignition::math::Vector3d v2 = subMesh->Vertex(subMesh->GetIndex(j+2));
std::vector<ignition::math::Vector3d> triangle;
triangle.push_back(v0);
triangle.push_back(v1);
triangle.push_back(v2);
int ev0 = -1;
for (unsigned int k = 0; k < triangle.size(); ++k)
{
if (ignition::math::Vector2d(triangle[k].X(), triangle[k].Y()) ==
edgeV0)
{
// found a vertex in triangle that matches the vertex of the edge
ev0 = k;
break;
}
}
if (ev0 >=0)
{
int ev1 = -1;
int ev2 = -1;
for (unsigned int k = 0; k < triangle.size()-1; ++k)
{
int index = (ev0 + k + 1) % triangle.size();
ignition::math::Vector3d triV = triangle[index];
if (math::Vector2d(triV.X(), triV.Y()) == edgeV1)
{
// found another vertex in triangle that matches the vertex of the
// other edge.
ev1 = index;
// Store the index of the third triangle vertex.
// It's either 0, 1, or 2. Find it using simple bitwise operation.
ev2 = ~(ev1 | ev0) & 0x03;
break;
}
}
if (ev1 >= 0 && ev2 >= 0 && ev0 != ev1 && ev0 != ev2)
{
// Found an edge in triangle that matches the exterior edge.
// Now find its normal.
ignition::math::Vector3d edgeVec = triangle[ev0] - triangle[ev1];
edgeVec.Normalize();
ignition::math::Vector3d normal(edgeVec.Y(), -edgeVec.X(), 0);
ignition::math::Vector3d otherEdgeVec = triangle[ev0] - triangle[ev2];
otherEdgeVec.Normalize();
double angle0 = otherEdgeVec.Dot(normal);
double angle1 = otherEdgeVec.Dot(-normal);
if (angle0 > angle1)
{
if (angle0 >= 0)
normals.push_back(normal);
}
else
{
if (angle1 >= 0)
normals.push_back(-normal);
}
}
}
}
}
// number of exterior edge normals found should be equal to the number of
// exterior edges
if (normals.size() != edges.size())
{
gzerr << "Unable to extrude mesh. Triangulation failed" << std::endl;
delete mesh;
return;
}
unsigned int numVertices = subMesh->GetVertexCount();
// add normal for bottom face
for (unsigned int i = 0; i < numVertices; ++i)
subMesh->AddNormal(-ignition::math::Vector3d::UnitZ);
// create the top face
for (unsigned int i = 0; i < numVertices; ++i)
{
ignition::math::Vector3d v = subMesh->Vertex(i);
subMesh->AddVertex(v.X(), v.Y(), _height);
subMesh->AddNormal(ignition::math::Vector3d::UnitZ);
}
unsigned int numIndices = subMesh->GetIndexCount();
for (unsigned int i = 0; i < numIndices; i+=3)
{
unsigned int i0 = subMesh->GetIndex(i);
unsigned int i1 = subMesh->GetIndex(i+1);
unsigned int i2 = subMesh->GetIndex(i+2);
subMesh->AddIndex(numVertices+i0);
subMesh->AddIndex(numVertices+i2);
subMesh->AddIndex(numVertices+i1);
}
// create the side faces
for (unsigned int i = 0; i < edges.size(); ++i)
{
// we retrieve each edge's coordinates
int i0 = edges[i][0];
int i1 = edges[i][1];
ignition::math::Vector2d v0 = vertices[i0];
ignition::math::Vector2d v1 = vertices[i1];
ignition::math::Vector2d edge2d = v1 - v0;
ignition::math::Vector3d edge(edge2d.X(), edge2d.Y(), 0);
ignition::math::Vector3d cross = edge.Cross(normals[i]);
unsigned int vCount = subMesh->GetVertexCount();
subMesh->AddVertex(ignition::math::Vector3d(v0.X(), v0.Y(), 0));
if (cross.Z() >0)
{
subMesh->AddVertex(ignition::math::Vector3d(v0.X(), v0.Y(), _height));
subMesh->AddVertex(ignition::math::Vector3d(v1.X(), v1.Y(), _height));
}
else
{
subMesh->AddVertex(ignition::math::Vector3d(v1.X(), v1.Y(), _height));
subMesh->AddVertex(ignition::math::Vector3d(v0.X(), v0.Y(), _height));
}
subMesh->AddVertex(ignition::math::Vector3d(v0.X(), v0.Y(), 0));
if (cross.Z() >0)
{
subMesh->AddVertex(ignition::math::Vector3d(v1.X(), v1.Y(), _height));
subMesh->AddVertex(ignition::math::Vector3d(v1.X(), v1.Y(), 0));
}
else
{
subMesh->AddVertex(ignition::math::Vector3d(v1.X(), v1.Y(), 0));
subMesh->AddVertex(ignition::math::Vector3d(v1.X(), v1.Y(), _height));
}
for (unsigned int j = 0; j < 6; ++j)
{
subMesh->AddIndex(vCount++);
subMesh->AddNormal(normals[i]);
}
}
this->meshes.insert(std::make_pair(_name, mesh));
return;
}
//////////////////////////////////////////////////
void MeshManager::CreateCamera(const std::string &_name, float _scale)
{
int i, k;
if (this->HasMesh(_name))
{
return;
}
Mesh *mesh = new Mesh();
mesh->SetName(_name);
this->meshes.insert(std::make_pair(_name, mesh));
SubMesh *subMesh = new SubMesh();
mesh->AddSubMesh(subMesh);
// Vertex values
float v[8][3] =
{
{-1, -1, -1}, {-1, -1, +1}, {+1, -1, +1}, {+1, -1, -1},
{-1, +1, -1}, {-1, +1, +1}, {+1, +1, +1}, {+1, +1, -1}
};
// Normals for each vertex
float n[8][3]=
{
{-0.577350, -0.577350, -0.577350},
{-0.577350, -0.577350, 0.577350},
{0.577350, -0.577350, 0.577350},
{0.577350, -0.577350, -0.577350},
{-0.577350, 0.577350, -0.577350},
{-0.577350, 0.577350, 0.577350},
{0.577350, 0.577350, 0.577350},
{0.577350, 0.577350, -0.577350}
};
// Texture coords
/*float t[4][2] =
{
{uvCoords.X(), 0}, {0, 0}, {0, uvCoords.Y()}, {uvCoords.X(), uvCoords.Y()}
};*/
// Vertices
int faces[6][4] =
{
{2, 1, 0, 3}, {5, 6, 7, 4},
{2, 6, 5, 1}, {1, 5, 4, 0},
{0, 4, 7, 3}, {6, 2, 3, 7}
};
// Indices
int ind[36] =
{
0, 1, 2,
2, 3, 0,
4, 5, 7,
7, 5, 6,
11, 8, 9,
9, 10, 11,
12, 13, 15,
15, 13, 14,
16, 17, 18,
18, 19, 16,
21, 22, 23,
23, 20, 21,
};
// Compute the vertices
for (i = 0; i < 8; ++i)
{
v[i][0] *= _scale * 0.5;
v[i][1] *= _scale * 0.5;
v[i][2] *= _scale * 0.5;
}
// For each face
for (i = 0; i < 6; ++i)
{
// For each vertex in the face
for (k = 0; k < 4; k++)
{
subMesh->AddVertex(v[faces[i][k]][0], v[faces[i][k]][1],
v[faces[i][k]][2]);
subMesh->AddNormal(n[faces[i][k]][0], n[faces[i][k]][1],
n[faces[i][k]][2]);
// subMesh->AddTexCoord(t[k][0], t[k][1]);
}
}
// Set the indices
for (i = 0; i < 36; ++i)
subMesh->AddIndex(ind[i]);
mesh->RecalculateNormals();
}
//////////////////////////////////////////////////
void MeshManager::CreateCylinder(const std::string &name, float radius,
float height, int rings, int segments)
{
ignition::math::Vector3d vert, norm;
unsigned int verticeIndex = 0;
int ring, seg;
float deltaSegAngle = (2.0 * M_PI / segments);
if (this->HasMesh(name))
{
return;
}
Mesh *mesh = new Mesh();
mesh->SetName(name);
this->meshes.insert(std::make_pair(name, mesh));
SubMesh *subMesh = new SubMesh();
mesh->AddSubMesh(subMesh);
// Generate the group of rings for the cylinder
for (ring = 0; ring <= rings; ++ring)
{
vert.Z() = ring * height/rings - height/2.0;
// Generate the group of segments for the current ring
for (seg = 0; seg <= segments; ++seg)
{
vert.Y() = radius * cosf(seg * deltaSegAngle);
vert.X() = radius * sinf(seg * deltaSegAngle);
// TODO: Don't think these normals are correct.
norm = vert;
norm.Z() = 0;
norm.Normalize();
// Add one vertex to the strip which makes up the sphere
subMesh->AddVertex(vert);
subMesh->AddNormal(norm);
subMesh->AddTexCoord(
static_cast<float>(seg) / static_cast<float>(segments),
static_cast<float>(ring) / static_cast<float>(rings));
if (ring != rings)
{
// each vertex (except the last) has six indices pointing to it
subMesh->AddIndex(verticeIndex + segments + 1);
subMesh->AddIndex(verticeIndex);
subMesh->AddIndex(verticeIndex + segments);
subMesh->AddIndex(verticeIndex + segments + 1);
subMesh->AddIndex(verticeIndex + 1);
subMesh->AddIndex(verticeIndex);
verticeIndex++;
}
}
}
// This block generates the top cap
{
vert.Z() = height/2.0;
// Generate the group of segments for the top ring
for (seg = 0; seg <= segments; ++seg)
{
vert.Y() = radius * cosf(seg * deltaSegAngle);
vert.X() = radius * sinf(seg * deltaSegAngle);
subMesh->AddVertex(vert);
subMesh->AddNormal(0, 0, 1);
subMesh->AddTexCoord(
static_cast<float>(seg) / static_cast<float>(segments), 1.0);
}
// The top-middle cap vertex
subMesh->AddVertex(0, 0, height/2.0);
subMesh->AddNormal(0, 0, 1);
subMesh->AddTexCoord(0, 0);
// Create the top fan
verticeIndex = subMesh->GetVertexCount()-1;
for (seg = 0; seg < segments; seg++)
{
subMesh->AddIndex(verticeIndex);
subMesh->AddIndex(verticeIndex - segments + seg);
subMesh->AddIndex(verticeIndex - segments + seg - 1);
}
}
// This block generates the bottom cap
{
vert.Z() = -height/2.0;
// Generate the group of segments for the bottom ring
for (seg = 0; seg <= segments; ++seg)
{
vert.Y() = radius * cosf(seg * deltaSegAngle);
vert.X() = radius * sinf(seg * deltaSegAngle);
subMesh->AddVertex(vert);
subMesh->AddNormal(0, 0, -1);
subMesh->AddTexCoord(
static_cast<float>(seg) / static_cast<float>(segments), 0.0);
}
// The bottom-middle cap vertex
subMesh->AddVertex(0, 0, -height/2.0);
subMesh->AddNormal(0, 0, -1);
subMesh->AddTexCoord(0, 0);
// Create the bottom fan
verticeIndex = subMesh->GetVertexCount()-1;
for (seg = 0; seg < segments; seg++)
{
subMesh->AddIndex(verticeIndex);
subMesh->AddIndex(verticeIndex - segments + seg - 1);
subMesh->AddIndex(verticeIndex - segments + seg);
}
}
}
//////////////////////////////////////////////////
void MeshManager::CreateCone(const std::string &name, float radius,
float height, int rings, int segments)
{
ignition::math::Vector3d vert, norm;
unsigned int verticeIndex = 0;
unsigned int i, j;
int ring, seg;
if (this->HasMesh(name))
{
return;
}
Mesh *mesh = new Mesh();
mesh->SetName(name);
this->meshes.insert(std::make_pair(name, mesh));
SubMesh *subMesh = new SubMesh();
mesh->AddSubMesh(subMesh);
if (segments <3)
segments = 3;
float deltaSegAngle = (2.0 * M_PI / segments);
// Generate the group of rings for the cone
for (ring = 0; ring < rings; ring++)
{
vert.Z() = ring * height/rings - height/2.0;
double ringRadius = ((height - (vert.Z()+height/2.0)) / height) * radius;
// Generate the group of segments for the current ring
for (seg = 0; seg <= segments; seg++)
{
vert.Y() = ringRadius * cosf(seg * deltaSegAngle);
vert.X() = ringRadius * sinf(seg * deltaSegAngle);
// TODO: Don't think these normals are correct.
norm = vert;
norm.Normalize();
// Add one vertex to the strip which makes up the sphere
subMesh->AddVertex(vert);
subMesh->AddNormal(norm);
subMesh->AddTexCoord(
static_cast<float>(seg) / static_cast<float>(segments),
static_cast<float>(ring) / static_cast<float>(rings));
if (ring != (rings-1))
{
// each vertex (except the last) has six indices pointing to it
subMesh->AddIndex(verticeIndex + segments + 1);
subMesh->AddIndex(verticeIndex);
subMesh->AddIndex(verticeIndex + segments);
subMesh->AddIndex(verticeIndex + segments + 1);
subMesh->AddIndex(verticeIndex + 1);
subMesh->AddIndex(verticeIndex);
verticeIndex++;
}
}
}
/// The top point vertex
subMesh->AddVertex(0, 0, height/2.0);
subMesh->AddNormal(0, 0, 1);
subMesh->AddTexCoord(0, 0);
// The bottom cap vertex
subMesh->AddVertex(0, 0, -height/2.0);
subMesh->AddNormal(0, 0, -1);
subMesh->AddTexCoord(0, 0);
// Create the top fan
verticeIndex += segments+1;
for (seg = 0; seg < segments; seg++)
{
subMesh->AddIndex(verticeIndex);
subMesh->AddIndex(verticeIndex - segments + seg);
subMesh->AddIndex(verticeIndex - segments + seg - 1);
}
// Create the bottom fan
verticeIndex++;
for (seg = 0; seg < segments; seg++)
{
subMesh->AddIndex(verticeIndex);
subMesh->AddIndex(seg);
subMesh->AddIndex(seg+1);
}
// Fix all the normals
for (i = 0; i + 3 < subMesh->GetIndexCount(); i += 3)
{
norm.Set();
for (j = 0; j < 3; ++j)
norm += subMesh->Normal(subMesh->GetIndex(i+j));
norm /= 3;
norm.Normalize();
for (j = 0; j < 3; ++j)
subMesh->SetNormal(subMesh->GetIndex(i+j), norm);
}
mesh->RecalculateNormals();
}
//////////////////////////////////////////////////
void MeshManager::CreateTube(const std::string &_name, float _innerRadius,
float _outerRadius, float _height, int _rings, int _segments, double _arc)
{
ignition::math::Vector3d vert, norm;
unsigned int verticeIndex = 0;
int ring, seg;
// Needs at lest 1 ring, and 3 segments
int rings = std::max(_rings, 1);
int segments = std::max(_segments, 3);
float deltaSegAngle = (_arc / segments);
float radius = 0;
radius = _outerRadius;
if (this->HasMesh(_name))
return;
Mesh *mesh = new Mesh();
mesh->SetName(_name);
this->meshes.insert(std::make_pair(_name, mesh));
SubMesh *subMesh = new SubMesh();
mesh->AddSubMesh(subMesh);
// Generate the group of rings for the outsides of the cylinder
for (ring = 0; ring <= rings; ++ring)
{
vert.Z() = ring * _height/rings - _height/2.0;
// Generate the group of segments for the current ring
for (seg = 0; seg <= segments; ++seg)
{
vert.Y() = radius * cosf(seg * deltaSegAngle);
vert.X() = radius * sinf(seg * deltaSegAngle);
// TODO: Don't think these normals are correct.
norm = vert;
norm.Normalize();
// Add one vertex to the strip which makes up the tube
subMesh->AddVertex(vert);
subMesh->AddNormal(norm);
subMesh->AddTexCoord(
static_cast<float>(seg) / static_cast<float>(segments),
static_cast<float>(ring) / static_cast<float>(rings));
// outer triangles connecting ring [ring] to ring [ring + 1]
if (ring != rings)
{
if (seg != 0)
{
subMesh->AddIndex(verticeIndex + segments + 1);
subMesh->AddIndex(verticeIndex);
subMesh->AddIndex(verticeIndex + segments);
}
if (seg != segments)
{
subMesh->AddIndex(verticeIndex + segments + 1);
subMesh->AddIndex(verticeIndex + 1);
subMesh->AddIndex(verticeIndex);
}
}
// ring [rings] is the edge of the top cap
else if (seg != segments)
{
// These indices form the top cap
subMesh->AddIndex(verticeIndex);
subMesh->AddIndex(verticeIndex + segments + 1);
subMesh->AddIndex(verticeIndex+1);
subMesh->AddIndex(verticeIndex+1);
subMesh->AddIndex(verticeIndex + segments + 1);
subMesh->AddIndex(verticeIndex + segments + 2);
}
// ring [0] is the edge of the bottom cap
if (ring == 0 && seg < segments)
{
// These indices form the bottom cap
subMesh->AddIndex(verticeIndex+1);
subMesh->AddIndex(verticeIndex + (segments+1) * (((rings+1)*2)-1));
subMesh->AddIndex(verticeIndex);
subMesh->AddIndex(verticeIndex + (segments+1) * (((rings+1)*2)-1) + 1);
subMesh->AddIndex(verticeIndex + (segments+1) * (((rings+1)*2)-1));
subMesh->AddIndex(verticeIndex+1);
}
verticeIndex++;
}
}
// Generate the group of rings for the inside of the cylinder
radius = _innerRadius;
for (ring = 0; ring <= rings; ++ring)
{
vert.Z() = (_height/2.0) - (ring * _height/rings);
// Generate the group of segments for the current ring
for (seg = 0; seg <= segments; ++seg)
{
vert.Y() = radius * cosf(seg * deltaSegAngle);
vert.X() = radius * sinf(seg * deltaSegAngle);
// TODO: Don't think these normals are correct.
norm = vert;
norm.Normalize();
// Add one vertex to the strip which makes up the tube
subMesh->AddVertex(vert);
subMesh->AddNormal(norm);
subMesh->AddTexCoord(
static_cast<float>(seg) / static_cast<float>(segments),
static_cast<float>(ring) / static_cast<float>(rings));
// inner triangles connecting ring [ring] to ring [ring + 1]
if (ring != rings)
{
// each vertex has six indices (2 triangles)
if (seg != 0)
{
subMesh->AddIndex(verticeIndex + segments + 1);
subMesh->AddIndex(verticeIndex);
subMesh->AddIndex(verticeIndex + segments);
}
if (seg != segments)
{
subMesh->AddIndex(verticeIndex + segments + 1);
subMesh->AddIndex(verticeIndex + 1);
subMesh->AddIndex(verticeIndex);
}
}
verticeIndex++;
}
}
// Close ends in case it's not a full circle
if (!ignition::math::equal(_arc, 2.0 * M_PI))
{
for (ring = 0; ring < rings; ++ring)
{
// Close beginning
subMesh->AddIndex((segments+1)*(ring+1));
subMesh->AddIndex((segments+1)*ring);
subMesh->AddIndex((segments+1)*((rings+1)*2-2-ring));
subMesh->AddIndex((segments+1)*((rings+1)*2-2-ring));
subMesh->AddIndex((segments+1)*ring);
subMesh->AddIndex((segments+1)*((rings+1)*2-1-ring));
// Close end
subMesh->AddIndex((segments+1)*((rings+1)*2-2-ring)+segments);
subMesh->AddIndex((segments+1)*((rings+1)*2-1-ring)+segments);
subMesh->AddIndex((segments+1)*(ring+1)+segments);
subMesh->AddIndex((segments+1)*(ring+1)+segments);
subMesh->AddIndex((segments+1)*((rings+1)*2-1-ring)+segments);
subMesh->AddIndex((segments+1)*ring+segments);
}
}
mesh->RecalculateNormals();
}
//////////////////////////////////////////////////
void MeshManager::Tesselate2DMesh(SubMesh *sm, int meshWidth, int meshHeight,
bool doubleSided)
{
int vInc, v, iterations;
int uCount;
if (doubleSided)
{
iterations = 2;
vInc = 1;
v = 0;
}
else
{
iterations = 1;
vInc = 1;
v = 0;
}
int v1, v2, v3;
while (iterations--)
{
// Make tris in a zigzag pattern (compatible with strips)
int u = 0;
int uInc = 1;
int vCount = meshHeight - 1;
while (vCount--)
{
uCount = meshWidth - 1;
while (uCount--)
{
// First tri in cell
v1 = ((v + vInc) * meshWidth) + u;
v2 = (v * meshWidth) + u;
v3 = ((v + vInc) * meshWidth) + (u + uInc);
// Output indexes
sm->AddIndex(v1);
sm->AddIndex(v2);
sm->AddIndex(v3);
// Second Tri in cell
v1 = ((v + vInc) * meshWidth) + (u + uInc);
v2 = (v * meshWidth) + u;
v3 = (v * meshWidth) + (u + uInc);
// Output indexes
sm->AddIndex(v1);
sm->AddIndex(v2);
sm->AddIndex(v3);
// Next column
u += uInc;
}
// Next row
v += vInc;
u = 0;
}
// Reverse vInc for double sided
v = meshHeight - 1;
vInc = -vInc;
}
}
#ifdef HAVE_GTS
//////////////////////////////////////////////////
void MeshManager::CreateBoolean(const std::string &_name, const Mesh *_m1,
const Mesh *_m2, int _operation, const ignition::math::Pose3d &_offset)
{
if (this->HasMesh(_name))
return;
MeshCSG csg;
Mesh *mesh = csg.CreateBoolean(_m1, _m2, _operation, _offset);
mesh->SetName(_name);
this->meshes.insert(std::make_pair(_name, mesh));
}
#endif
//////////////////////////////////////////////////
size_t MeshManager::AddUniquePointToVerticesTable(
std::vector<ignition::math::Vector2d> &_vertices,
const ignition::math::Vector2d &_p,
double _tol)
{
double sqrTol = _tol * _tol;
for (auto i = 0u; i != _vertices.size(); ++i)
{
auto v = _vertices[i] - _p;
double d = (v.X() * v.X() + v.Y() * v.Y());
if ( d < sqrTol)
{
return i;
}
}
_vertices.push_back(_p);
size_t r = _vertices.size() -1;
return r;
}
//////////////////////////////////////////////////
void MeshManager::ConvertPolylinesToVerticesAndEdges(
const std::vector<std::vector<ignition::math::Vector2d> > &_polys,
double _tol,
std::vector<ignition::math::Vector2d> &_vertices,
std::vector<ignition::math::Vector2i> &edges)
{
for (auto poly : _polys)
{
ignition::math::Vector2d previous = poly[0];
for (auto i = 1u; i != poly.size(); ++i)
{
auto p = poly[i];
auto startPointIndex = AddUniquePointToVerticesTable(_vertices,
previous, _tol);
auto endPointIndex = AddUniquePointToVerticesTable(_vertices,
p, _tol);
// current end point is now the starting point for the next edge
previous = p;
if (startPointIndex == endPointIndex)
{
gzwarn << "Ignoring edge without 2 distinct vertices" << std::endl;
continue;
}
// add the new edge
ignition::math::Vector2i e(startPointIndex, endPointIndex);
edges.push_back(e);
}
}
}
Reference in New Issue View Git Blame Copy Permalink