1351 lines
37 KiB
C++
1351 lines
37 KiB
C++
/*
|
|
* 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);
|
|
}
|
|
}
|
|
}
|