├── .cproject ├── .project ├── LICENSE ├── README.md ├── app.cpp ├── graph.h ├── graphics_item.h ├── helper_functions.cpp ├── helper_functions.h ├── main_window.cpp ├── main_window.h ├── main_window_moc.cpp ├── mp.jar ├── number_type.h ├── resources ├── app.ico ├── fitview.png ├── hexcycle.png ├── hexgrid.png ├── open.png ├── play.png ├── sg.png └── solution.png ├── roadmap-generator-vc10 ├── make-moc.vcxproj ├── make-moc.vcxproj.filters ├── make-moc.vcxproj.user ├── roadmap-generator-vc10.sln ├── roadmap-generator-vc10.suo ├── roadmap-generator-vc10.vcxproj ├── roadmap-generator-vc10.vcxproj.filters └── roadmap-generator-vc10.vcxproj.user ├── roadmap.cpp ├── roadmap.h ├── shortest_path └── README ├── test-envs ├── cross.txt ├── empty.txt ├── regular.txt ├── tirangles.txt └── vertical.txt ├── types.h └── vc10 └── main_window_moc.cpp /.cproject: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 38 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 52 | 53 | 54 | 55 | 56 | 63 | 67 | 68 | 69 | 70 | 77 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 114 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 140 | 143 | 144 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | -------------------------------------------------------------------------------- /.project: -------------------------------------------------------------------------------- 1 | 2 | 3 | optimal-mrpp-continuous 4 | 5 | 6 | 7 | 8 | 9 | org.eclipse.cdt.managedbuilder.core.genmakebuilder 10 | clean,full,incremental, 11 | 12 | 13 | 14 | 15 | org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder 16 | full,incremental, 17 | 18 | 19 | 20 | 21 | 22 | org.eclipse.cdt.core.cnature 23 | org.eclipse.cdt.core.ccnature 24 | org.eclipse.cdt.managedbuilder.core.managedBuildNature 25 | org.eclipse.cdt.managedbuilder.core.ScannerConfigNature 26 | 27 | 28 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | All components of this library are licensed under the BSD 3-Clause 2 | License. 3 | 4 | Copyright (c) 2015-, Rutgers Algorithmic Robotics and Control Group 5 | (https://arc.cs.rutgers.edu). All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are 9 | met: 10 | 11 | * Redistributions of source code must retain the above copyright notice, 12 | this list of conditions and the following disclaimer. 13 | 14 | * Redistributions in binary form must reproduce the above copyright 15 | notice, this list of conditions and the following disclaimer in the 16 | documentation and/or other materials provided with the distribution. 17 | 18 | * Neither the name of the copyright holder nor the names of its 19 | contributors may be used to endorse or promote products derived from 20 | this software without specific prior written permission. 21 | 22 | 23 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 26 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 27 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 28 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 29 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 30 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 31 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 32 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ###Optimal Multi-Robot Path Planning in 2D Continuous Domain 2 | 3 | 4 | ![mrpp](https://user-images.githubusercontent.com/23622170/124700709-3129b680-debb-11eb-8e14-b9927f3b2cf3.png) 5 | 6 | 7 | Cross platform C++ source code used in the following work 8 | 9 | J. Yu and D. Rus. An Effective Algorithmic Framework for Near Optimal Multi-Robot Path Planning. The 2015 International Symposium on Robotics Research (ISRR 2015). 10 | 11 | ###Requires 12 | 13 | Boost (1.55.0 used): http://www.boost.org/ 14 | Compile shared library for your platform. 15 | 16 | CGAL (4.3 used): http://www.cgal.org/ 17 | Use CMAKE to generate and compile for your platform. 18 | 19 | Gurobi (6.5+): http://www.gurobi.com/ 20 | Install and make gurobi.jar (as well as mp.jar) available to the executable. 21 | 22 | LibQGLViewer (2.6): http://libqglviewer.com/ 23 | Obtain the distribution and made available for CGAL to use. 24 | 25 | Qt (4.8.2 used): https://www.qt.io/ 26 | Install and make moc.exe available to run on command line. 27 | 28 | Visilibity: http://visilibity.org/ 29 | See README under shortest_path folder for instructions on this library. 30 | 31 | -------------------------------------------------------------------------------- /app.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Main app class 3 | * 4 | * Created on: Jan 30, 2015 5 | * Author: Jingjin Yu 6 | */ 7 | 8 | #include "helper_functions.h" 9 | #include "main_window.h" 10 | 11 | // Qt headers 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | // GraphicsView items and event filters (input classes) 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | // Base window class 25 | #include 26 | #include 27 | 28 | 29 | int main(int argc, char* argv[]){ 30 | QApplication a(argc, argv, true, 0); 31 | MainWindow mainWindow(&a, (argc > 1 ? QString(argv[1]) : QString("F:\\temp\\mr")), "Multi-Robot Path Planning", "./resources/app.ico", false); 32 | mainWindow.show(); 33 | 34 | 35 | return a.exec(); 36 | } 37 | -------------------------------------------------------------------------------- /graph.h: -------------------------------------------------------------------------------- 1 | /* 2 | * A basic adjacency list based simple, undirected graph in which each vertex is indexed 3 | * with an integer ID. 4 | * 5 | * Created on: Jan 30, 2015 6 | * Author: Jingjin Yu 7 | */ 8 | 9 | #ifndef _O_GRAPH_H_ 10 | #define _O_GRAPH_H_ 11 | 12 | #include "types.h" 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | class Graph{ 20 | private: 21 | std::set vSet; 22 | std::map > adjSetMap; 23 | std::set > edgeSet; 24 | std::set emptySet; 25 | 26 | public: 27 | // ===================================================================================== 28 | // Core methods 29 | // ===================================================================================== 30 | 31 | Graph(){} 32 | 33 | void clear(){ 34 | vSet.clear(); 35 | adjSetMap.clear(); 36 | edgeSet.clear(); 37 | emptySet.clear(); 38 | } 39 | 40 | bool hasVertex(int v){ 41 | return (vSet.find(v) != vSet.end()); 42 | } 43 | 44 | std::set &getVertexSet(){return vSet;} 45 | 46 | void removeVertex(int v){ 47 | // Delete all edges from the vertex 48 | std::set nbrSet = getNeighborSet(v); 49 | for(std::set::iterator vit = nbrSet.begin(); vit != nbrSet.end(); vit++){ 50 | removeEdge(v, *vit); 51 | } 52 | 53 | // Erase the vertex itself 54 | vSet.erase(v); 55 | } 56 | 57 | void addEdge(int v1, int v2){ 58 | // Need to add vertex? 59 | if(vSet.find(v1) == vSet.end()){vSet.insert(v1);adjSetMap[v1] = std::set();} 60 | if(vSet.find(v2) == vSet.end()){vSet.insert(v2);adjSetMap[v2] = std::set();} 61 | // Adding edges as needed 62 | adjSetMap[v1].insert(v2); 63 | adjSetMap[v2].insert(v1); 64 | edgeSet.insert(std::pair(v1 < v2? v1 : v2, v1 < v2? v2 : v1)); 65 | } 66 | 67 | std::set > &getEdgeSet(){return edgeSet;} 68 | 69 | void removeEdge(int v1, int v2){ 70 | // Check that we have the vertices 71 | if(vSet.find(v1) == vSet.end() || vSet.find(v2) == vSet.end()) return; 72 | 73 | // Remove edges as needed 74 | adjSetMap[v1].erase(v2); 75 | adjSetMap[v2].erase(v1); 76 | edgeSet.erase(std::pair(v1 < v2? v1 : v2, v1 < v2? v2 : v1)); 77 | 78 | // Remove v1, v2 as needed 79 | if(adjSetMap[v1].size() == 0){vSet.erase(v1); adjSetMap.erase(v1);} 80 | if(adjSetMap[v2].size() == 0){vSet.erase(v2); adjSetMap.erase(v2);} 81 | } 82 | 83 | bool hasEdge(int v1, int v2){ 84 | // Check we have vertex 85 | if(vSet.find(v1)!=vSet.end()){ 86 | return (adjSetMap[v1].find(v2) != adjSetMap[v1].end()); 87 | } 88 | return false; 89 | } 90 | 91 | std::set& getNeighborSet(int v){ 92 | if(vSet.find(v)!=vSet.end()){ 93 | return adjSetMap[v]; 94 | } 95 | return emptySet; 96 | } 97 | 98 | 99 | 100 | // ===================================================================================== 101 | // Auxiliary methods 102 | // ===================================================================================== 103 | 104 | // If the graph is a cycle, this function returns the vertices of the cycle 105 | // ordered sequentially 106 | void getCycleVertexVector(std::vector& vVec){ 107 | if(vSet.size() == 0) return; 108 | std::set processedVertexSet; 109 | // Start from a vertex and retrive other vertices in order. We assume that 110 | // the cycle is big enough 111 | int vStart = *(vSet.begin()); 112 | vVec.push_back(vStart); 113 | processedVertexSet.insert(vStart); 114 | int vNext = *(getNeighborSet(vStart).begin()); 115 | while(vSet.size() > vVec.size()){ 116 | vVec.push_back(vNext); 117 | processedVertexSet.insert(vNext); 118 | std::set::iterator vit = getNeighborSet(vNext).begin(); 119 | vNext = *vit++; 120 | if(processedVertexSet.find(vNext) != processedVertexSet.end()){ 121 | vNext = *vit; 122 | } 123 | } 124 | } 125 | 126 | 127 | }; 128 | 129 | #endif //_O_GRAPH_H_ 130 | -------------------------------------------------------------------------------- /graphics_item.h: -------------------------------------------------------------------------------- 1 | /* 2 | * A GraphicsItem derived class to enable the filling of the inside of a polygonal region. 3 | * Qt GraphicsItem class does not do filling by default (at least as of 4.8.5). 4 | * 5 | * Created on: Jan 30, 2015 6 | * Author: Jingjin Yu 7 | */ 8 | 9 | #ifndef _O_ADV_GRAPHICS_ITEM_H_ 10 | #define _O_ADV_GRAPHICS_ITEM_H_ 11 | 12 | #include "types.h" 13 | #include "roadmap.h" 14 | 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | template 23 | class AdvancedGraphicsItem: public CGAL::Qt::PolygonGraphicsItem { 24 | public: 25 | bool m_bShowVertices; // Draw vertices? 26 | bool m_bShowEdge; // Draw edges? 27 | bool m_bFill; // Fill the polygon? 28 | 29 | QPen m_vertexPen; // Vertex pen 30 | QPen m_edgePen; // Edge pen 31 | QBrush m_fillBrush; // Fill brush 32 | 33 | // Need this constructor to take a T* pointer 34 | AdvancedGraphicsItem(T* p); 35 | 36 | // We need to overwrite paint method to do custom rendering such as filling the area. This 37 | // is not provided by default by QGraphicsItem in any other way. 38 | void paint( QPainter * painter, const QStyleOptionGraphicsItem * option, QWidget * widget); 39 | }; 40 | 41 | template AdvancedGraphicsItem 42 | ::AdvancedGraphicsItem(T* p): CGAL::Qt::PolygonGraphicsItem(p), 43 | m_bShowVertices(false),m_bShowEdge(true), m_bFill(true), 44 | m_vertexPen(QPen(Qt::red, 0.025, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)), 45 | m_edgePen(QPen(Qt::red, 0.025, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)), 46 | m_fillBrush(QColor(Qt::gray)){} 47 | 48 | 49 | // We need to overwrite paint method to do custom rendering such as filling the area. This 50 | // is not provided by default by QGraphicsItem in any other way. 51 | template void AdvancedGraphicsItem 52 | ::paint( QPainter * painter, const QStyleOptionGraphicsItem * option, QWidget * widget){ 53 | 54 | // Paint the edges 55 | if(m_bShowEdge){ 56 | painter->setPen(m_edgePen); 57 | CGAL::Qt::PolygonGraphicsItem::painterostream = CGAL::Qt::PainterOstream(painter); 58 | if(CGAL::Qt::PolygonGraphicsItem::drawEdges()) { 59 | for(typename T::Edge_const_iterator eit = CGAL::Qt::PolygonGraphicsItem::poly->edges_begin(); 60 | eit != CGAL::Qt::PolygonGraphicsItem::poly->edges_end(); 61 | ++eit){ 62 | CGAL::Qt::PolygonGraphicsItem::painterostream << *eit; 63 | } 64 | } 65 | } 66 | 67 | // Paint vertices if needed and obtain the fill area 68 | if(m_bShowVertices || m_bFill){ 69 | CGAL::Qt::Converter convert; 70 | painter->setPen(m_vertexPen); 71 | QMatrix matrix = painter->matrix(); 72 | painter->resetMatrix(); 73 | QPainterPath path; 74 | for(typename T::Vertex_iterator it = CGAL::Qt::PolygonGraphicsItem::poly->vertices_begin(); 75 | it != CGAL::Qt::PolygonGraphicsItem::poly->vertices_end(); 76 | it++){ 77 | QPointF point = matrix.map(convert(*it)); 78 | // Draw vertices here as needed 79 | if(m_bShowVertices){ 80 | painter->drawPoint(point); 81 | } 82 | if(it == CGAL::Qt::PolygonGraphicsItem::poly->vertices_begin()){ 83 | path.moveTo(point.rx(), point.ry()); 84 | } 85 | else{ 86 | path.lineTo(point.rx(), point.ry()); 87 | } 88 | } 89 | path.closeSubpath(); 90 | // Fill the area if needed 91 | if(m_bFill){ 92 | painter->fillPath(path, QBrush(m_fillBrush)); 93 | } 94 | } 95 | } 96 | 97 | #endif /* _O_ADV_GRAPHICS_ITEM_H_ */ 98 | -------------------------------------------------------------------------------- /helper_functions.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Helper function implementations 3 | * 4 | * Created on: Jan 30, 2015 5 | * Author: Jingjin Yu 6 | */ 7 | #include "helper_functions.h" 8 | 9 | void populateApproximateDisc(Polygon_2 &poly, Point_2 ¢er, double radius, int segments){ 10 | for(int i = 0; i < segments; i ++){ 11 | poly.push_back (Point_2 ( center.x() + radius*cos(i*PI*2/segments), center.y() + radius*sin(i*PI*2/segments))); 12 | } 13 | } 14 | 15 | Polygon_2 growPolygonByRadius(Polygon_2 &poly, double radius, int segments){ 16 | // Get the disk for computing Minkowski sum 17 | Polygon_2 disc; 18 | Point_2 p(0, 0); 19 | populateApproximateDisc(disc, p, radius, segments); 20 | 21 | // Do computation 22 | if(!poly.is_clockwise_oriented()){ 23 | poly.reverse_orientation(); 24 | } 25 | return minkowski_sum_2 (poly, disc).outer_boundary(); 26 | } 27 | 28 | double getPathLength(std::list path){ 29 | double length = 0; 30 | std::list::iterator vit = path.begin(); 31 | CGAL::Cartesian_converter K_ICK_converter; 32 | if(vit != path.end()){ 33 | ICPoint_2 p = K_ICK_converter(*vit); 34 | vit++; 35 | for(; vit != path.end(); vit++){ 36 | ICPoint_2 p2 = K_ICK_converter(*vit); 37 | length += sqrt((p2.x() - p.x())*(p2.x() - p.x()) + (p2.y() - p.y())*(p2.y() - p.y())); 38 | p = p2; 39 | } 40 | } 41 | return length; 42 | } 43 | 44 | double getDistance(Point_2& p1x, Point_2& p2x){ 45 | static CGAL::Cartesian_converter K_ICK_converter; 46 | ICPoint_2 p1 = K_ICK_converter(p1x); 47 | ICPoint_2 p2 = K_ICK_converter(p2x); 48 | return sqrt((p2.x() - p1.x())*(p2.x() - p1.x()) + (p2.y() - p1.y())*(p2.y() - p1.y())); 49 | } 50 | 51 | double getDistance(Point_2& p, double x, double y){ 52 | static CGAL::Cartesian_converter K_ICK_converter; 53 | ICPoint_2 pp = K_ICK_converter(p); 54 | return sqrt((pp.x() - x)*(pp.x() - x) + (pp.y() - y)*(pp.y() - y)); 55 | } 56 | 57 | void getAllPairsShortestPath (Graph* pGraph, map > &dist){ 58 | int V = pGraph->getVertexSet().size(); 59 | 60 | /* dist[][] will be the output matrix that will finally have the shortest 61 | distances between every pair of vertices */ 62 | int i, j, k; 63 | 64 | /* Initialize the solution matrix same as input graph matrix. Or 65 | we can say the initial values of shortest distances are based 66 | on shortest paths considering no intermediate vertex. */ 67 | for (i = 0; i < V; i++) 68 | { 69 | for (j = 0; j < V; j++){ 70 | dist[i][j] = (pGraph->hasEdge(i, j) ? 1 : (i == j?0:100000000)); 71 | } 72 | } 73 | 74 | /* Add all vertices one by one to the set of intermediate vertices. 75 | ---> Before start of a iteration, we have shortest distances between all 76 | pairs of vertices such that the shortest distances consider only the 77 | vertices in set {0, 1, 2, .. k-1} as intermediate vertices. 78 | ----> After the end of a iteration, vertex no. k is added to the set of 79 | intermediate vertices and the set becomes {0, 1, 2, .. k} */ 80 | for (k = 0; k < V; k++) 81 | { 82 | // Pick all vertices as source one by one 83 | for (i = 0; i < V; i++) 84 | { 85 | // Pick all vertices as destination for the 86 | // above picked source 87 | for (j = 0; j < V; j++) 88 | { 89 | // If vertex k is on the shortest path from 90 | // i to j, then update the value of dist[i][j] 91 | if (dist[i][k] + dist[k][j] < dist[i][j]) 92 | dist[i][j] = dist[i][k] + dist[k][j]; 93 | } 94 | } 95 | } 96 | } 97 | 98 | void converFromCGALtoVisilibity(ECPolygon_2 &poly, Polygon &visiPoly, bool ccw){ 99 | // Reverse orientation as needed 100 | if((poly.is_counterclockwise_oriented() && ccw == false) || 101 | (!poly.is_counterclockwise_oriented() && ccw)){ 102 | poly.reverse_orientation(); 103 | } 104 | 105 | // Get the vertices and convert 106 | static CGAL::Cartesian_converter EK_ICK_converter; 107 | vector vp; 108 | for(ECPolygon_2::Vertex_iterator vi = poly.vertices_begin(); vi != poly.vertices_end(); vi++){ 109 | ICPoint_2 p = EK_ICK_converter(*vi); 110 | vp.push_back(Point(p.x(), p.y())); 111 | } 112 | 113 | visiPoly.set_vertices(vp); 114 | } 115 | 116 | bool boundaryInterset(ECPolygon_2 &poly1, ECPolygon_2 &poly2){ 117 | // Check edge by edge 118 | for(int i = 0; i < poly1.size(); i++){ 119 | ECSegment_2 seg1 = poly1.edge(i); 120 | for(int j = 0; j < poly2.size(); j ++){ 121 | ECSegment_2 seg2 = poly2.edge(j); 122 | if(CGAL::do_intersect(seg1, seg2)){return true;} 123 | } 124 | } 125 | return false; 126 | } 127 | 128 | ECPoint_2 convertToExactPoint(Point_2 &p){ 129 | static CGAL::Cartesian_converter K_ICK_converter; 130 | ICPoint_2 pp = K_ICK_converter(p); 131 | return ECPoint_2(pp.x(), pp.y()); 132 | } 133 | 134 | 135 | ECPolygon_2 convertToExactPolygon(Polygon_2 &poly){ 136 | ECPolygon_2 ecPoly; 137 | for(Polygon_2::Vertex_iterator vi = poly.vertices_begin(); vi != poly.vertices_end(); vi ++){ 138 | static CGAL::Cartesian_converter K_ICK_converter; 139 | ICPoint_2 pp = K_ICK_converter(*vi); 140 | ecPoly.push_back(ECPoint_2(pp.x(), pp.y())); 141 | } 142 | return ecPoly; 143 | } 144 | -------------------------------------------------------------------------------- /helper_functions.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Some helper functions for geometric computing 3 | * 4 | * Created on: Jan 30, 2015 5 | * Author: Jingjin Yu 6 | */ 7 | 8 | #ifndef _O_CGAL_HELPER_H_ 9 | #define _O_CGAL_HELPER_H_ 10 | 11 | #include "types.h" 12 | #include "graph.h" 13 | #include "shortest_path/visilibity.hpp" 14 | #include 15 | 16 | using namespace std; 17 | using namespace VisiLibity; 18 | 19 | // Populate the polygon as an approximate disc with segments nuber of sides 20 | void populateApproximateDisc(Polygon_2 &poly, Point_2 ¢er, double radius, int segments = 18); 21 | 22 | // Compute Minkowski sum of a polygon with a disc 23 | Polygon_2 growPolygonByRadius(Polygon_2 &poly, double radius, int segments = 18); 24 | 25 | // Compute the distance between two points 26 | double getDistance(Point_2& p1, Point_2& p2); 27 | double getDistance(Point_2& p, double x, double y); 28 | 29 | // Compute path length 30 | double getPathLength(std::list path); 31 | 32 | // All pairs shortest path computation 33 | void getAllPairsShortestPath (Graph* pGraph, map > &dist); 34 | 35 | // Convert a CGAL polygon to a VisiLibity polygon 36 | void converFromCGALtoVisilibity(ECPolygon_2 &poly, Polygon &visiPoly, bool ccw = true); 37 | 38 | // Does two polygons have boundary edges that intersect? 39 | bool boundaryInterset(ECPolygon_2 &poly1, ECPolygon_2 &poly2); 40 | 41 | // Convert to exact contruction 42 | ECPoint_2 convertToExactPoint(Point_2 &p); 43 | ECPolygon_2 convertToExactPolygon(Polygon_2 &poly); 44 | 45 | #endif //_O_CGAL_HELPER_H_ 46 | -------------------------------------------------------------------------------- /main_window.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Main window class implemenation. 3 | * 4 | * Created on: Jan 30, 2015 5 | * Author: Jingjin Yu 6 | */ 7 | 8 | #include "main_window.h" 9 | #include "helper_functions.h" 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #if _MSC_VER == 1600 20 | #include 21 | #endif 22 | 23 | #include 24 | 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | 31 | MainWindow::MainWindow( QApplication *pApp, QString& fileFolder, 32 | QString title, QString iconPath, bool addDefaultContent) 33 | :m_pApp(pApp), m_fileFolder(fileFolder), m_state(BUILD_STATE_UNKNOWN),CGAL::Qt::DemosMainWindow() 34 | { 35 | m_pBoundingPolyAGI = 0; 36 | m_pBoundingPolyAGI2 = 0; 37 | 38 | // Set title 39 | setWindowTitle(title); 40 | 41 | // Set icon if the icon path is given 42 | if(iconPath.length() > 0){ 43 | this->setWindowIcon(QIcon(iconPath)); 44 | } 45 | 46 | // Check whether to display some default content 47 | if(addDefaultContent){ 48 | resize(600, 400); 49 | QLabel * label = new QLabel(tr("Central Widget")); 50 | setCentralWidget(label); 51 | label->setAlignment(Qt::AlignCenter); 52 | m_pView->setMouseTracking(true); 53 | } 54 | else{ 55 | m_pView = new QGraphicsView(this); 56 | #if _MSC_VER == 1600 57 | m_pView->setViewport(new QGLWidget(QGLFormat(QGL::SampleBuffers))); 58 | #endif 59 | 60 | m_scene.setItemIndexMethod(QGraphicsScene::NoIndex); 61 | m_scene.setSceneRect(-100, -100, 100, 100); 62 | m_pView->setScene(&m_scene); 63 | m_pView->setMouseTracking(true); 64 | 65 | this->view = m_pView; 66 | 67 | resize(620, 710); 68 | this->view->resize(620, 710); 69 | this->view->scale(1, 1); 70 | this->addNavigation(this->view); 71 | 72 | this->setupStatusBar(); 73 | setCentralWidget(m_pView); 74 | } 75 | 76 | m_pView->setRenderHint(QPainter::Antialiasing); 77 | 78 | // Setup timer 79 | m_pTimer = new QTimer(this); 80 | connect(m_pTimer, SIGNAL(timeout()), this, SLOT(animatePath())); 81 | 82 | // Setup toolbar 83 | setupMenuAndToolbar(); 84 | } 85 | 86 | void MainWindow::setupMenuAndToolbar(){ 87 | int iconSize = 40; 88 | m_pMainToolBar = new QToolBar(tr("Main Toolbar"), this); 89 | m_pMainToolBar->setIconSize(QSize(iconSize, iconSize)); 90 | addToolBar(m_pMainToolBar); 91 | // m_pMainToolBar->setVisible(false); 92 | 93 | 94 | // Create the environment menu 95 | QMenu* fileMenu = menuBar()->addMenu(tr("&Environment")); 96 | 97 | // Open action 98 | m_pOpenEnvAction = new QAction(tr("&Open Environment"), this); 99 | m_pOpenEnvAction->setStatusTip(tr("Open a new envrionment")); 100 | m_pOpenEnvAction->setIcon(QIcon("./resources/open.png")); 101 | connect(m_pOpenEnvAction, SIGNAL(triggered()), this, SLOT(openEnvironment())); 102 | fileMenu->addAction(m_pOpenEnvAction); 103 | m_pMainToolBar->addAction(m_pOpenEnvAction); 104 | 105 | m_pMainToolBar->addSeparator(); 106 | 107 | // Connect graph action 108 | m_pOverlayLatticAction = new QAction(tr("&Overlay Lattice Graph"), this); 109 | m_pOverlayLatticAction->setStatusTip(tr("Overlay covering lattice graph")); 110 | m_pOverlayLatticAction->setIcon(QIcon("./resources/hexgrid.png")); 111 | connect(m_pOverlayLatticAction, SIGNAL(triggered()), this, SLOT(overlayLattice())); 112 | fileMenu->addSeparator(); 113 | fileMenu->addAction(m_pOverlayLatticAction); 114 | m_pMainToolBar->addAction(m_pOverlayLatticAction); 115 | m_pOverlayLatticAction->setDisabled(true); // Disabled at beginning 116 | 117 | m_pLocateBoundaryAction = new QAction(tr("&Trim Lattice Graph"), this); 118 | m_pLocateBoundaryAction->setStatusTip(tr("Remove lattice parts outside the free space")); 119 | m_pLocateBoundaryAction->setIcon(QIcon("./resources/hexcycle.png")); 120 | connect(m_pLocateBoundaryAction, SIGNAL(triggered()), this, SLOT(locateBoundingLatticeCycle())); 121 | fileMenu->addAction(m_pLocateBoundaryAction); 122 | m_pMainToolBar->addAction(m_pLocateBoundaryAction); 123 | m_pLocateBoundaryAction->setDisabled(true); // Disabled at beginning 124 | 125 | QWidget* empty3 = new QWidget(); 126 | empty3->setFixedWidth(4); 127 | m_pMainToolBar->addWidget(empty3); 128 | 129 | m_pNumberofRobotLabel = new QLabel(this); 130 | m_pNumberofRobotLabel->setText(" Robots: "); 131 | m_pNumberofRobotLabel->setFixedHeight(iconSize-2); 132 | m_pMainToolBar->addWidget(m_pNumberofRobotLabel); 133 | 134 | m_pLineEdit = new QLineEdit(this); 135 | m_pLineEdit->setFixedWidth(30); 136 | m_pLineEdit->setFixedHeight(iconSize-2); 137 | m_pLineEdit->setText(QString("25")); 138 | m_pMainToolBar->addWidget(m_pLineEdit); 139 | 140 | QWidget* empty1 = new QWidget(); 141 | empty1->setFixedWidth(4); 142 | m_pMainToolBar->addWidget(empty1); 143 | 144 | m_pMinDistLabel = new QLabel(this); 145 | m_pMinDistLabel->setText(" Spacing: "); 146 | m_pMinDistLabel->setFixedHeight(iconSize-2); 147 | m_pMainToolBar->addWidget(m_pMinDistLabel); 148 | 149 | m_pMinDistLineEdit = new QLineEdit(this); 150 | m_pMinDistLineEdit->setFixedWidth(30); 151 | m_pMinDistLineEdit->setFixedHeight(iconSize-2); 152 | m_pMinDistLineEdit->setText(QString("3")); 153 | m_pMainToolBar->addWidget(m_pMinDistLineEdit); 154 | 155 | QWidget* empty2 = new QWidget(); 156 | empty2->setFixedWidth(4); 157 | m_pMainToolBar->addWidget(empty2); 158 | 159 | m_pCreateAction = new QAction(tr("&Create Random Problem"), this); 160 | m_pCreateAction->setStatusTip(tr("Create a random instance")); 161 | m_pCreateAction->setIcon(QIcon("./resources/sg.png")); 162 | connect(m_pCreateAction, SIGNAL(triggered()), this, SLOT(createRandomProblem())); 163 | fileMenu->addAction(m_pCreateAction); 164 | m_pMainToolBar->addAction(m_pCreateAction); 165 | m_pCreateAction->setDisabled(true); // Disabled at beginning 166 | 167 | m_pSolveAction = new QAction(tr("&Solve"), this); 168 | m_pSolveAction->setStatusTip(tr("Solve a randomly created instance")); 169 | m_pSolveAction->setIcon(QIcon("./resources/solution.png")); 170 | connect(m_pSolveAction, SIGNAL(triggered()), this, SLOT(solveProblem())); 171 | fileMenu->addAction(m_pSolveAction); 172 | m_pMainToolBar->addAction(m_pSolveAction); 173 | m_pSolveAction->setDisabled(true); // Disabled at beginning 174 | 175 | m_pPlayAction = new QAction(tr("&Animate"), this); 176 | m_pPlayAction->setStatusTip(tr("Animate the solution")); 177 | m_pPlayAction->setIcon(QIcon("./resources/play.png")); 178 | connect(m_pPlayAction, SIGNAL(triggered()), this, SLOT(animate())); 179 | fileMenu->addAction(m_pPlayAction); 180 | m_pMainToolBar->addAction(m_pPlayAction); 181 | m_pPlayAction->setDisabled(true); // Disabled at beginning 182 | 183 | // Fit to screen action 184 | m_pFitScreenAction = new QAction(tr("&Fit content to view"), this); 185 | m_pFitScreenAction->setStatusTip(tr("Fit content to view")); 186 | m_pFitScreenAction->setIcon(QIcon("./resources/fitview.png")); 187 | connect(m_pFitScreenAction, SIGNAL(triggered()), this, SLOT(fitView())); 188 | fileMenu->addSeparator(); 189 | fileMenu->addAction(m_pFitScreenAction); 190 | m_pMainToolBar->addAction(m_pFitScreenAction); 191 | 192 | } 193 | 194 | void MainWindow::openEnvironment(){ 195 | // Open file dialog for file selection 196 | QString fileName = QFileDialog::getOpenFileName(this, tr("Open Environment Description File"), "", tr("Files (*.*)")); 197 | if(fileName == 0 || fileName.length() < 0) return; 198 | 199 | QStringList qsl = fileName.split("/"); 200 | m_envFileName = qsl[qsl.size()-1].split(".")[0]; 201 | 202 | // Clean up 203 | m_scene.clear(); 204 | m_boundingPoly.clear(); 205 | m_boundingPoly2.clear(); 206 | m_envPolyList.clear(); 207 | m_envObsPolyList.clear(); 208 | m_PolyAGIList.clear(); /* The element pointers are gone when the scene object is cleared */ 209 | 210 | // Reading in the environment, first the raidus of the (disc) robot 211 | std::ifstream ifs(qPrintable(fileName)); 212 | double radius; 213 | ifs >> radius; 214 | 215 | // Then the number of obstacle polygons 216 | int numberOfPolygons; 217 | ifs >> numberOfPolygons; 218 | 219 | // Then read in all obstacle polygons and compute the configuration space for a single disc 220 | for(int i = 0; i < numberOfPolygons; i ++){ 221 | // Load polygon 222 | Polygon_2 tp; 223 | ifs >> tp; 224 | m_envPolyList.push_back(tp); 225 | 226 | // Add raw obstacle to scene and set fill to be true with fill transparency 227 | AdvancedGraphicsItem* pAGI = new AdvancedGraphicsItem(&(m_envPolyList.back())); 228 | m_PolyAGIList.push_back(pAGI); 229 | // m_scene.addItem(pAGI); 230 | pAGI->m_bShowEdge = false; 231 | pAGI->m_bShowVertices = false; 232 | pAGI->m_bFill = true; 233 | pAGI->m_fillBrush = QColor(16,16,16,192); 234 | 235 | // Computing the Minkowski sum 236 | Polygon_2 ep = growPolygonByRadius(tp, radius); 237 | m_envObsPolyList.push_back(ep); 238 | 239 | // Add the configuration space obstacle to the scene 240 | pAGI = new AdvancedGraphicsItem(&(m_envObsPolyList.back())); 241 | m_PolyAGIList.push_back(pAGI); 242 | // m_scene.addItem(pAGI); 243 | pAGI->m_bFill = false; 244 | pAGI->m_edgePen = QPen(Qt::gray, 0.25, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin); 245 | pAGI->m_vertexPen = QPen(Qt::black, 0.5, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin); 246 | } 247 | 248 | // Then read the bounding rectangle (configuration space) 249 | ifs >> m_boundingPoly; 250 | 251 | // Add bounding walls 252 | CGAL::Cartesian_converter K_ICK_converter; 253 | 254 | // Add to scence 255 | m_pBoundingPolyAGI = new AdvancedGraphicsItem(&m_boundingPoly); 256 | m_pBoundingPolyAGI->m_bFill = false; 257 | m_pBoundingPolyAGI->m_bShowVertices = false; 258 | m_pBoundingPolyAGI->m_bShowEdge = true; 259 | m_pBoundingPolyAGI->m_edgePen = QPen(Qt::gray, 0.25, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin); 260 | m_PolyAGIList.push_back(m_pBoundingPolyAGI); 261 | // m_scene.addItem(m_pBoundingPolyAGI); 262 | 263 | // Then compute the outside bounding rectangle 264 | double x1, y1, x2, y2; 265 | x1 = K_ICK_converter(m_boundingPoly[0].x()); y1 = K_ICK_converter(m_boundingPoly[0].y()); 266 | x2 = K_ICK_converter(m_boundingPoly[2].x()); y2 = K_ICK_converter(m_boundingPoly[2].y()); 267 | m_boundingPoly2.push_back(Point_2(x1 - radius, y1 - radius)); 268 | m_boundingPoly2.push_back(Point_2(x1 - radius, y2 + radius)); 269 | m_boundingPoly2.push_back(Point_2(x2 + radius, y2 + radius)); 270 | m_boundingPoly2.push_back(Point_2(x2 + radius, y1 - radius)); 271 | 272 | // Add to scene 273 | m_pBoundingPolyAGI2 = new AdvancedGraphicsItem(&m_boundingPoly2); 274 | m_pBoundingPolyAGI2->m_bFill = false; 275 | m_pBoundingPolyAGI2->m_bShowVertices = false; 276 | m_pBoundingPolyAGI2->m_bShowEdge = true; 277 | m_pBoundingPolyAGI2->m_edgePen = QPen(Qt::black, 0.25, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin); 278 | m_PolyAGIList.push_back(m_pBoundingPolyAGI2); 279 | // m_scene.addItem(m_pBoundingPolyAGI2); 280 | 281 | m_radius = radius; 282 | 283 | drawBasicEnvironment(); 284 | 285 | // Do roadmap building setup 286 | m_roadmap.buildRoadmap(&m_envObsPolyList, &m_boundingPoly, radius); 287 | // m_roadmap.addToScene(m_scene); 288 | 289 | m_boundingRect = QRectF(x1 - radius*4, y1 - radius*4, x2 + radius*4, y2 + radius*4); 290 | 291 | // Fit to the view 292 | fitView(); 293 | 294 | // Enable the actions for building the graph 295 | m_state = BUILD_STATE_MAP_OPEN; 296 | m_pOverlayLatticAction->setEnabled(true); 297 | m_pLocateBoundaryAction->setEnabled(true); 298 | m_pCreateAction->setEnabled(false); 299 | m_pSolveAction->setEnabled(false); 300 | m_pPlayAction->setEnabled(false); 301 | 302 | } 303 | 304 | void MainWindow::drawBasicEnvironment(){ 305 | m_PolyAGIList.clear(); 306 | for(Polygon2_list::iterator pit = m_envPolyList.begin(); pit != m_envPolyList.end(); pit++){ 307 | // Add raw obstacle to scene and set fill to be true with fill transparency 308 | AdvancedGraphicsItem* pAGI = new AdvancedGraphicsItem(&(*pit)); 309 | m_PolyAGIList.push_back(pAGI); 310 | pAGI->m_bShowEdge = false; 311 | pAGI->m_bShowVertices = false; 312 | pAGI->m_bFill = true; 313 | pAGI->m_fillBrush = QColor(16,16,16,192); 314 | } 315 | 316 | for(Polygon2_list::iterator pit = m_envObsPolyList.begin(); pit != m_envObsPolyList.end(); pit++){ 317 | // Add the configuration space obstacle to the scene 318 | AdvancedGraphicsItem* pAGI = new AdvancedGraphicsItem(&(*pit)); 319 | m_PolyAGIList.push_back(pAGI); 320 | pAGI->m_bFill = false; 321 | pAGI->m_edgePen = QPen(Qt::gray, 0.25, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin); 322 | pAGI->m_vertexPen = QPen(Qt::black, 0.5, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin); 323 | } 324 | 325 | // Add to scence 326 | m_pBoundingPolyAGI = new AdvancedGraphicsItem(&m_boundingPoly); 327 | m_pBoundingPolyAGI->m_bFill = false; 328 | m_pBoundingPolyAGI->m_bShowVertices = false; 329 | m_pBoundingPolyAGI->m_bShowEdge = true; 330 | m_pBoundingPolyAGI->m_edgePen = QPen(Qt::gray, 0.25, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin); 331 | m_PolyAGIList.push_back(m_pBoundingPolyAGI); 332 | 333 | // Add to scene 334 | m_pBoundingPolyAGI2 = new AdvancedGraphicsItem(&m_boundingPoly2); 335 | m_pBoundingPolyAGI2->m_bFill = false; 336 | m_pBoundingPolyAGI2->m_bShowVertices = false; 337 | m_pBoundingPolyAGI2->m_bShowEdge = true; 338 | m_pBoundingPolyAGI2->m_edgePen = QPen(Qt::black, 0.25, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin); 339 | m_PolyAGIList.push_back(m_pBoundingPolyAGI2); 340 | 341 | for(std::vector *>::iterator pagiItor = m_PolyAGIList.begin(); 342 | pagiItor != m_PolyAGIList.end(); pagiItor ++){ 343 | AdvancedGraphicsItem* pAGI = *pagiItor; 344 | m_scene.addItem(pAGI); 345 | } 346 | } 347 | 348 | 349 | void MainWindow::overlayLattice(){ 350 | m_scene.clear(); 351 | m_roadmap.buildHexgaonLattice(); 352 | drawBasicEnvironment(); 353 | m_roadmap.drawHexagonLattice(m_scene, true); 354 | 355 | // Set build state and enable the actions for building the graph 356 | m_state = BUILD_STATE_LATTICE_OVERLAYED; 357 | m_pOverlayLatticAction->setEnabled(false); 358 | } 359 | 360 | void MainWindow::locateBoundingLatticeCycle(){ 361 | // Clean up 362 | m_scene.clear(); 363 | 364 | // Build depending on the build state 365 | switch(m_state){ 366 | case BUILD_STATE_MAP_OPEN: 367 | m_roadmap.buildHexgaonLattice(); 368 | m_pOverlayLatticAction->setEnabled(false); 369 | case BUILD_STATE_LATTICE_OVERLAYED: 370 | m_roadmap.removeExcessEdges(); 371 | m_pLocateBoundaryAction->setEnabled(false); 372 | default:; 373 | } 374 | 375 | // Draw environment 376 | drawBasicEnvironment(); 377 | 378 | // Draw the lattice (now truncated) 379 | m_roadmap.drawHexagonLattice(m_scene, true); 380 | #ifdef _DEBUG 381 | m_roadmap.drawBoundingCycle(m_scene); 382 | m_roadmap.drawVertexIds(m_scene); 383 | #endif 384 | 385 | // Set build state 386 | m_state = BUILD_STATE_TRIMMED; 387 | 388 | // Enable random instance creation 389 | m_pCreateAction->setEnabled(true); 390 | } 391 | 392 | // Create and solve a random problem over current graph 393 | void MainWindow::createRandomProblem() { 394 | // Stop any ongoing timer 395 | m_pTimer->stop(); 396 | 397 | // Temporarily disable the action 398 | m_pCreateAction->setEnabled(false); 399 | m_pApp->processEvents(); 400 | 401 | // Do clean up 402 | m_pRobotItemVec.clear(); 403 | m_pTextItemVec.clear(); 404 | m_svVec.clear(); 405 | m_gvVec.clear(); 406 | m_pathMap.clear(); 407 | m_pPathLineItems.clear(); 408 | 409 | // Get number of robots 410 | m_numRobots = m_pLineEdit->text().toInt(); 411 | double spacing = m_pMinDistLineEdit->text().toDouble(); 412 | 413 | srand (time(NULL)); 414 | 415 | // Setup problem 416 | vector > vvg; 417 | vector svv; 418 | vector gvv; 419 | while(true){ 420 | try{ 421 | // Create random start and goal vertices 422 | m_svVec.clear(); m_gvVec.clear(); svv.clear(); gvv.clear(); 423 | m_roadmap.createRandomStartGoalPairs(m_numRobots, spacing, m_svVec, m_gvVec); 424 | 425 | // Snap to graph 426 | m_sgVec.clear(); 427 | m_roadmap.snapToGraph(m_svVec, svv); 428 | m_roadmap.snapToGraph(m_gvVec, gvv); 429 | break; 430 | } 431 | catch(...){ 432 | } 433 | } 434 | 435 | for(int r = 0; r < m_numRobots; r ++){ 436 | m_sgVec.push_back(pair(svv[r], gvv[r])); 437 | } 438 | #ifdef _DEBUG 439 | for(int r = 0; r < m_numRobots; r ++){ 440 | cout << setw(3) << svv[r]; 441 | } 442 | cout << endl; 443 | for(int r = 0; r < m_numRobots; r ++){ 444 | cout << setw(3) << gvv[r]; 445 | } 446 | cout << endl; 447 | #endif 448 | 449 | // Draw start and end robot locations 450 | m_scene.clear(); 451 | drawBasicEnvironment(); 452 | #ifdef _DEBUG 453 | m_roadmap.drawBoundingCycle(m_scene); 454 | m_roadmap.drawVertexIds(m_scene); 455 | #endif 456 | 457 | // Draw the robots 458 | QFont font; 459 | QPainterPath path; 460 | font.setPointSizeF(m_radius/1.5); 461 | font.setBold(true); 462 | 463 | for(int r = 0; r < m_numRobots; r ++){ 464 | // Robots at current (start) locations 465 | QGraphicsEllipseItem *prs = m_scene.addEllipse(m_svVec[r].first - m_radius, m_svVec[r].second - m_radius, m_radius*2-0.2, m_radius*2-0.2, 466 | QPen(Qt::black, 0.2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin), 467 | QBrush(Qt::blue)); 468 | prs->setZValue(10); 469 | m_pRobotItemVec.push_back(prs); 470 | 471 | // Labels for robots at current (start) locations 472 | QGraphicsSimpleTextItem *ti = m_scene.addSimpleText(QString::number(r+1), font); 473 | ti->setPos(m_svVec[r].first - m_radius/2*(3.5/(m_radius+1)) + (r < 9 ? m_radius / 6 *(2.2/(m_radius-0.3)) :(r > 99 ? -m_radius / 6 *(2.2/(m_radius-0.3)) :0 )) , m_svVec[r].second - m_radius/2*(2./(m_radius-0.5))); 474 | ti->setPen(QPen(QColor(Qt::white), 0.15, Qt::SolidLine, Qt::RoundCap,Qt::RoundJoin)); 475 | ti->setZValue(15); 476 | m_pTextItemVec.push_back(ti); 477 | 478 | // Robots at goal locations 479 | QGraphicsEllipseItem *prg = m_scene.addEllipse(m_gvVec[r].first - m_radius, m_gvVec[r].second - m_radius, m_radius*2-0.2, m_radius*2-0.2, 480 | QPen(QColor(127, 127, 127, 64), 0.2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin), 481 | QBrush(QColor(255, 127, 127, 64))); 482 | prg->setZValue(5); 483 | 484 | // Labels for robots at goal locations 485 | ti = m_scene.addSimpleText(QString::number(r+1), font); 486 | ti->setPos(m_gvVec[r].first - m_radius/2*(3.5/(m_radius+1)) + (r < 9 ? m_radius / 6 *(2.2/(m_radius-0.3)) :(r > 99 ? -m_radius / 6 *(2.2/(m_radius-0.3)) :0 )), m_gvVec[r].second - m_radius/2*(2./(m_radius-0.5))); 487 | ti->setPen(QPen(QColor(Qt::red), 0.15, Qt::SolidLine, Qt::RoundCap,Qt::RoundJoin)); 488 | ti->setZValue(6); 489 | } 490 | 491 | // Always build visibility graph and compute shortest possible makespan 492 | m_roadmap.buildVisibilityGraph(); 493 | double maxLength = 0.; 494 | for(int r = 0; r < m_numRobots; r ++){ 495 | vector > path; 496 | double length = m_roadmap.computeShortestPath(m_svVec[r].first, m_svVec[r].second, m_gvVec[r].first, m_gvVec[r].second, path); 497 | if(length > maxLength){ maxLength = length; } 498 | } 499 | m_shotestPathLength = maxLength; 500 | #ifdef _DEBUG 501 | cout << "Min possible makespan: " << m_shotestPathLength << endl; 502 | #endif 503 | 504 | // Update action enable/disable status 505 | m_pCreateAction->setEnabled(true); 506 | m_pSolveAction->setEnabled(true); 507 | m_pPlayAction->setEnabled(false); 508 | } 509 | 510 | 511 | void MainWindow::solveProblem() { 512 | // Setup animation. Note that the solution will be computed the first time animatePath() 513 | // is called 514 | m_pCreateAction->setEnabled(false); 515 | m_pSolveAction->setEnabled(false); 516 | m_roadmap.solveProblem(m_sgVec, m_pathMap, m_fileFolder.toStdString(), m_envFileName.toStdString()); 517 | m_pPlayAction->setEnabled(true); 518 | m_pCreateAction->setEnabled(true); 519 | } 520 | 521 | void MainWindow::animate(){ 522 | // Remove all paths line items from scence 523 | for(vector::iterator lii = m_pPathLineItems.begin(); 524 | lii != m_pPathLineItems.end(); lii ++) 525 | { 526 | m_scene.removeItem(*lii); 527 | } 528 | m_pPathLineItems.clear(); 529 | 530 | // Disable play action 531 | m_pPlayAction->setEnabled(false); 532 | m_pApp->processEvents(); 533 | 534 | // Sleep for half a second 535 | boost::this_thread::sleep(boost::posix_time::milliseconds(500)); 536 | 537 | // Put all start to the initial location 538 | m_aniState = ANI_STATE_FROM_START; 539 | m_pathStep = 0; 540 | m_aniStatePercent = 0.0; 541 | for(int r = 0; r < m_numRobots; r++){ 542 | m_currentStartMap[r] = m_svVec[r]; 543 | m_currentGoalMap[r] = m_roadmap.getVertexLocationFromID(m_pathMap[r][0]); 544 | double x = m_currentStartMap[r].first*(1-m_aniStatePercent) + m_currentGoalMap[r].first*m_aniStatePercent; 545 | double y = m_currentStartMap[r].second*(1-m_aniStatePercent) + m_currentGoalMap[r].second*m_aniStatePercent; 546 | m_pRobotItemVec[r]->setPos(x - m_svVec[r].first, y - m_svVec[r].second); 547 | m_pTextItemVec[r]->setPos(x - m_radius/2*(3.5/(m_radius+1)) + (r < 9 ? m_radius / 6*(2.2/(m_radius-0.3)):0), y - m_radius/2*(2./(m_radius-0.5))); 548 | } 549 | m_pApp->processEvents(); 550 | 551 | // Sleep for half a second 552 | boost::this_thread::sleep(boost::posix_time::milliseconds(2000)); 553 | 554 | // Start animation 555 | m_pTimer->start(35); 556 | } 557 | 558 | void MainWindow::animatePath(){ 559 | // If we are at the beginning of a stage, set the temp starts and goals 560 | if(m_aniStatePercent == 0.0){ 561 | for(int r = 0; r < m_numRobots; r ++){ 562 | switch(m_aniState){ 563 | // Snapping from start to the graph 564 | case ANI_STATE_FROM_START: 565 | m_currentStartMap[r] = m_svVec[r]; 566 | m_currentGoalMap[r] = m_roadmap.getVertexLocationFromID(m_pathMap[r][0]); 567 | break; 568 | case ANI_STATE_GRID_PATH: 569 | m_currentStartMap[r] = m_roadmap.getVertexLocationFromID(m_pathMap[r][m_pathStep]); 570 | m_currentGoalMap[r] = m_roadmap.getVertexLocationFromID(m_pathMap[r][m_pathStep + 1]); 571 | break; 572 | case ANI_STATE_TO_GOAL: 573 | m_currentStartMap[r] = m_roadmap.getVertexLocationFromID(m_pathMap[r][m_pathStep]); 574 | m_currentGoalMap[r] = m_gvVec[r]; 575 | break; 576 | case ANI_STATE_COMPLETE: 577 | default: 578 | return; 579 | } 580 | } 581 | } 582 | 583 | // Animate 584 | m_aniStatePercent += 0.100001; 585 | if(m_aniState == ANI_STATE_FROM_START || m_aniState == ANI_STATE_TO_GOAL) m_aniStatePercent += 0.100001; 586 | for(int r = 0; r < m_numRobots; r++){ 587 | double x = m_currentStartMap[r].first*(1-m_aniStatePercent) + m_currentGoalMap[r].first*m_aniStatePercent; 588 | double y = m_currentStartMap[r].second*(1-m_aniStatePercent) + m_currentGoalMap[r].second*m_aniStatePercent; 589 | m_pRobotItemVec[r]->setPos(x - m_svVec[r].first, y - m_svVec[r].second); 590 | m_pTextItemVec[r]->setPos(x - m_radius/2*(3.5/(m_radius+1)) + (r < 9 ? m_radius / 6*(2.2/(m_radius-0.3)):0), y - m_radius/2*(2./(m_radius-0.5))); 591 | } 592 | 593 | // Check current stage perecentage 594 | if(m_aniStatePercent > 1.0){ 595 | m_aniStatePercent = 0.0; 596 | switch(m_aniState){ 597 | // Snapping from start to the graph 598 | case ANI_STATE_FROM_START: 599 | m_aniState = ANI_STATE_GRID_PATH; 600 | break; 601 | case ANI_STATE_GRID_PATH: 602 | m_pathStep ++; 603 | if(m_pathStep == m_pathMap[0].size() - 1){ 604 | m_aniState = ANI_STATE_TO_GOAL; 605 | } 606 | break; 607 | case ANI_STATE_TO_GOAL: 608 | m_aniState = ANI_STATE_COMPLETE; 609 | m_pTimer->stop(); 610 | m_pPlayAction->setEnabled(true); 611 | break; 612 | } 613 | } 614 | } 615 | 616 | // Fit the content to the current view port 617 | void MainWindow::fitView(){ 618 | if(m_pBoundingPolyAGI2 != 0){ 619 | m_pView->setSceneRect(m_pBoundingPolyAGI2->boundingRect()); 620 | m_pView->fitInView(m_boundingRect, Qt::KeepAspectRatio); 621 | } 622 | } 623 | -------------------------------------------------------------------------------- /main_window.h: -------------------------------------------------------------------------------- 1 | /* 2 | * The main window class. 3 | * 4 | * Created on: Jan 30, 2015 5 | * Author: Jingjin Yu 6 | */ 7 | 8 | #ifndef _MAIN_WINDOW_H_ 9 | #define _MAIN_WINDOW_H_ 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include "helper_functions.h" 24 | #include "types.h" 25 | #include "graphics_item.h" 26 | #include "roadmap.h" 27 | 28 | class MainWindow : public CGAL::Qt::DemosMainWindow 29 | { 30 | Q_OBJECT 31 | 32 | public: 33 | MainWindow( QApplication *pApp, QString& fileFolder, 34 | QString title = "Main Window", QString iconPath = "", bool addDefaultContent = false); 35 | 36 | void setupMenuAndToolbar(); 37 | 38 | public slots: 39 | void openEnvironment(); 40 | 41 | void overlayLattice(); 42 | void locateBoundingLatticeCycle(); 43 | 44 | void createRandomProblem(); 45 | void solveProblem(); 46 | void animate(); 47 | 48 | void fitView(); 49 | 50 | void animatePath(); 51 | 52 | protected: 53 | QApplication * m_pApp; 54 | QString m_fileFolder; 55 | 56 | QToolBar * m_pMainToolBar; 57 | 58 | QAction * m_pOpenEnvAction; 59 | 60 | QAction * m_pOverlayLatticAction; 61 | QAction * m_pLocateBoundaryAction; 62 | QAction * m_pConnectGraphAction; 63 | 64 | QAction * m_pCreateAction; 65 | QAction * m_pSolveAction; 66 | QAction * m_pPlayAction; 67 | 68 | QLabel * m_pNumberofRobotLabel; 69 | QLineEdit * m_pLineEdit; 70 | 71 | QLabel * m_pMinDistLabel; 72 | QLineEdit * m_pMinDistLineEdit; 73 | 74 | QAction * m_pFitScreenAction; 75 | 76 | QString m_envFileName; 77 | 78 | QGraphicsView* m_pView; 79 | QGraphicsScene m_scene; 80 | 81 | // Internal building state 82 | int m_state; 83 | static const int BUILD_STATE_UNKNOWN = -1; 84 | static const int BUILD_STATE_MAP_OPEN = 0; 85 | static const int BUILD_STATE_LATTICE_OVERLAYED = 1; 86 | static const int BUILD_STATE_TRIMMED = 2; 87 | static const int BUILD_STATE_CONNECTION_FIXED = 3; 88 | 89 | // The environment 90 | double m_radius; 91 | Polygon_2 m_boundingPoly; 92 | Polygon_2 m_boundingPoly2; 93 | Polygon2_list m_envPolyList; 94 | Polygon2_list m_envObsPolyList; 95 | 96 | AdvancedGraphicsItem * m_pBoundingPolyAGI; 97 | AdvancedGraphicsItem * m_pBoundingPolyAGI2; 98 | std::vector *> m_PolyAGIList; 99 | 100 | // Bounding rect 101 | QRectF m_boundingRect; 102 | 103 | // Roadmap 104 | Roadmap m_roadmap; 105 | 106 | // Number of robots 107 | int m_numRobots; 108 | 109 | // Timer for animation 110 | QTimer * m_pTimer; 111 | 112 | // Start and goal locations 113 | vector > m_svVec; 114 | vector > m_gvVec; 115 | vector > m_sgVec; 116 | 117 | // Paths 118 | map > m_pathMap; 119 | 120 | // Current 121 | map > m_currentStartMap; 122 | map > m_currentGoalMap; 123 | 124 | // Vector of robot graphics item pointers 125 | vector m_pRobotItemVec; 126 | vector m_pTextItemVec; 127 | 128 | static const int ANI_STATE_FROM_START = 0; 129 | static const int ANI_STATE_GRID_PATH = 10; 130 | static const int ANI_STATE_TO_GOAL = 20; 131 | static const int ANI_STATE_COMPLETE = 30; 132 | 133 | // Shortest path related 134 | double m_shotestPathLength; 135 | vector m_pPathLineItems; 136 | 137 | int m_aniState; 138 | int m_pathStep; 139 | double m_aniStatePercent; 140 | 141 | private: 142 | // Rendering 143 | void drawBasicEnvironment(); 144 | 145 | } ; 146 | 147 | #endif //_MAIN_WINDOW_H_ 148 | -------------------------------------------------------------------------------- /main_window_moc.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | ** Meta object code from reading C++ file 'main_window.h' 3 | ** 4 | ** Created by: The Qt Meta Object Compiler version 63 (Qt 4.8.6) 5 | ** 6 | ** WARNING! All changes made in this file will be lost! 7 | *****************************************************************************/ 8 | 9 | #include "main_window.h" 10 | #if !defined(Q_MOC_OUTPUT_REVISION) 11 | #error "The header file 'main_window.h' doesn't include ." 12 | #elif Q_MOC_OUTPUT_REVISION != 63 13 | #error "This file was generated using the moc from 4.8.6. It" 14 | #error "cannot be used with the include files from this version of Qt." 15 | #error "(The moc has changed too much.)" 16 | #endif 17 | 18 | QT_BEGIN_MOC_NAMESPACE 19 | static const uint qt_meta_data_MainWindow[] = { 20 | 21 | // content: 22 | 6, // revision 23 | 0, // classname 24 | 0, 0, // classinfo 25 | 7, 14, // methods 26 | 0, 0, // properties 27 | 0, 0, // enums/sets 28 | 0, 0, // constructors 29 | 0, // flags 30 | 0, // signalCount 31 | 32 | // slots: signature, parameters, type, tag, flags 33 | 11, 29, 29, 29, 0x0a, 34 | 30, 29, 29, 29, 0x0a, 35 | 47, 29, 29, 29, 0x0a, 36 | 76, 29, 29, 29, 0x0a, 37 | 98, 29, 29, 29, 0x0a, 38 | 111, 29, 29, 29, 0x0a, 39 | 121, 29, 29, 29, 0x0a, 40 | 41 | 0 // eod 42 | }; 43 | 44 | static const char qt_meta_stringdata_MainWindow[] = { 45 | "MainWindow\0openEnvironment()\0\0" 46 | "overlayLattice()\0locateBoundingLatticeCycle()\0" 47 | "restoreConnectivity()\0buildGraph()\0" 48 | "fitView()\0saveGraph()\0" 49 | }; 50 | 51 | void MainWindow::qt_static_metacall(QObject *_o, QMetaObject::Call _c, int _id, void **_a) 52 | { 53 | if (_c == QMetaObject::InvokeMetaMethod) { 54 | Q_ASSERT(staticMetaObject.cast(_o)); 55 | MainWindow *_t = static_cast(_o); 56 | switch (_id) { 57 | case 0: _t->openEnvironment(); break; 58 | case 1: _t->overlayLattice(); break; 59 | case 2: _t->locateBoundingLatticeCycle(); break; 60 | case 3: _t->restoreConnectivity(); break; 61 | case 4: _t->buildGraph(); break; 62 | case 5: _t->fitView(); break; 63 | case 6: _t->saveGraph(); break; 64 | default: ; 65 | } 66 | } 67 | Q_UNUSED(_a); 68 | } 69 | 70 | const QMetaObjectExtraData MainWindow::staticMetaObjectExtraData = { 71 | 0, qt_static_metacall 72 | }; 73 | 74 | const QMetaObject MainWindow::staticMetaObject = { 75 | { &CGAL::Qt::DemosMainWindow::staticMetaObject, qt_meta_stringdata_MainWindow, 76 | qt_meta_data_MainWindow, &staticMetaObjectExtraData } 77 | }; 78 | 79 | #ifdef Q_NO_DATA_RELOCATION 80 | const QMetaObject &MainWindow::getStaticMetaObject() { return staticMetaObject; } 81 | #endif //Q_NO_DATA_RELOCATION 82 | 83 | const QMetaObject *MainWindow::metaObject() const 84 | { 85 | return QObject::d_ptr->metaObject ? QObject::d_ptr->metaObject : &staticMetaObject; 86 | } 87 | 88 | void *MainWindow::qt_metacast(const char *_clname) 89 | { 90 | if (!_clname) return 0; 91 | if (!strcmp(_clname, qt_meta_stringdata_MainWindow)) 92 | return static_cast(const_cast< MainWindow*>(this)); 93 | typedef CGAL::Qt::DemosMainWindow QMocSuperClass; 94 | return QMocSuperClass::qt_metacast(_clname); 95 | } 96 | 97 | int MainWindow::qt_metacall(QMetaObject::Call _c, int _id, void **_a) 98 | { 99 | typedef CGAL::Qt::DemosMainWindow QMocSuperClass; 100 | _id = QMocSuperClass::qt_metacall(_c, _id, _a); 101 | if (_id < 0) 102 | return _id; 103 | if (_c == QMetaObject::InvokeMetaMethod) { 104 | if (_id < 7) 105 | qt_static_metacall(this, _c, _id, _a); 106 | _id -= 7; 107 | } 108 | return _id; 109 | } 110 | QT_END_MOC_NAMESPACE 111 | -------------------------------------------------------------------------------- /mp.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/arc-l/optimal-mrpp-continuous/d9a85869f79b640984c7469fb8c3aaa4da14a0b2/mp.jar -------------------------------------------------------------------------------- /number_type.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Number type selection header 3 | * 4 | * Created on: Jan 30, 2015 5 | * Author: Jingjin Yu 6 | */ 7 | #ifndef _O_CGAL_MS_RATIONAL_NT_H 8 | #define _O_CGAL_MS_RATIONAL_NT_H 9 | 10 | #ifndef PI 11 | #define PI 3.14159 12 | #endif 13 | 14 | #ifndef BOOST_ALL_NO_LIB 15 | #if _MSC_VER == 1600 16 | #define BOOST_ALL_NO_LIB 17 | #endif 18 | #endif 19 | 20 | #include 21 | 22 | #ifdef CGAL_USE_GMP 23 | 24 | // GMP is installed. Use the GMP rational number-type. 25 | #include 26 | 27 | typedef CGAL::Gmpq Number_type; 28 | 29 | #else 30 | 31 | // GMP is not installed. Use CGAL's exact rational number-type. 32 | #include 33 | #include 34 | 35 | typedef CGAL::Quotient Number_type; 36 | 37 | #endif 38 | 39 | #endif 40 | -------------------------------------------------------------------------------- /resources/app.ico: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/arc-l/optimal-mrpp-continuous/d9a85869f79b640984c7469fb8c3aaa4da14a0b2/resources/app.ico -------------------------------------------------------------------------------- /resources/fitview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/arc-l/optimal-mrpp-continuous/d9a85869f79b640984c7469fb8c3aaa4da14a0b2/resources/fitview.png -------------------------------------------------------------------------------- /resources/hexcycle.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/arc-l/optimal-mrpp-continuous/d9a85869f79b640984c7469fb8c3aaa4da14a0b2/resources/hexcycle.png -------------------------------------------------------------------------------- /resources/hexgrid.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/arc-l/optimal-mrpp-continuous/d9a85869f79b640984c7469fb8c3aaa4da14a0b2/resources/hexgrid.png -------------------------------------------------------------------------------- /resources/open.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/arc-l/optimal-mrpp-continuous/d9a85869f79b640984c7469fb8c3aaa4da14a0b2/resources/open.png -------------------------------------------------------------------------------- /resources/play.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/arc-l/optimal-mrpp-continuous/d9a85869f79b640984c7469fb8c3aaa4da14a0b2/resources/play.png -------------------------------------------------------------------------------- /resources/sg.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/arc-l/optimal-mrpp-continuous/d9a85869f79b640984c7469fb8c3aaa4da14a0b2/resources/sg.png -------------------------------------------------------------------------------- /resources/solution.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/arc-l/optimal-mrpp-continuous/d9a85869f79b640984c7469fb8c3aaa4da14a0b2/resources/solution.png -------------------------------------------------------------------------------- /roadmap-generator-vc10/make-moc.vcxproj: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Debug 6 | Win32 7 | 8 | 9 | Release 10 | Win32 11 | 12 | 13 | 14 | {C81C3BB6-17F0-4B58-B109-8EE33FA13752} 15 | makemoc 16 | 17 | 18 | 19 | Application 20 | true 21 | MultiByte 22 | 23 | 24 | Application 25 | false 26 | true 27 | MultiByte 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | ..\..\..\Dev\Projects\temp\roadmap-generator-moc\$(Configuration)\ 41 | 42 | 43 | ..\..\..\Dev\Projects\temp\$(Configuration)\ 44 | 45 | 46 | ..\..\..\Dev\Projects\temp\roadmap-generator-moc\$(Configuration)\ 47 | 48 | 49 | ..\..\..\Dev\Projects\temp\$(Configuration)\ 50 | 51 | 52 | 53 | Level3 54 | Disabled 55 | 56 | 57 | true 58 | 59 | 60 | moc.exe $(SolutionDir)/../main_window.h -o $(SolutionDir)/../main_window_moc.cpp 61 | 62 | 63 | moc.exe $(SolutionDir)/../main_window.h -o $(SolutionDir)/../vc10/main_window_moc.cpp 64 | 65 | 66 | 67 | 68 | Level3 69 | MaxSpeed 70 | true 71 | true 72 | 73 | 74 | true 75 | true 76 | true 77 | 78 | 79 | 80 | 81 | 82 | 83 | moc.exe $(SolutionDir)/../main_window.h -o $(SolutionDir)/../vc10/main_window_moc.cpp 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | -------------------------------------------------------------------------------- /roadmap-generator-vc10/make-moc.vcxproj.filters: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | {4FC737F1-C7A5-4376-A066-2A32D752A2FF} 6 | cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx 7 | 8 | 9 | {93995380-89BD-4b04-88EB-625FBE52EBFB} 10 | h;hpp;hxx;hm;inl;inc;xsd 11 | 12 | 13 | {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} 14 | rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms 15 | 16 | 17 | -------------------------------------------------------------------------------- /roadmap-generator-vc10/make-moc.vcxproj.user: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /roadmap-generator-vc10/roadmap-generator-vc10.sln: -------------------------------------------------------------------------------- 1 | 2 | Microsoft Visual Studio Solution File, Format Version 11.00 3 | # Visual C++ Express 2010 4 | Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "roadmap-generator-vc10", "roadmap-generator-vc10.vcxproj", "{C9F8FD9F-3FE6-485F-B688-744447877886}" 5 | EndProject 6 | Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "make-moc", "make-moc.vcxproj", "{C81C3BB6-17F0-4B58-B109-8EE33FA13752}" 7 | EndProject 8 | Global 9 | GlobalSection(SolutionConfigurationPlatforms) = preSolution 10 | Debug|Win32 = Debug|Win32 11 | Release|Win32 = Release|Win32 12 | EndGlobalSection 13 | GlobalSection(ProjectConfigurationPlatforms) = postSolution 14 | {C9F8FD9F-3FE6-485F-B688-744447877886}.Debug|Win32.ActiveCfg = Debug|Win32 15 | {C9F8FD9F-3FE6-485F-B688-744447877886}.Debug|Win32.Build.0 = Debug|Win32 16 | {C9F8FD9F-3FE6-485F-B688-744447877886}.Release|Win32.ActiveCfg = Release|Win32 17 | {C9F8FD9F-3FE6-485F-B688-744447877886}.Release|Win32.Build.0 = Release|Win32 18 | {C81C3BB6-17F0-4B58-B109-8EE33FA13752}.Debug|Win32.ActiveCfg = Debug|Win32 19 | {C81C3BB6-17F0-4B58-B109-8EE33FA13752}.Debug|Win32.Build.0 = Debug|Win32 20 | {C81C3BB6-17F0-4B58-B109-8EE33FA13752}.Release|Win32.ActiveCfg = Release|Win32 21 | {C81C3BB6-17F0-4B58-B109-8EE33FA13752}.Release|Win32.Build.0 = Release|Win32 22 | EndGlobalSection 23 | GlobalSection(SolutionProperties) = preSolution 24 | HideSolutionNode = FALSE 25 | EndGlobalSection 26 | EndGlobal 27 | -------------------------------------------------------------------------------- /roadmap-generator-vc10/roadmap-generator-vc10.suo: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/arc-l/optimal-mrpp-continuous/d9a85869f79b640984c7469fb8c3aaa4da14a0b2/roadmap-generator-vc10/roadmap-generator-vc10.suo -------------------------------------------------------------------------------- /roadmap-generator-vc10/roadmap-generator-vc10.vcxproj: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Debug 6 | Win32 7 | 8 | 9 | Release 10 | Win32 11 | 12 | 13 | 14 | {C9F8FD9F-3FE6-485F-B688-744447877886} 15 | roadmapgeneratorvc10 16 | 17 | 18 | 19 | Application 20 | true 21 | MultiByte 22 | 23 | 24 | Application 25 | false 26 | true 27 | MultiByte 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | ..\..\..\Dev\Projects\temp\roadmap-generator\ 41 | C:\Dev\Qt\4.8.2\include\QtGui;C:\Dev\Qt\4.8.2\include;C:\Dev\Qt\4.8.2\include\QtCore;C:\Dev\Qt\4.8.2\include\Qt;C:\Dev\CGAL43\auxiliary\gmp\include;C:\Dev\CGAL43\include;C:\Dev\boost_1_55_0;$(VCInstallDir)include;$(VCInstallDir)atlmfc\include;$(WindowsSdkDir)include;$(FrameworkSDKDir)\include; 42 | ..\..\..\Dev\Projects\temp\roadmap-generator\$(Configuration)\ 43 | 44 | 45 | ..\..\..\Dev\Projects\temp\ 46 | C:\Dev\Qt\4.8.2\include\QtOpenGL;C:\Dev\Qt\4.8.2\include\QtGui;C:\Dev\Qt\4.8.2\include;C:\Dev\Qt\4.8.2\include\QtCore;C:\Dev\Qt\4.8.2\include\Qt;C:\Dev\CGAL43\auxiliary\gmp\include;C:\Dev\CGAL43\include;C:\Dev\boost_1_55_0;$(VCInstallDir)include;$(VCInstallDir)atlmfc\include;$(WindowsSdkDir)include;$(FrameworkSDKDir)\include; 47 | ..\..\..\Dev\Projects\temp\roadmap-generator\$(Configuration)\ 48 | false 49 | 50 | 51 | 52 | Level1 53 | Disabled 54 | MultiThreadedDebugDLL 55 | 56 | 57 | true 58 | C:\Gorubi\605\win32\lib;C:\Dev\CGAL43\lib;C:\Dev\Qt\4.8.2\lib;C:\Dev\CGAL43\auxiliary\gmp\lib;C:\Dev\CGAL43\lib\Debug;C:\Dev\boost_1_55_0\lib32-msvc-10.0;%(AdditionalLibraryDirectories) 59 | boost_system-vc100-mt-gd-1_55.lib;boost_thread-vc100-mt-gd-1_55.lib;QtOpenGLd4.lib;libgmp-10.lib;libmpfr-4.lib;QtGuid4.lib;QtCored4.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | Level1 69 | MaxSpeed 70 | true 71 | true 72 | MultiThreadedDLL 73 | AnySuitable 74 | 75 | 76 | true 77 | true 78 | true 79 | C:\Gorubi\605\win32\lib;C:\Dev\CGAL43\lib;C:\Dev\Qt\4.8.2\lib;C:\Dev\CGAL43\auxiliary\gmp\lib;C:\Dev\CGAL43\lib\Release;C:\Dev\boost_1_55_0\lib32-msvc-10.0;%(AdditionalLibraryDirectories) 80 | boost_system-vc100-mt-1_55.lib;boost_thread-vc100-mt-1_55.lib;QtOpenGL4.lib;libgmp-10.lib;libmpfr-4.lib;QtGui4.lib;QtCore4.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) 81 | 82 | 83 | moc.exe $(SolutionDir)/../main_window.h -o $(SolutionDir)/../main_window_moc.cpp 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | -------------------------------------------------------------------------------- /roadmap-generator-vc10/roadmap-generator-vc10.vcxproj.filters: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | {4FC737F1-C7A5-4376-A066-2A32D752A2FF} 6 | cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx 7 | 8 | 9 | {93995380-89BD-4b04-88EB-625FBE52EBFB} 10 | h;hpp;hxx;hm;inl;inc;xsd 11 | 12 | 13 | {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} 14 | rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms 15 | 16 | 17 | {ecaba881-1042-412b-a19e-69cda4121ebe} 18 | 19 | 20 | 21 | 22 | Source Files 23 | 24 | 25 | Source Files 26 | 27 | 28 | Source Files 29 | 30 | 31 | Source Files 32 | 33 | 34 | Source Files 35 | 36 | 37 | Shotest Path 38 | 39 | 40 | 41 | 42 | Header Files 43 | 44 | 45 | Header Files 46 | 47 | 48 | Header Files 49 | 50 | 51 | Header Files 52 | 53 | 54 | Header Files 55 | 56 | 57 | Header Files 58 | 59 | 60 | Header Files 61 | 62 | 63 | Shotest Path 64 | 65 | 66 | -------------------------------------------------------------------------------- /roadmap-generator-vc10/roadmap-generator-vc10.vcxproj.user: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | .. 5 | WindowsLocalDebugger 6 | 7 | 8 | .. 9 | WindowsLocalDebugger 10 | C:/temp 11 | 12 | -------------------------------------------------------------------------------- /roadmap.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * The core class (implementation) for roadmap building 3 | * 4 | * Created on: Jan 30, 2015 5 | * Author: Jingjin Yu 6 | */ 7 | 8 | #include "roadmap.h" 9 | #include "helper_functions.h" 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | #include 21 | #include 22 | 23 | /*------------------------------------------------------------------------------------------ 24 | see resources/lattice-indexing.pptx for the processing logic 25 | --------------------------------------------------------------------------------------------*/ 26 | 27 | static const QColor BASIC_QCOLORS8[] = {Qt::red, Qt::blue, Qt::green, Qt::magenta, Qt::cyan, Qt::gray, Qt::black, Qt::yellow}; 28 | static int colorCounter = 0; 29 | 30 | void Roadmap::buildRoadmap(Polygon2_list* pObsList, Polygon_2 *pBoundingRect, double radius){ 31 | // Populate some internal variables for use across calls 32 | m_radius = radius; 33 | m_edgeLength = radius/0.43; 34 | m_obstaclePolyList = *pObsList; 35 | m_pBoundingRect = pBoundingRect; 36 | if(m_pVisibilityGraph != 0){ 37 | delete m_pVisibilityGraph; 38 | m_pVisibilityGraph = 0; 39 | } 40 | if(m_pEnvironment != 0){ 41 | delete m_pEnvironment; 42 | m_pEnvironment = 0; 43 | } 44 | 45 | // Some basic setup 46 | ICPoint_2 bottomLeft = K_ICK_converter((*m_pBoundingRect)[0]); 47 | ICPoint_2 topRight = K_ICK_converter((*m_pBoundingRect)[2]); 48 | 49 | bottomLeftX = bottomLeft.x(); 50 | bottomLeftY = bottomLeft.y(); 51 | width = topRight.x() - bottomLeft.x(); 52 | height = topRight.y() - bottomLeft.y();; 53 | sqrt3 = sqrt(3.0); 54 | 55 | // Compute number of columns and rows 56 | n_w = (int)(ceil(width/(m_edgeLength*3/2))) + 3; 57 | n_h = (int)(ceil(height/(m_edgeLength*sqrt3))) + 3; 58 | 59 | // The lattice start x, y 60 | xs = bottomLeftX - (3/2)*m_edgeLength*1.35; 61 | ys = bottomLeftY - sqrt3*m_edgeLength*1.4; 62 | 63 | // Clean up from previous build 64 | m_pointList.clear(); 65 | m_vidPointMap.clear(); 66 | m_pointVidMap.clear(); 67 | m_graph.clear(); 68 | m_finalGraph.clear(); 69 | m_vidFGMap.clear(); 70 | m_vidGFMap.clear(); 71 | m_edgeToBeRemovedSet.clear(); 72 | m_vertexToBeRemovedSet.clear(); 73 | m_boundaryBoundingCycle.clear(); 74 | m_connectingPathMap.clear(); 75 | for(std::vector::iterator git = m_obsBoundingCycleVec.begin(); git != m_obsBoundingCycleVec.end(); git++){ 76 | delete (*git); 77 | } 78 | m_obsBoundingCycleVec.clear(); 79 | 80 | /* 81 | // Build the roadmap, first obtain a lattice that cover the outer boundary 82 | buildHexgaonLattice(); 83 | 84 | // Remove extra edges, at the same time, find smallest cycles enclosing the obstacles 85 | removeExcessEdges(); 86 | 87 | // Preserve connectivity 88 | checkAndFixConnectivity(); 89 | */ 90 | } 91 | 92 | void Roadmap::removeExcessEdges(){ 93 | // We do this in several steps. First, we go through each polygon obstacle boundary 94 | // and delete all edges of the lattice that intersect with these boundaries. Then, 95 | // we find the connected components of the remaining lattice graph. For each component 96 | // we only need to test one vertex to know whether it belongs to the configuration space 97 | // or not. We keep all components that belong to the configuration space 98 | 99 | // ===================================================================================== 100 | // Compute the set of edges that falls on obstacle boundaries, at the same time also 101 | // compute the smallest cycle in the full lattice that encloses the obstacle. First do 102 | // it for the bounding polygon 103 | getIntersectingEdges(*m_pBoundingRect, m_boundaryBoundingCycle, true); 104 | 105 | // Then for all obstacles 106 | for(Polygon2_list::iterator obsit = m_obstaclePolyList.begin(); obsit != m_obstaclePolyList.end(); obsit++){ 107 | Graph* pg = new Graph(); 108 | m_obsBoundingCycleVec.push_back(pg); 109 | getIntersectingEdges(*obsit, *pg); 110 | } 111 | 112 | // ===================================================================================== 113 | // Remove edges that do not belong to the graph 114 | Graph g; 115 | std::set > edgeSet = m_graph.getEdgeSet(); 116 | for(std::set >::iterator eit = edgeSet.begin(); eit != edgeSet.end(); eit++){ 117 | int fv = (*eit).first; 118 | int sv = (*eit).second; 119 | if(!edgeInSet(fv, sv, m_edgeToBeRemovedSet)){ 120 | g.addEdge(fv, sv); 121 | } 122 | } 123 | m_graph = g; 124 | 125 | // ===================================================================================== 126 | // Go through all vertices and find all vertices and edges inside the configuration 127 | // space. To do so, iterate over all lattices points (the full lattice minus the edges that 128 | // crosses obstacle boundaries) and for each point, if it has not been checked, see whether 129 | // the point is inside the configuration space. Then we do a BFS from the point and visit 130 | // all points/edges connected to the point. We add the edges to our final graph if and only 131 | // if the starting vertex is in the c-space. This way, we need to do c-space membership 132 | // check geometrically only very limited number of times, usually around the number of 133 | // obstacles in the c-space. 134 | 135 | g.clear(); 136 | std::set visitedVertices; 137 | for(std::map::iterator vit = m_vidPointMap.begin(); vit != m_vidPointMap.end(); vit++){ 138 | int vid = vit->first; 139 | if(visitedVertices.find(vid) == visitedVertices.end()){ 140 | visitedVertices.insert(vid); 141 | // Test whether the vertex is inside the configuration space 142 | bool inCSpace = isPointInCSpace(vit->second); 143 | 144 | // Do BFS 145 | std::list tempQueue; 146 | tempQueue.push_back(vid); 147 | while(tempQueue.size() > 0){ 148 | int current = tempQueue.front(); 149 | tempQueue.pop_front(); 150 | 151 | std::set neighborSet = m_graph.getNeighborSet(current); 152 | 153 | for(std::set::iterator vit = neighborSet.begin(); vit != neighborSet.end(); vit++){ 154 | // Retrieve first and second vertices 155 | int et = *vit; 156 | if(visitedVertices.find(et) == visitedVertices.end()){ 157 | visitedVertices.insert(et); 158 | tempQueue.push_back(et); 159 | } 160 | // Add edge as needed 161 | if(inCSpace) g.addEdge(current, et); 162 | } 163 | } 164 | } 165 | } 166 | m_graph.clear(); 167 | m_graph = g; 168 | 169 | // Remove isolated vertices 170 | for(std::set::iterator vit = m_vertexToBeRemovedSet.begin(); vit != m_vertexToBeRemovedSet.end(); vit++){ 171 | // std::cout << "Removing vertex with id: " << *vit << std::endl; 172 | m_graph.removeVertex(*vit); 173 | } 174 | 175 | // Remove single degree edges 176 | std::set vSetCopy = m_graph.getVertexSet(); 177 | for(std::set::iterator vit = vSetCopy.begin(); vit != vSetCopy.end(); vit++){ 178 | if(m_graph.hasVertex(*vit)){ 179 | if(m_graph.getNeighborSet(*vit).size() == 0){ 180 | m_graph.removeVertex(*vit); 181 | // std::cout << "Removing vertex with id: " << *vit << std::endl; 182 | } 183 | } 184 | } 185 | 186 | buildFinalGraph(); 187 | } 188 | 189 | void Roadmap::buildFinalGraph(){ 190 | // Construct m_finalGraph 191 | std::set>& eSet = m_graph.getEdgeSet(); 192 | int vIDCount = 0; 193 | for(std::set >::iterator eit = eSet.begin(); eit != eSet.end(); eit++){ 194 | int fv = eit->first; 195 | int sv = eit->second; 196 | 197 | // Check whether fv was added 198 | if(m_vidGFMap.find(fv) == m_vidGFMap.end()){ 199 | m_vidGFMap[fv] = vIDCount; 200 | m_vidFGMap[vIDCount] = fv; 201 | fv = vIDCount++; 202 | } 203 | else{ 204 | fv = m_vidGFMap[fv]; 205 | } 206 | 207 | // Check whether sv was added 208 | if(m_vidGFMap.find(sv) == m_vidGFMap.end()){ 209 | m_vidGFMap[sv] = vIDCount; 210 | m_vidFGMap[vIDCount] =sv; 211 | sv = vIDCount++; 212 | } 213 | else{ 214 | sv = m_vidGFMap[sv]; 215 | } 216 | m_finalGraph.addEdge(fv, sv); 217 | } 218 | } 219 | 220 | void Roadmap::drawBoundingCycle(QGraphicsScene& scene){ 221 | // Draw the cycles 222 | QPen regularPen = QPen(QColor(0, 255, 0, 127), 0.5, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin); 223 | QPen illegalPen = QPen(QColor(255, 0, 0, 127), 0.5, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin); 224 | for(std::vector::iterator git = m_obsBoundingCycleVec.begin(); git != m_obsBoundingCycleVec.end(); git++){ 225 | Graph& g = *(*git); 226 | std::set > edgeSet = g.getEdgeSet(); 227 | for(std::set >::iterator eit = edgeSet.begin(); eit != edgeSet.end(); eit++){ 228 | int v1 = (*eit).first; 229 | int v2 = (*eit).second; 230 | ICPoint_2 p1 = K_ICK_converter(m_vidPointMap[v1]); 231 | ICPoint_2 p2 = K_ICK_converter(m_vidPointMap[v2]); 232 | if(m_graph.hasEdge(v1, v2)){ 233 | scene.addLine(p1.x(), p1.y(), p2.x(), p2.y(), regularPen); 234 | } 235 | else{ 236 | scene.addLine(p1.x(), p1.y(), p2.x(), p2.y(), illegalPen); 237 | } 238 | } 239 | } 240 | } 241 | 242 | bool Roadmap::isPointInCSpace(Point_2 &p){ 243 | // Check whether the point is inside the bounding rect 244 | if(m_pBoundingRect->bounded_side(p) == CGAL::ON_UNBOUNDED_SIDE){ 245 | return false; 246 | } 247 | 248 | for(Polygon2_list::iterator pli = m_obstaclePolyList.begin(); pli != m_obstaclePolyList.end(); pli++){ 249 | Polygon_2 &tp = *(pli); 250 | if(tp.bounded_side(p) == CGAL::ON_BOUNDED_SIDE){ 251 | return false; 252 | } 253 | } 254 | return true; 255 | } 256 | 257 | 258 | void Roadmap::drawVertexIds(QGraphicsScene& scene){ 259 | // Draw the id text of the vretex 260 | QFont font; 261 | QPainterPath path; 262 | font.setPointSizeF(m_radius/1.5); 263 | font.setBold(false); 264 | for(std::set::iterator vit = m_graph.getVertexSet().begin(); vit != m_graph.getVertexSet().end(); vit ++){ 265 | ICPoint_2 p = K_ICK_converter(m_vidPointMap[*vit]); 266 | QGraphicsSimpleTextItem *ti = scene.addSimpleText(QString::number(m_vidGFMap[*vit]), font); 267 | ti->setPos(p.x() + m_radius/2, p.y() - m_radius/2); 268 | ti->setPen(QPen(QColor(Qt::green), 0.03*m_radius, Qt::SolidLine, Qt::RoundCap,Qt::RoundJoin)); 269 | ti->setZValue(2); 270 | } 271 | } 272 | 273 | void Roadmap::buildVisibilityGraph(){ 274 | // To build the visibility graph, we use the package VisiLibity, which is not precise arithematic, 275 | // but good for our purpose since we do not use visibility graph as part of our main logic. 276 | 277 | // Only build once per roadmap 278 | if(m_pVisibilityGraph != 0) return; 279 | 280 | // Vector to hold all polygon for contructing VisiLibity object 281 | vector polyVec; 282 | 283 | // We assume that the infated obstacles do not intersect each other but may intersect the boundary. 284 | // Therefore, we test such intersection and obtain an updated boundary (the inside) 285 | ECPolygon_2 boundary = convertToExactPolygon(*m_pBoundingRect); 286 | if(boundary.is_clockwise_oriented()) boundary.reverse_orientation(); 287 | for(Polygon2_list::iterator pli = m_obstaclePolyList.begin(); pli != m_obstaclePolyList.end(); pli++){ 288 | ECPolygon_2 ecPoly = convertToExactPolygon(*pli); 289 | if(boundaryInterset(boundary, ecPoly)){ 290 | vector outVec; 291 | 292 | // Remove the obstacle "from" the boundary polygon 293 | CGAL::difference(boundary, ecPoly, back_inserter(outVec)); 294 | 295 | // Update boundary 296 | boundary = outVec[0].outer_boundary(); 297 | } 298 | else{ 299 | // Add all non intersecting obstacles 300 | Polygon tempPoly; 301 | converFromCGALtoVisilibity(ecPoly, tempPoly, false); 302 | polyVec.push_back(tempPoly); 303 | } 304 | } 305 | 306 | // Insert the boundary 307 | Polygon boundaryPoly; 308 | converFromCGALtoVisilibity(boundary, boundaryPoly, true); 309 | polyVec.insert(polyVec.begin(), boundaryPoly); 310 | 311 | // Build environment and visibility graph 312 | m_pEnvironment = new Environment(polyVec); 313 | m_pVisibilityGraph = new Visibility_Graph(*m_pEnvironment, m_epsilon); 314 | } 315 | 316 | double Roadmap::computeShortestPath(Point_2& p1, Point_2& p2, std::list & path){ 317 | ICPoint_2 p1x = K_ICK_converter(p1); 318 | ICPoint_2 p2x = K_ICK_converter(p2); 319 | return computeShortestPath(p1x.x(), p1x.y(), p2x.x(), p2x.y(), path); 320 | } 321 | 322 | double Roadmap::computeShortestPath(double x1, double y1, double x2, double y2, std::list & path){ 323 | Polyline pl = m_pEnvironment->shortest_path(Point(x1, y1), Point(x2, y2), *m_pVisibilityGraph, m_epsilon); 324 | for(int i = 0; i < pl.size(); i++){ 325 | path.push_back(Point_2(pl[i].x(), pl[i].y())); 326 | } 327 | return getPathLength(path); 328 | } 329 | 330 | double Roadmap::computeShortestPath(double x1, double y1, double x2, double y2, vector >& path){ 331 | // Compute shortest path 332 | std::list shortestPath; 333 | double length = computeShortestPath(x1, y1, x2, y2, shortestPath); 334 | 335 | // Convert the path 336 | for(std::list::iterator vi = shortestPath.begin(); vi != shortestPath.end(); vi++){ 337 | ICPoint_2 p = K_ICK_converter(*vi); 338 | path.push_back(pair(p.x(), p.y())); 339 | } 340 | return length; 341 | } 342 | 343 | void Roadmap::addToScene(QGraphicsScene& scene, bool drawEdge, QPen edgePen, bool drawVertex, QPen vertexPen){ 344 | // Paint the edges 345 | if(drawEdge){ 346 | std::set > edgeSet = m_graph.getEdgeSet(); 347 | for(std::set >::iterator eit = edgeSet.begin(); eit != edgeSet.end(); eit++){ 348 | int v1 = (*eit).first; 349 | int v2 = (*eit).second; 350 | ICPoint_2 p1 = K_ICK_converter(m_vidPointMap[v1]); 351 | ICPoint_2 p2 = K_ICK_converter(m_vidPointMap[v2]); 352 | scene.addLine(p1.x(), p1.y(), p2.x(), p2.y(), edgePen); 353 | /*}*/ 354 | 355 | if(m_boundaryBoundingCycle.hasEdge(v1, v2)){ 356 | scene.addLine(p1.x(), p1.y(), p2.x(), p2.y(), QPen(Qt::blue, 0.05, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); 357 | } 358 | else{ 359 | for(std::vector::iterator git = m_obsBoundingCycleVec.begin(); git != m_obsBoundingCycleVec.end(); git++){ 360 | if((*git)->hasEdge(v1, v2)){ 361 | scene.addLine(p1.x(), p1.y(), p2.x(), p2.y(), QPen(Qt::yellow, 0.05, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); 362 | } 363 | } 364 | } 365 | } 366 | } 367 | 368 | // Paint vertices if needed and obtain the fill area 369 | if(!drawVertex){ 370 | for(std::list::iterator it = m_pointList.begin(); it != m_pointList.end(); it ++){ 371 | ICPoint_2 p = K_ICK_converter(*it); 372 | scene.addEllipse(p.x() - 0.025, p.y() - 0.025, 0.05, 0.05, vertexPen); 373 | } 374 | } 375 | 376 | for(Path_Map::iterator pathIt = m_connectingPathMap.begin(); pathIt != m_connectingPathMap.end(); pathIt ++){ 377 | std::list& shortestPath = pathIt->second; 378 | std::list::iterator vit = shortestPath.begin(); 379 | if(vit != shortestPath.end()){ 380 | ICPoint_2 p = K_ICK_converter(*vit); 381 | vit++; 382 | for(; vit != shortestPath.end(); vit++){ 383 | ICPoint_2 p2 = K_ICK_converter(*vit); 384 | scene.addLine(p.x(), p.y(), p2.x(), p2.y(), QPen(Qt::magenta, 0.025, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); 385 | p = p2; 386 | } 387 | } 388 | } 389 | 390 | // std::cout << shortestPath.size() << std::endl; 391 | 392 | } 393 | 394 | void Roadmap::getIntersectingEdges(Polygon_2 & poly, Graph& boundingCycle, bool outerBoundary){ 395 | // Detecting all edges of the lattice graph that intersects the boundary of poly 396 | 397 | // ===================================================================================== 398 | // Get a vertex of the poly and find the hexgaon of the lattice that contains the vetex. 399 | // For each col and row combo, there are three possible hexgaons the point may fall into 400 | 401 | // Locate the hexgon (relative to some arbitrary base choice) 402 | ICPoint_2 p0 = K_ICK_converter(poly.vertex(0)); 403 | std::pair crp = locateHexagonForPoint(p0); 404 | int col = crp.first; 405 | int row = crp.second; 406 | 407 | // Compute the current hexagon 408 | Point_2 p[6]; 409 | Segment_2 e[6]; 410 | Polygon_2 hex; 411 | int pIndex[6]; 412 | populateHexagon(col, row, p, e, hex, pIndex); 413 | 414 | // ===================================================================================== 415 | // With a point of the obstacle and a hexgon containing the point, we iteratively check 416 | // intersections between the segments of the obstacle and the hexagon. We assume that 417 | // the obstacle would have at least three vertices. Note that a obstacle may be smaller than 418 | // a single hexagon. 419 | 420 | std::set > tempEdgeToRemoveSet; 421 | int numVertices = poly.size(); // 422 | int currentVertexIndex = 0; 423 | while(currentVertexIndex < numVertices){ 424 | // Get the current segment of polygon to be checked 425 | Point_2 nextVertex = poly[currentVertexIndex==numVertices-1?0:currentVertexIndex+1]; 426 | 427 | // Check whether nextVertex is outside of the current hexagon. If we are still in the 428 | // same hexagon, then move to the next obstacle vertex 429 | if(hex.bounded_side(nextVertex) == CGAL::ON_BOUNDED_SIDE) { 430 | currentVertexIndex ++; 431 | continue; 432 | } 433 | 434 | // If we are here, then we jumped outside of a lattice hexagon. Figure out which edge is 435 | // being intersected. We keep doing this until we cover the entire obstacle edge 436 | Point_2 currentVertex = poly[currentVertexIndex]; 437 | Segment_2 obsEdge(currentVertex, nextVertex); 438 | 439 | std::pair lastPair(-10, -10); 440 | while(true){ 441 | int edgeIndex = 0; 442 | while(edgeIndex < 6){ 443 | if(intersection(e[edgeIndex], obsEdge)){ 444 | std::pair edgePair(pIndex[edgeIndex], pIndex[edgeIndex==5?0:edgeIndex+1]); 445 | std::pair edgePairReverse(pIndex[edgeIndex==5?0:edgeIndex+1], pIndex[edgeIndex]); 446 | if(lastPair != edgePair && lastPair != edgePairReverse){ 447 | lastPair = edgePair; 448 | break; 449 | } 450 | } 451 | edgeIndex ++; 452 | }; 453 | 454 | if(edgeIndex < 6){ 455 | // We hit an intersection, mark the edge as to be removed 456 | tempEdgeToRemoveSet.insert(lastPair); 457 | m_edgeToBeRemovedSet.insert(lastPair); 458 | 459 | // Figure out the next hexgon to be checked 460 | if(col%2 == 0){ 461 | switch(edgeIndex){ 462 | case 0: col--; break; 463 | case 1: row++; break; 464 | case 2: col++; break; 465 | case 3: col++; row--; break; 466 | case 4: row--; break; 467 | case 5: col--; row--; break; 468 | default: break; 469 | } 470 | } 471 | else{ 472 | switch(edgeIndex){ 473 | case 0: col--; row++; break; 474 | case 1: row++; break; 475 | case 2: col++; row++; break; 476 | case 3: col++; break; 477 | case 4: row--; break; 478 | case 5: col--; break; 479 | default: break; 480 | } 481 | } 482 | populateHexagon(col, row, p, e, hex, pIndex); 483 | } 484 | else{ 485 | // No more intersections, we are dong with the current obstacle edge 486 | break; 487 | } 488 | } 489 | currentVertexIndex++; 490 | } 491 | 492 | // ===================================================================================== 493 | // We now have all the edges of the lattice that lie on the polygonal obstacle boundary. 494 | // Next, we locate the bounding cycle in the full lattice graph surrounding the obstacle 495 | if(tempEdgeToRemoveSet.size() > 0){ 496 | // Iterate through edges to be removed and find one with an end vertex that 497 | // has an associated edge in the configuraiton space 498 | int sIndex, t1Index, t2Index; 499 | for(std::set >::iterator eit = tempEdgeToRemoveSet.begin(); 500 | eit != tempEdgeToRemoveSet.end(); eit++){ 501 | std::pair edge = *eit; 502 | // Try one vertex 503 | sIndex = edge.first; 504 | if((t2Index = pointBelongToEdgeOutideObstacle(poly, sIndex, tempEdgeToRemoveSet, outerBoundary))!= -1) 505 | { 506 | t1Index = edge.second; 507 | break; 508 | } 509 | 510 | // Try the other vertex 511 | sIndex = edge.second; 512 | if((t2Index = pointBelongToEdgeOutideObstacle(poly, sIndex, tempEdgeToRemoveSet, outerBoundary))!= -1) 513 | { 514 | t1Index = edge.first; 515 | break; 516 | } 517 | 518 | } 519 | 520 | // Now edge (sIndex, t1Index) crosses the boundary, and (sIndex, t2Index) it outside the boundary 521 | // We simply locate a hexagon containing these two edges to extend the cycle 522 | Point_2 p1 = m_vidPointMap[sIndex]; 523 | Point_2 p2 = m_vidPointMap[t1Index]; 524 | Point_2 p3 = m_vidPointMap[t2Index]; 525 | ICPoint_2 midPoint = K_ICK_converter(Point_2((p2.x() + p3.x())/2, (p2.y() + p3.y())/2)); 526 | crp = locateHexagonForPoint(midPoint); 527 | populateHexagon(crp.first, crp.second, p, e, hex, pIndex); 528 | 529 | int endIndex = sIndex; // When we see this index again, we are done 530 | 531 | // Add valid edge to cycle 532 | addEdgeIfNotThere(sIndex, t2Index, boundingCycle); 533 | 534 | // With the first hexagon, we can keep going along the boundary 535 | while(true){ 536 | // Get next candidate index 537 | int nextIndex = getNextNodeInSequence(sIndex, t2Index, pIndex); 538 | 539 | p1 = m_vidPointMap[sIndex]; 540 | p2 = m_vidPointMap[nextIndex]; 541 | p3 = m_vidPointMap[t2Index]; 542 | 543 | // Check whether the edge (t2Index, nextIndex) crosses boundary 544 | if(edgeInSet(t2Index, nextIndex, tempEdgeToRemoveSet)){ 545 | // In this case, we move to the next hexagon 546 | crp = getBorderHexagon(crp.first, crp.second, nextIndex, t2Index, pIndex); 547 | populateHexagon(crp.first, crp.second, p, e, hex, pIndex); 548 | sIndex = nextIndex; 549 | // t2Index = sIndex; 550 | } 551 | else{ 552 | // Edge is valid, update indices 553 | sIndex = t2Index; 554 | t2Index = nextIndex; 555 | 556 | // Add valid edge to cycle 557 | addEdgeIfNotThere(sIndex, t2Index, boundingCycle); 558 | } 559 | 560 | if(endIndex == t2Index) break; 561 | } 562 | } 563 | 564 | // ===================================================================================== 565 | // Process bounding cycle to remove single degree vertices 566 | std::set vSet = boundingCycle.getVertexSet(); 567 | std::set vSingleSet; 568 | // Collect single degree vertices 569 | for(std::set::iterator vit = vSet.begin(); vit != vSet.end(); vit++){ 570 | if(boundingCycle.getNeighborSet(*vit).size() == 1){ 571 | vSingleSet.insert(*vit); 572 | // Need to check the neighbor as well 573 | int prevNbr = *vit; 574 | int nbr = *(boundingCycle.getNeighborSet(*vit).begin()); 575 | while(boundingCycle.getNeighborSet(nbr).size()==2){ 576 | vSingleSet.insert(nbr); 577 | std::set nbrSet = boundingCycle.getNeighborSet(nbr); 578 | nbrSet.erase(prevNbr); 579 | 580 | prevNbr = nbr; 581 | nbr = *(nbrSet.begin()); 582 | } 583 | } 584 | } 585 | // Delete them 586 | for(std::set::iterator vit = vSingleSet.begin(); vit != vSingleSet.end(); vit++){ 587 | boundingCycle.removeVertex(*vit); 588 | 589 | // It seems good to remove these single degree vertices 590 | if(!outerBoundary)m_vertexToBeRemovedSet.insert(*vit); 591 | } 592 | } 593 | 594 | std::pair Roadmap::getBorderHexagon(int col, int row, int i1, int i2, int pIndex[]){ 595 | // First locate the index of i1 596 | int i1Index = 0; 597 | for(int i = 0; i < 6; i ++){ 598 | if(pIndex[i] == i1){ 599 | i1Index = i; 600 | break; 601 | } 602 | } 603 | 604 | // Figure out the edge index 605 | int edgeIndex = 0; 606 | int nextIndex = (i1Index == 5? 0 : i1Index + 1); 607 | if(pIndex[nextIndex] == i2){ 608 | // Going clockwise 609 | edgeIndex = i1Index; 610 | } 611 | else{ 612 | // Going counterclockwise 613 | edgeIndex = (i1Index + 6 - 1)%6; 614 | } 615 | 616 | // Locate the next hexagon 617 | if(col%2 == 0){ 618 | switch(edgeIndex){ 619 | case 0: col--; break; 620 | case 1: row++; break; 621 | case 2: col++; break; 622 | case 3: col++; row--; break; 623 | case 4: row--; break; 624 | case 5: col--; row--; break; 625 | default: break; 626 | } 627 | } 628 | else{ 629 | switch(edgeIndex){ 630 | case 0: col--; row++; break; 631 | case 1: row++; break; 632 | case 2: col++; row++; break; 633 | case 3: col++; break; 634 | case 4: row--; break; 635 | case 5: col--; break; 636 | default: break; 637 | } 638 | } 639 | return std::pair(col, row); 640 | } 641 | 642 | bool Roadmap::edgeInSet(int v1, int v2, std::set >& edgeSet){ 643 | std::pair edge(v1, v2); 644 | std::pair rEdge(v2, v1); 645 | return (edgeSet.find(edge) != edgeSet.end() || edgeSet.find(rEdge) != edgeSet.end()); 646 | } 647 | 648 | void Roadmap::addEdgeIfNotThere(int v1, int v2, Graph & graph){ 649 | if(!graph.hasEdge(v1, v2)){ 650 | graph.addEdge(v1, v2); 651 | } 652 | } 653 | 654 | int Roadmap::getNextNodeInSequence(int i1, int i2, int pIndex[]){ 655 | // First locate the index of i1 656 | int i1Index = 0; 657 | for(int i = 0; i < 6; i ++){ 658 | if(pIndex[i] == i1){ 659 | i1Index = i; 660 | break; 661 | } 662 | } 663 | int nextIndex = (i1Index == 5? 0 : i1Index + 1); 664 | if(pIndex[nextIndex] == i2){ 665 | // Going clockwise 666 | return pIndex[(i1Index + 2)%6]; 667 | } 668 | else{ 669 | // Going counterclockwise 670 | return pIndex[(6 + i1Index - 2)%6]; 671 | } 672 | } 673 | 674 | std::pair Roadmap::locateHexagonForPoint(ICPoint_2 &p0){ 675 | // Compute the rectangular (col, row) 676 | int col = (int)(floor((p0.x() - xs)/(m_edgeLength*1.5))); 677 | int row = (int)(floor((p0.y() - ys)/(m_edgeLength*sqrt3))); 678 | 679 | // Shift everything to the "origin" 680 | double dx = p0.x() - (xs + m_edgeLength*1.5*col); 681 | double dy = p0.y() - (ys + m_edgeLength*sqrt3*row); 682 | 683 | // For each col/row combo, which correspond to a rectangular region, there can be 684 | // three hexagons corrsponding to a point in that rectangle 685 | if(col%2 == 0){ 686 | if(dx <= 0.5*m_edgeLength && dy <= sqrt3*m_edgeLength - sqrt3*dx && dy >= sqrt3*dx){ 687 | // We need to move left 688 | col --; 689 | // row ++; 690 | } 691 | else if(dy >= m_edgeLength*0.5*sqrt3){ 692 | // Off by one row 693 | row ++; 694 | } 695 | else{ 696 | // Already in the right place, do nothing 697 | } 698 | } 699 | else{ 700 | if(dy >= sqrt3*0.5*m_edgeLength + sqrt3*dx){ 701 | col--; 702 | row++; 703 | } 704 | else if(dy <= sqrt3*0.5*m_edgeLength - sqrt3*dx){ 705 | col--; 706 | } 707 | else{ 708 | // Already in the right place, do nothing 709 | } 710 | } 711 | return std::pair(col, row); 712 | } 713 | 714 | int Roadmap::pointBelongToEdgeOutideObstacle(Polygon_2 & poly, int pIndex, 715 | std::set > &tempEdgeToRemoveSet, bool outerBoundary){ 716 | // Grab the point and check whether it is in the c-space 717 | if(pointOutsideObstacle(poly, pIndex, outerBoundary)){ 718 | // The point is not "in" the obstacle, get edges and check that they are not 719 | // intersecting with the boundary. Because pIndex is outside obstacle, as long 720 | // as one edge from pIndex is not intersecting boundary, the edge must be outside 721 | // the boundary 722 | std::set& nbrSet = m_graph.getNeighborSet(pIndex); 723 | for(std::set::iterator nit = nbrSet.begin(); nit != nbrSet.end(); nit++){ 724 | int et = *nit; 725 | if(!edgeInSet(pIndex, et, tempEdgeToRemoveSet)){ 726 | return et; 727 | } 728 | } 729 | } 730 | return -1; 731 | } 732 | 733 | bool Roadmap::pointOutsideObstacle(Polygon_2 & poly, int pIndex, bool outerBoundary){ 734 | Point_2 p = m_vidPointMap[pIndex]; 735 | int boundedStatus = poly.bounded_side(p); 736 | if((boundedStatus == CGAL::ON_BOUNDED_SIDE && outerBoundary) || 737 | (boundedStatus == CGAL::ON_UNBOUNDED_SIDE && !outerBoundary)){ 738 | return true; 739 | } 740 | return false; 741 | } 742 | 743 | void Roadmap::populateHexagon(int col, int row, Point_2* p, Segment_2* e, Polygon_2 &hex, int* pIndex){ 744 | hex.clear(); 745 | if(col%2 == 0){ 746 | pIndex[0] = (col + row*n_w)*2; 747 | pIndex[1] = (col + row*n_w)*2 + 1; 748 | pIndex[2] = (col + 1 + row*n_w)*2 + 1; 749 | pIndex[3] = (col + 1 + row*n_w)*2; 750 | pIndex[4] = (col + 1 + (row-1)*n_w)*2 + 1; 751 | pIndex[5] = (col + (row-1)*n_w)*2 + 1; 752 | } 753 | else{ 754 | pIndex[0] = (col + row*n_w)*2 + 1; 755 | pIndex[1] = (col + (row+1)*n_w)*2; 756 | pIndex[2] = (col + 1 + (row+1)*n_w)*2; 757 | pIndex[3] = (col + 1 + row*n_w)*2 + 1; 758 | pIndex[4] = (col + 1 + row*n_w)*2; 759 | pIndex[5] = (col + row*n_w)*2; 760 | } 761 | // Then we get all vertices 762 | for(int i = 0; i < 6; i ++){ 763 | p[i] = m_vidPointMap[pIndex[i]]; 764 | } 765 | 766 | // Then the edges and the hexgaon 767 | for(int i = 0; i < 6; i ++){ 768 | e[i] = Segment_2(p[i], p[i == 5? 0 : i+1]); 769 | hex.push_back(p[i]); 770 | } 771 | } 772 | 773 | void Roadmap::buildHexgaonLattice(){ 774 | 775 | // Start the lattice at bottomLeftX, bottomLeftY. 776 | // Assume that there are n_w and n_h hexgons inside the rectangle, then we should have 777 | // (1/2)*sideLength + n_w*sideLength*3/2 <= width and width < (1/2)*sideLength + (n_w + 1)*sideLength*3/2 778 | // (sqrt(3)/2)*sideLength + n_h*(sqrt(3)/2)*sideLength <= height and height < (sqrt(3)/2)*sideLength + (n_h + 1)*(sqrt(3)/2)*sideLength 779 | // From these we can compute n_w and n_h. We add a few to make sure that we cover everything. 780 | 781 | // Compute lattice nodes 782 | for(int i = 0; i < n_w; i ++){ 783 | for(int j = 0; j < n_h; j ++){ 784 | // Build one vertical "wave" of vertices 785 | Point_2 v0(xs + i*m_edgeLength*1.5 + (i%2 == 0? 0 : m_edgeLength*0.5), 786 | ys + j*m_edgeLength*sqrt3); 787 | Point_2 v1(xs + m_edgeLength/2 + i*m_edgeLength*1.5 + (i%2 == 0? 0 : - m_edgeLength*0.5), 788 | ys + sqrt3*m_edgeLength/2 + j*m_edgeLength*sqrt3); 789 | 790 | m_vidPointMap[(i + j*n_w)*2] = v0; 791 | m_pointVidMap[v0] = (i + j*n_w)*2; 792 | m_vidPointMap[(i + j*n_w)*2 + 1] = v1; 793 | m_pointVidMap[v1] = (i + j*n_w)*2 + 1; 794 | 795 | m_pointList.push_back(v0); 796 | m_pointList.push_back(v1); 797 | } 798 | } 799 | 800 | // Build adjacency, for each vertex, check whether its three neighbors are present 801 | for(int id = 0; id < n_w*n_h*2; id ++){ 802 | // Compute w, h 803 | int w = (id/2)%n_w; 804 | int h = (id/2)/n_w; 805 | bool odd = (id%2==1); 806 | 807 | // If odd is true, check the vertex above, which has index (w, h + 1, 0) 808 | if(odd){ 809 | if(h + 1 < n_h){ 810 | m_graph.addEdge(id, (w + (h + 1)*n_w)*2); 811 | } 812 | } 813 | else{ 814 | m_graph.addEdge(id, (w + (h)*n_w)*2 + 1); 815 | } 816 | // If odd is true and w is even, check (w + 1, h, 1) 817 | if(odd && w%2 == 0){ 818 | if(w + 1 < n_w){ 819 | m_graph.addEdge(id, (w + 1 + h*n_w)*2 + 1); 820 | } 821 | } 822 | 823 | // If odd is false and w is odd, check (w + 1, h, 0) 824 | if(odd == false && w%2 == 1){ 825 | if(w + 1 < n_w){ 826 | m_graph.addEdge(id, (w + 1 + h*n_w)*2); 827 | } 828 | } 829 | } 830 | } 831 | 832 | void Roadmap::drawHexagonLattice(QGraphicsScene& scene, bool drawVetex){ 833 | QPen edgePen = QPen(Qt::gray, 0.25, Qt::DashLine, Qt::RoundCap, Qt::RoundJoin); 834 | QPen vertexPen = QPen(Qt::blue, 0.4, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin); 835 | std::set > edgeSet = m_graph.getEdgeSet(); 836 | std::set vSet; 837 | for(std::set >::iterator eit = edgeSet.begin(); eit != edgeSet.end(); eit++){ 838 | int v1 = (*eit).first; 839 | int v2 = (*eit).second; 840 | vSet.insert(v1); vSet.insert(v2); 841 | ICPoint_2 p1 = K_ICK_converter(m_vidPointMap[v1]); 842 | ICPoint_2 p2 = K_ICK_converter(m_vidPointMap[v2]); 843 | scene.addLine(p1.x(), p1.y(), p2.x(), p2.y(), edgePen); 844 | } 845 | 846 | // Paint vertices if needed and obtain the fill area 847 | if(drawVetex){ 848 | for(std::set::iterator vit = vSet.begin(); vit != vSet.end(); vit ++){ 849 | ICPoint_2 p = K_ICK_converter(m_vidPointMap[*vit]); 850 | scene.addEllipse(p.x() - 0.25, p.y() - 0.25, 0.5, 0.5, vertexPen); 851 | } 852 | } 853 | } 854 | 855 | void Roadmap::snapToGraph(vector >&coords, vector&snapped){ 856 | set usedVertices; 857 | // For each member in coords, locate the hexagon it belongs and then locate the closest 858 | // graph vertex to the said coordinate. 859 | for(int r = 0; r < coords.size(); r ++){ 860 | double x = coords[r].first; 861 | double y = coords[r].second; 862 | Point_2 p0 = Point_2(x, y); 863 | 864 | ICPoint_2 p0x = K_ICK_converter(p0); 865 | std::pair crp = locateHexagonForPoint(p0x); 866 | int col = crp.first; 867 | int row = crp.second; 868 | 869 | // Compute the current hexagon 870 | Point_2 p[6]; 871 | Segment_2 e[6]; 872 | Polygon_2 hex; 873 | int pIndex[6]; 874 | populateHexagon(col, row, p, e, hex, pIndex); 875 | 876 | // Find the closest vertex that is not occupied and inside the configuration space 877 | int bestV = -1; 878 | double minDist = -1; 879 | for(int vi = 0; vi < 6; vi ++){ 880 | // Only work with vertices inside the configuration space and not used 881 | if(m_graph.hasVertex(pIndex[vi]) 882 | && usedVertices.find(pIndex[vi]) == usedVertices.end()){ 883 | if(minDist < 0){ 884 | minDist = getDistance(p0, p[vi]); 885 | bestV = pIndex[vi]; 886 | } 887 | else{ 888 | double dist = getDistance(p0, p[vi]); 889 | if(minDist > dist){ 890 | minDist = dist; 891 | bestV = pIndex[vi]; 892 | } 893 | } 894 | } 895 | } 896 | 897 | // Snap! 898 | if(bestV != -1){ 899 | #ifdef _DEBUG 900 | cout << "Snapping point (" << x << ", " << y << ") to vertex " << m_vidGFMap[bestV] << endl; 901 | #endif 902 | snapped.push_back(m_vidGFMap[bestV]); 903 | usedVertices.insert(bestV); 904 | } 905 | else{ 906 | throw "ERROR: Cannot find suitable vertex for snapping"; 907 | } 908 | } 909 | 910 | } 911 | 912 | void Roadmap::createRandomStartGoalPairs(int numRobots, double spacing, vector >& starts, 913 | vector >& goals){ 914 | 915 | createRandomCoords(numRobots, spacing, starts); 916 | createRandomCoords(numRobots, spacing, goals); 917 | } 918 | 919 | void Roadmap::createRandomCoords(int numRobots, double spacing, vector >& coords){ 920 | // Uniform sample from the free space and pick 5 start/goal pairs 921 | CGAL::Bbox_2 bbox = m_pBoundingRect->bbox(); 922 | while(true){ 923 | long trialCount = 0; 924 | for(int i = 0; i < numRobots; i ++){ 925 | while(trialCount < numRobots*30000){ 926 | trialCount ++; 927 | #ifdef _DEBUG 928 | if(trialCount %1000 == 0) {cout << trialCount << endl;} 929 | #endif 930 | double x = (rand()%10000)/10000.*(bbox.xmax() - bbox.xmin()) + bbox.xmin(); 931 | double y = (rand()%10000)/10000.*(bbox.ymax() - bbox.ymin()) + bbox.ymin(); 932 | #ifdef _DEBUG 933 | cout << "Randomly created point with x=" << x <<", y=" << y; 934 | #endif 935 | Point_2 p(x, y); 936 | // Check that the point is in free configuration space 937 | if(isPointInCSpace(p)){ 938 | bool good = true; 939 | // Check that the point has good distance from existing points 940 | for(int vi = 0; vi < coords.size(); vi ++){ 941 | if(getDistance(p, coords[vi].first, coords[vi].second) < (spacing > 2 ? spacing : 2)*m_radius){ 942 | good = false; 943 | } 944 | } 945 | 946 | if(good){ 947 | // No problem! We have a good set 948 | coords.push_back(pair(x, y)); 949 | #ifdef _DEBUG 950 | cout << " - added to set for robot " << i << endl; 951 | #endif 952 | break; 953 | } 954 | else{ 955 | #ifdef _DEBUG 956 | cout << endl; 957 | #endif 958 | } 959 | 960 | } 961 | else{ 962 | #ifdef _DEBUG 963 | cout << endl; 964 | #endif 965 | } 966 | } 967 | if(trialCount >= numRobots*30000){ 968 | // cout << coords.size() << endl; 969 | coords.clear(); 970 | break; 971 | } 972 | } 973 | if(coords.size() == numRobots)return; 974 | } 975 | } 976 | 977 | 978 | bool Roadmap::solveProblem(vector >& sgVec, map >& paths, 979 | string& fileFolder, string& fileNameExtra){ 980 | // Solve the problem by calling external java solver 981 | 982 | // ===================================================================================== 983 | // First write the problem 984 | string pfString = fileFolder + "\\p" + fileNameExtra + ".txt"; 985 | string sfString = fileFolder + "\\s" + fileNameExtra + ".txt"; 986 | ofstream ps(pfString.c_str(), ios::out); 987 | 988 | // Write the the number of robots 989 | ps << m_finalGraph.getVertexSet().size() << endl; 990 | 991 | // Write out all the edges 992 | set >& edgeSet = m_finalGraph.getEdgeSet(); 993 | for(set >::iterator ei = edgeSet.begin(); ei != edgeSet.end(); ei ++){ 994 | ps << ei->first << ":" << ei->second << " "; 995 | } 996 | ps << endl; 997 | 998 | // Write out the starts and then the goals 999 | for(int r = 0; r < sgVec.size(); r ++){ 1000 | ps << sgVec[r].first << " "; 1001 | } 1002 | ps << endl; 1003 | for(int r = 0; r < sgVec.size(); r ++){ 1004 | ps << sgVec[r].second << " "; 1005 | } 1006 | ps << endl; 1007 | ps.close(); 1008 | 1009 | // ===================================================================================== 1010 | // Make system call 1011 | string callStr = "java -cp gurobi.jar;mp.jar projects.multipath.general.algorithms.Main"; 1012 | if(fileNameExtra.length() > 0) { 1013 | callStr.append(" ").append(pfString).append(" ").append(sfString); 1014 | } 1015 | int i = system(callStr.c_str()); 1016 | 1017 | // ===================================================================================== 1018 | // Read in the solution, there should be sgVec.size() robots 1019 | ifstream ss(sfString.c_str(), ios::in); 1020 | if(ss.good()){ 1021 | for(int r = 0; r < sgVec.size(); r++){ 1022 | string line; 1023 | getline(ss, line); 1024 | QString s(line.c_str()); 1025 | QStringList qsl = s.split(" "); 1026 | paths[r] = vector(); 1027 | for(int t = 0; t < qsl.size(); t ++){ 1028 | paths[r].push_back(qsl[t].toInt()); 1029 | } 1030 | paths[r].pop_back(); 1031 | } 1032 | ss.close(); 1033 | } 1034 | else{ 1035 | ss.close(); 1036 | return false; 1037 | } 1038 | 1039 | // Solve the problem locally 1040 | // ILPSolver solver(&m_finalGraph); 1041 | // solver.solve(sgVec, paths, -1); 1042 | 1043 | 1044 | improvePaths(paths); 1045 | return true; 1046 | } 1047 | 1048 | void Roadmap::improvePaths(map >& paths){ 1049 | // Remove obvious ossilation from the paths 1050 | for(int t = 1; t < paths[0].size() - 1; t ++){ 1051 | for(int r = 0; r < paths.size(); r ++){ 1052 | // Do we have single step ossilation? 1053 | if(paths[r][t - 1] == paths[r][t + 1] && paths[r][t - 1] != paths[r][t]){ 1054 | // Check whether any other robots goes to paths[r][t - 1] at t 1055 | bool conflict = false; 1056 | for(int ori = 0; ori < paths.size(); ori ++){ 1057 | if(ori == r) continue; 1058 | if(paths[ori][t] == paths[r][t - 1]){ 1059 | conflict = true; 1060 | } 1061 | } 1062 | 1063 | // If not, let the robot stay 1064 | if(!conflict){ 1065 | paths[r][t] = paths[r][t - 1]; 1066 | } 1067 | } 1068 | // Do a two step look ahead as well 1069 | else if((t < paths[0].size() - 2) && paths[r][t - 1] == paths[r][t + 2] && 1070 | paths[r][t] == paths[r][t + 1] && paths[r][t - 1] != paths[r][t]){ 1071 | // Check whether any other robots goes to paths[r][t - 1] at t 1072 | bool conflict = false; 1073 | for(int ori = 0; ori < paths.size(); ori ++){ 1074 | if(ori == r) continue; 1075 | if(paths[ori][t] == paths[r][t - 1] || paths[ori][t + 1] == paths[r][t - 1]){ 1076 | conflict = true; 1077 | } 1078 | } 1079 | 1080 | // If not, let the robot stay 1081 | if(!conflict){ 1082 | paths[r][t+1] = paths[r][t] = paths[r][t - 1]; 1083 | } 1084 | } 1085 | } 1086 | } 1087 | } 1088 | 1089 | pair Roadmap::getVertexLocationFromID(int vid){ 1090 | // Get vid in the original graph 1091 | vid = m_vidFGMap[vid]; 1092 | 1093 | // Locate the vid 1094 | Point_2& p = m_vidPointMap[vid]; 1095 | ICPoint_2 pp = K_ICK_converter(p); 1096 | 1097 | return pair(pp.x(), pp.y()); 1098 | } -------------------------------------------------------------------------------- /roadmap.h: -------------------------------------------------------------------------------- 1 | /* 2 | * The core class (header) for roadmap building. Roadmap building is done in four steps: 3 | * 1. Overlay a regular lattice (currently only hexagonal lattice) 4 | * 2. Remove lattice eges that intersect with configuration space obstacles, and compute the 5 | * smallest cycle on the remaining lattice that encloses the C-space obstacles. 6 | * 3. When a cycle computed about does not lie completely in the free C-space, compute 7 | * shortest paths in C-space that complete the cycle 8 | * 4. Post-processing the computed shortest paths for inclusion into the lattice graph 9 | * 10 | * Created on: Jan 30, 2015 11 | * Author: Jingjin Yu 12 | */ 13 | 14 | #ifndef _O_ROADMAP_H_ 15 | #define _O_ROADMAP_H_ 16 | 17 | #include "types.h" 18 | #include "graph.h" 19 | #include "shortest_path/visilibity.hpp" 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | using namespace std; 28 | using namespace VisiLibity; 29 | 30 | class Roadmap{ 31 | private: 32 | double m_edgeLength; // Edge length of the hexgaon 33 | Polygon_2* m_pBoundingRect; // Bounding polygon 34 | Polygon2_list m_obstaclePolyList; // Obstacle polygon list pointer 35 | 36 | std::list m_pointList; // List of all vertices 37 | std::map m_vidPointMap; // Vertex id to point map 38 | std::map m_pointVidMap; // Point to vertex id map 39 | 40 | Graph m_graph; // The lattice graph 41 | Graph m_finalGraph; // The final graph for doing planning 42 | std::map m_vidFGMap; // Vertex id MAP from final graph to graph 43 | std::map m_vidGFMap; // Vertex id MAP from graph to final graph 44 | 45 | std::set > m_edgeToBeRemovedSet; // Edges to be removed 46 | std::set m_vertexToBeRemovedSet; // Vertices to be removed (single degree) 47 | 48 | Graph m_boundaryBoundingCycle;// Cycle just inside the bounding rect 49 | std::vector m_obsBoundingCycleVec; // List of cycles around obstacle 50 | 51 | typedef std::map, std::list > Path_Map; 52 | Path_Map m_connectingPathMap; // Look up map for "bridging" paths 53 | 54 | // Temporary local variables 55 | double bottomLeftX, bottomLeftY, width, height; // Bounding rectangle paramaters 56 | double sqrt3; // sqrt(3) 57 | double xs, ys; // Starting x, y for the raw lattice 58 | int n_w, n_h; // Raw lattice columns and rows 59 | 60 | 61 | // Converter 62 | CGAL::Cartesian_converter K_ICK_converter; 63 | 64 | // Visibility graph 65 | double m_epsilon; 66 | Environment * m_pEnvironment; 67 | Visibility_Graph * m_pVisibilityGraph; 68 | 69 | double m_radius; 70 | 71 | public: 72 | Roadmap(){m_epsilon = 0.000001; m_pEnvironment = 0; m_pVisibilityGraph = 0;} 73 | 74 | // Build roadmap 75 | void buildRoadmap(Polygon2_list* pObsList, Polygon_2* pbondingRect, double radius); 76 | 77 | // Retrive all vertices and edges 78 | std::list & getAllVertices(){return m_pointList;}; 79 | 80 | // Paint method 81 | void addToScene(QGraphicsScene& scene, bool drawEdge = true, 82 | QPen edgePen = QPen(Qt::red, 0.025, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin), 83 | bool drawVertex = true, 84 | QPen vertexPen = QPen(Qt::red, 0.05, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); 85 | 86 | // Build a hexagonal lattice that covers the entire bounding rectangle. The rectangle should 87 | // contain the configuraiton space and not the free space 88 | void buildHexgaonLattice(); 89 | void drawHexagonLattice(QGraphicsScene& scene, bool drawVetex = false); 90 | 91 | // Remove edges that do not fully belong to the configuration space 92 | void removeExcessEdges(); 93 | void drawBoundingCycle(QGraphicsScene& scene); 94 | 95 | void drawVertexIds(QGraphicsScene& scene); 96 | 97 | // Build the final graph for graph-based computation 98 | void buildFinalGraph(); 99 | 100 | // Generate random start and goal pairs 101 | void createRandomStartGoalPairs(int numRobots, double spacing, vector >& starts, 102 | vector >& goals); 103 | 104 | // Solve a problem 105 | bool solveProblem(vector >& sgVec, map >& paths, 106 | string& fileFolder, string& fileNameExtra = string("")); 107 | 108 | // Post process path to reduce ossilation 109 | void improvePaths(map >& paths); 110 | 111 | // Retrive the location of a vertex given the int id in m_finalGraph 112 | pair getVertexLocationFromID(int vid); 113 | 114 | // Build visibility graph 115 | void buildVisibilityGraph(); 116 | 117 | // Compute shortest path between points 118 | double computeShortestPath(Point_2& p1, Point_2& p2, std::list & path); 119 | double computeShortestPath(double x1, double y1, double x2, double y2, std::list & path); 120 | double computeShortestPath(double x1, double y1, double x2, double y2, vector >& path); 121 | 122 | private: 123 | // Refresh edge list 124 | void refreshEdgeList(); 125 | 126 | // Check whether a point is inside the configuration space 127 | bool isPointInCSpace(Point_2 &p); 128 | 129 | // Get the (col, row) of the hexagon a point belongs to 130 | std::pair locateHexagonForPoint(ICPoint_2 &p); 131 | 132 | // Check whether and edge is in an edge set 133 | bool edgeInSet(int v1, int v2, std::set >& edgeSet); 134 | 135 | // Add edge to graph if one is not already in 136 | void addEdgeIfNotThere(int v1, int v2, Graph & graph); 137 | 138 | // Get the next hexagon bordering the current at an edge 139 | std::pair getBorderHexagon(int col, int row, int v1, int v2, int pIndex[]); 140 | 141 | // Get the next vertex in a sequence of vertices 142 | int getNextNodeInSequence(int i1, int i2, int pIndex[]); 143 | 144 | // Compute a hexgon on the lattice given a (col, row) 145 | void populateHexagon(int col, int row, Point_2* p, Segment_2* e, Polygon_2 &hex, int* pIndex); 146 | 147 | // Compute all intersecting edges of the full lattice with the polygon obstacle 148 | void getIntersectingEdges(Polygon_2 & poly, Graph& boundingCycle, bool outerBoundary = false); 149 | 150 | // Check whether a point belongs to some edge not in the obstacle 151 | int pointBelongToEdgeOutideObstacle(Polygon_2 & poly, int pIndex, std::set > &tempEdgeToRemoveSet, bool outerBoundary = false); 152 | 153 | // Check whether a point is outside an obstacle 154 | bool pointOutsideObstacle(Polygon_2 & poly, int pIndex, bool outerBoundary = false); 155 | 156 | // Generate random start and goal pairs 157 | void createRandomCoords(int numRobots, double spacing, vector >& coords); 158 | 159 | public: 160 | 161 | // Snapping a set of robots to grid points 162 | void snapToGraph(vector >&coords, vector&snapped); 163 | 164 | }; 165 | 166 | #endif //_OPTMLY_ROADMAP_H_ 167 | -------------------------------------------------------------------------------- /shortest_path/README: -------------------------------------------------------------------------------- 1 | Obtain and put the visilibity library files (hpp and cpp) here. -------------------------------------------------------------------------------- /test-envs/cross.txt: -------------------------------------------------------------------------------- 1 | 1.8 2 | 5 3 | 12 4 | 28 16 5 | 32 16 6 | 32 28 7 | 44 28 8 | 44 32 9 | 32 32 10 | 32 44 11 | 28 44 12 | 28 32 13 | 16 32 14 | 16 28 15 | 28 28 16 | 4 17 | 11 11 18 | 14 11 19 | 14 14 20 | 11 14 21 | 4 22 | 11 49 23 | 14 49 24 | 14 46 25 | 11 46 26 | 4 27 | 46 11 28 | 49 11 29 | 49 14 30 | 46 14 31 | 4 32 | 49 49 33 | 46 49 34 | 46 46 35 | 49 46 36 | 4 37 | 0 0 38 | 0 60 39 | 60 60 40 | 60 0 41 | -------------------------------------------------------------------------------- /test-envs/empty.txt: -------------------------------------------------------------------------------- 1 | 1.8 2 | 0 3 | 4 4 | 0 0 5 | 0 60 6 | 60 60 7 | 60 0 8 | -------------------------------------------------------------------------------- /test-envs/regular.txt: -------------------------------------------------------------------------------- 1 | 1.801 2 | 3 3 | 5 4 | 45 9.3 5 | 49 17.5 6 | 48.5 24 7 | 42 26 8 | 45 18 9 | 3 10 | 12 19 11 | 22 23 12 | 15 26 13 | 5 14 | 33 38 15 | 39 38 16 | 43 46 17 | 36 51 18 | 26 47 19 | 4 20 | 0 0 21 | 0 60 22 | 60 60 23 | 60 0 24 | -------------------------------------------------------------------------------- /test-envs/tirangles.txt: -------------------------------------------------------------------------------- 1 | 1.8 2 | 12 3 | 3 4 | 12.6 2 5 | 9.6 6 6 | 14.5 5.2 7 | 3 8 | 47.6 2.47 9 | 52 3.85 10 | 50.15 -1.2 11 | 3 12 | 1.28 16.5 13 | 4.65 15.8 14 | 5.73 20 15 | 3 16 | 23.6 11.8 17 | 25.5 15.6 18 | 22.85 17.8 19 | 3 20 | 40.9 12.15 21 | 41.9 19.1 22 | 45.8 14.7 23 | 3 24 | 9.3 34.4 25 | 10.5 41.8 26 | 6.4 38.5 27 | 3 28 | 35.1 36.7 29 | 31.25 41.5 30 | 30.4 37.7 31 | 3 32 | 49.5 30.4 33 | 51 34.7 34 | 48.3 37.3 35 | 3 36 | 0.4 51.8 37 | 3.3 55.5 38 | 0 58.2 39 | 3 40 | 18.8 50 41 | 21.6 52.5 42 | 14.9 53.4 43 | 3 44 | 33.7 54.8 45 | 38.8 52.4 46 | 34.1 58.6 47 | 3 48 | 52.8 49.85 49 | 55.6 53.4 50 | 52.8 55 51 | 4 52 | 0 0 53 | 0 60 54 | 60 60 55 | 60 0 56 | -------------------------------------------------------------------------------- /test-envs/vertical.txt: -------------------------------------------------------------------------------- 1 | 1.8 2 | 7 3 | 4 4 | 28.5 -2 5 | 31.5 -2 6 | 31.5 8 7 | 28.5 8 8 | 4 9 | 28.5 52 10 | 31.5 52 11 | 31.5 62 12 | 28.5 62 13 | 4 14 | 28.5 26 15 | 31.5 26 16 | 31.5 34 17 | 28.5 34 18 | 4 19 | 13.5 10 20 | 16.5 10 21 | 16.5 20 22 | 13.5 20 23 | 4 24 | 13.5 40 25 | 16.5 40 26 | 16.5 50 27 | 13.5 50 28 | 4 29 | 43.5 10 30 | 46.5 10 31 | 46.5 20 32 | 43.5 20 33 | 4 34 | 43.5 40 35 | 46.5 40 36 | 46.5 50 37 | 43.5 50 38 | 4 39 | 0 0 40 | 0 60 41 | 60 60 42 | 60 0 43 | -------------------------------------------------------------------------------- /types.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Type definitions 3 | * 4 | * Created on: Jan 30, 2015 5 | * Author: Jingjin Yu 6 | */ 7 | 8 | #ifndef _O_CGAL_TYPES_H_ 9 | #define _O_CGAL_TYPES_H_ 10 | 11 | #define _CRT_SECURE_NO_WARNINGS 1 12 | #include "number_type.h" 13 | 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | 32 | #include 33 | 34 | // Kernel used for computation 35 | typedef CGAL::Exact_predicates_inexact_constructions_kernel K; 36 | typedef K::Point_2 Point_2; 37 | typedef K::Segment_2 Segment_2; 38 | typedef std::list Seg2_list; 39 | typedef K::Line_2 Line_2; 40 | typedef K::Vector_2 Vector_2; 41 | 42 | typedef CGAL::Polygon_2 Polygon_2; 43 | typedef std::list Polygon2_list; 44 | typedef CGAL::Polygon_with_holes_2 Polygon_with_holes_2; 45 | typedef CGAL::Polygon_set_2 Polygon_set_2; 46 | 47 | typedef CGAL::Point_set_2::Vertex_handle Vertex_handle; 48 | 49 | // Kernel used for extracting some intermediate data 50 | typedef CGAL::Exact_predicates_inexact_constructions_kernel ICK; 51 | typedef ICK::Point_2 ICPoint_2; 52 | typedef CGAL::Polygon_2 ICPolygon_2; 53 | 54 | 55 | typedef CGAL::Exact_predicates_exact_constructions_kernel EK; 56 | typedef EK::Point_2 ECPoint_2; 57 | typedef EK::Segment_2 ECSegment_2; 58 | typedef CGAL::Polygon_2 ECPolygon_2; 59 | typedef CGAL::Polygon_with_holes_2 ECPolygon_with_holes_2; 60 | 61 | 62 | #endif //_OPTMLY_CGAL_TYPES_H_ 63 | -------------------------------------------------------------------------------- /vc10/main_window_moc.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | ** Meta object code from reading C++ file 'main_window.h' 3 | ** 4 | ** Created: Sun Nov 20 14:25:09 2016 5 | ** by: The Qt Meta Object Compiler version 63 (Qt 4.8.2) 6 | ** 7 | ** WARNING! All changes made in this file will be lost! 8 | *****************************************************************************/ 9 | 10 | #include "../main_window.h" 11 | #if !defined(Q_MOC_OUTPUT_REVISION) 12 | #error "The header file 'main_window.h' doesn't include ." 13 | #elif Q_MOC_OUTPUT_REVISION != 63 14 | #error "This file was generated using the moc from 4.8.2. It" 15 | #error "cannot be used with the include files from this version of Qt." 16 | #error "(The moc has changed too much.)" 17 | #endif 18 | 19 | QT_BEGIN_MOC_NAMESPACE 20 | static const uint qt_meta_data_MainWindow[] = { 21 | 22 | // content: 23 | 6, // revision 24 | 0, // classname 25 | 0, 0, // classinfo 26 | 8, 14, // methods 27 | 0, 0, // properties 28 | 0, 0, // enums/sets 29 | 0, 0, // constructors 30 | 0, // flags 31 | 0, // signalCount 32 | 33 | // slots: signature, parameters, type, tag, flags 34 | 12, 11, 11, 11, 0x0a, 35 | 30, 11, 11, 11, 0x0a, 36 | 47, 11, 11, 11, 0x0a, 37 | 76, 11, 11, 11, 0x0a, 38 | 98, 11, 11, 11, 0x0a, 39 | 113, 11, 11, 11, 0x0a, 40 | 123, 11, 11, 11, 0x0a, 41 | 133, 11, 11, 11, 0x0a, 42 | 43 | 0 // eod 44 | }; 45 | 46 | static const char qt_meta_stringdata_MainWindow[] = { 47 | "MainWindow\0\0openEnvironment()\0" 48 | "overlayLattice()\0locateBoundingLatticeCycle()\0" 49 | "createRandomProblem()\0solveProblem()\0" 50 | "animate()\0fitView()\0animatePath()\0" 51 | }; 52 | 53 | void MainWindow::qt_static_metacall(QObject *_o, QMetaObject::Call _c, int _id, void **_a) 54 | { 55 | if (_c == QMetaObject::InvokeMetaMethod) { 56 | Q_ASSERT(staticMetaObject.cast(_o)); 57 | MainWindow *_t = static_cast(_o); 58 | switch (_id) { 59 | case 0: _t->openEnvironment(); break; 60 | case 1: _t->overlayLattice(); break; 61 | case 2: _t->locateBoundingLatticeCycle(); break; 62 | case 3: _t->createRandomProblem(); break; 63 | case 4: _t->solveProblem(); break; 64 | case 5: _t->animate(); break; 65 | case 6: _t->fitView(); break; 66 | case 7: _t->animatePath(); break; 67 | default: ; 68 | } 69 | } 70 | Q_UNUSED(_a); 71 | } 72 | 73 | const QMetaObjectExtraData MainWindow::staticMetaObjectExtraData = { 74 | 0, qt_static_metacall 75 | }; 76 | 77 | const QMetaObject MainWindow::staticMetaObject = { 78 | { &CGAL::Qt::DemosMainWindow::staticMetaObject, qt_meta_stringdata_MainWindow, 79 | qt_meta_data_MainWindow, &staticMetaObjectExtraData } 80 | }; 81 | 82 | #ifdef Q_NO_DATA_RELOCATION 83 | const QMetaObject &MainWindow::getStaticMetaObject() { return staticMetaObject; } 84 | #endif //Q_NO_DATA_RELOCATION 85 | 86 | const QMetaObject *MainWindow::metaObject() const 87 | { 88 | return QObject::d_ptr->metaObject ? QObject::d_ptr->metaObject : &staticMetaObject; 89 | } 90 | 91 | void *MainWindow::qt_metacast(const char *_clname) 92 | { 93 | if (!_clname) return 0; 94 | if (!strcmp(_clname, qt_meta_stringdata_MainWindow)) 95 | return static_cast(const_cast< MainWindow*>(this)); 96 | typedef CGAL::Qt::DemosMainWindow QMocSuperClass; 97 | return QMocSuperClass::qt_metacast(_clname); 98 | } 99 | 100 | int MainWindow::qt_metacall(QMetaObject::Call _c, int _id, void **_a) 101 | { 102 | typedef CGAL::Qt::DemosMainWindow QMocSuperClass; 103 | _id = QMocSuperClass::qt_metacall(_c, _id, _a); 104 | if (_id < 0) 105 | return _id; 106 | if (_c == QMetaObject::InvokeMetaMethod) { 107 | if (_id < 8) 108 | qt_static_metacall(this, _c, _id, _a); 109 | _id -= 8; 110 | } 111 | return _id; 112 | } 113 | QT_END_MOC_NAMESPACE 114 | --------------------------------------------------------------------------------