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; }