int AdjList::AddEdge(const Vertex& from, const Vertex& to, int cost)
{
if(!hasVertex(from))
{
AddVertex(from);
}
if(!hasVertex(to))
{
AddVertex(to);
}
for(auto& vinfo: m_vinfoList)
{
if(from == vinfo.m_vertex)
{
for(const auto& edge: vinfo.m_edges)
{
if(edge.m_to == to)
{
return -1; // already added
}
}
vinfo.m_edges.push_back({to, cost});
return GRAPH_OK;
}
}
return GRAPH_ERROR_VERTEX_NOT_FOUND;
}
vector<const Vertex*> DirectedGraph::getShortestPath(const Vertex &v1, const Vertex &v2) {
vector<const Vertex*> route;
if (hasVertex(v1) && hasVertex(v2)) {
clog << "Getting route from " << v1.toString() << " to " << v2.toString() << endl;
GraphDefinition::vertex_descriptor start = m_mapOfVertices[v1.getIdentifier()];
GraphDefinition::vertex_descriptor end = m_mapOfVertices[v2.getIdentifier()];
vector<GraphDefinition::vertex_descriptor> p(boost::num_vertices(m_graph));
vector<double> d(boost::num_vertices(m_graph));
try {
// Call A* algorithm.
boost::astar_search(m_graph, start,
DistanceHeuristic<GraphDefinition, double>(m_graph, end),
boost::predecessor_map(&p[0]).distance_map(&d[0]).visitor(AStarGoalVisitor<GraphDefinition::vertex_descriptor>(end)));
}
catch(found_goal &/*fg*/) {
// An exception is thrown if end was found.
list<GraphDefinition::vertex_descriptor> path;
for (GraphDefinition::vertex_descriptor v = end; ; v = p[v]) {
path.push_front(v);
if (v == p[v]) {
break;
}
}
list<GraphDefinition::vertex_descriptor>::iterator it = path.begin();
stringstream sstr;
while (it != path.end()) {
route.push_back(m_graph[*it]);
sstr << m_graph[*it]->toString() << " ";
it++;
}
cout << "Route: " << sstr.str() << endl;
}
}
return route;
}
void DirectedGraph::addVertex(const Vertex *v) {
if ( (v != NULL) && (!hasVertex(*v)) ) {
GraphDefinition::vertex_descriptor vertex = boost::add_vertex(m_graph);
// Safe newly created vertex for further usage.
m_mapOfVertices[v->getIdentifier()] = vertex;
// Safe pointer for removal.
m_listOfVertices.push_back(v);
// Actually add vertex to graph.
m_graph[vertex] = v;
}
}
VirtualRobot::ObstaclePtr MeshConverter::refineObjectSurface(VirtualRobot::ObstaclePtr object, float maxDist)
{
VirtualRobot::ObstaclePtr res;
if (!object || !object->getCollisionModel())
{
return res;
}
TriMeshModelPtr tm = object->getCollisionModel()->getTriMeshModel();
if (!tm)
{
return res;
}
VR_INFO << "Processing object with " << tm->faces.size() << " triangles" << endl;
// first create new object
TriMeshModelPtr triMesh2(new TriMeshModel());
int check;
for (size_t i = 0; i < tm->faces.size(); i++)
{
Eigen::Vector3f v1, v2, v3;
v1 = tm->vertices[tm->faces[i].id1];
v2 = tm->vertices[tm->faces[i].id2];
v3 = tm->vertices[tm->faces[i].id3];
unsigned int id1, id2, id3;
check = hasVertex(triMesh2->vertices, v1);
if (check > 0)
{
id1 = (int)check;
}
else
{
// add vertex
triMesh2->addVertex(v1);
id1 = triMesh2->vertices.size() - 1;
}
check = hasVertex(triMesh2->vertices, v2);
if (check > 0)
{
id2 = (int)check;
}
else
{
// add vertex
triMesh2->addVertex(v2);
id2 = triMesh2->vertices.size() - 1;
}
check = hasVertex(triMesh2->vertices, v3);
if (check > 0)
{
id3 = (int)check;
}
else
{
// add vertex
triMesh2->addVertex(v3);
id3 = triMesh2->vertices.size() - 1;
}
// create face
MathTools::TriangleFace face;
face.id1 = id1;
face.id2 = id2;
face.id3 = id3;
face.normal = tm->faces[i].normal;
triMesh2->addFace(face);
}
for (size_t i = 0; i < triMesh2->faces.size(); i++)
{
checkAndSplitVertex(triMesh2, i, maxDist);
}
VisualizationFactoryPtr cv = VisualizationFactory::first(NULL);
if (!cv)
{
return res;
}
Eigen::Matrix4f gp = object->getGlobalPose();
VisualizationNodePtr visu = cv->createTriMeshModelVisualization(triMesh2, false, gp);
CollisionModelPtr cm(new CollisionModel(visu));
res.reset(new Obstacle(object->getName(), visu, cm));
return res;
}
