BasicGraph* gridToGraph(const Grid<double>& world, double costFn(const TBLoc& from, const TBLoc& to, const Grid<double>& world)) { BasicGraph* graph = new BasicGraph(); VertexObserver* obs = new VertexObserver(); obs->world = &world; // add vertices int rows = world.numRows(); int cols = world.numCols(); for (int r = 0; r < rows; r++) { for (int c = 0; c < cols; c++) { string name = vertexName(r, c, world); Vertex* v = new Vertex(name); v->extraData = new TBLoc(r, c, world.get(r, c)); v->addObserver(obs); graph->addVertex(v); } } // add edges for (int r = 0; r < rows; r++) { for (int c = 0; c < cols; c++) { Vertex* v = graph->getVertex(vertexName(r, c, world)); for (int dr = -1; dr <= 1; dr++) { for (int dc = -1; dc <= 1; dc++) { int nr = r + dr; int nc = c + dc; if ((dr == 0 && dc == 0) || !world.inBounds(nr, nc)) { continue; } Vertex* neighbor = graph->getVertex(vertexName(nr, nc, world)); double cost = costFn(TBLoc(r, c), TBLoc(nr, nc), world); if (cost != POSITIVE_INFINITY) { Edge* e = new Edge(v, neighbor, cost); e->extraData = new TBEdge(TBLoc(r, c), TBLoc(nr, nc)); graph->addEdge(e); } } } } } return graph; }