Beispiel #1
0
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;
}