/* Here is a simple representation of the visibility graph for the figure, and some utility routines for creating and freeing the data structure. */ #include #include #include typedef struct { float x, y; // location float d; // distance from source int p; // predecessor } VertexType; typedef struct { int numVertices; VertexType *vertex; unsigned char **edge; } GraphType; // allocates data for a graph with nv vertices GraphType *mallocGraph(int nv); GraphType *mallocGraph(int nv) { GraphType *gt; int i; gt = malloc(sizeof(GraphType)); gt->numVertices = nv; // allocate nv vertices gt->vertex = malloc(sizeof(VertexType) * nv); // allocate nv char pointers gt->edge = malloc(sizeof(unsigned char *) * nv); // allocate nv * nv char locations gt->edge[0] = malloc(sizeof(unsigned char) * nv * nv); // point the pointers at the right place in the block of memory for(i=1;iedge[i] = &(gt->edge[0][i*nv]); // return the allocated graph return(gt); } void freeGraph(GraphType *gt); void freeGraph(GraphType *gt) { free(gt->edge[0]); free(gt->edge); free(gt->vertex); free(gt); return; } float cost(VertexType *vt1, VertexType *vt2); float cost(VertexType *vt1, VertexType *vt2) { return(sqrt((vt1->x - vt2->x)*(vt1->x - vt2->x) + (vt1->y - vt2->y) * (vt1->y - vt2->y))); } #define N 14 int main(int argc, char *argv[]) { GraphType *gt; int S[N]; // visited vertices int Q[N]; // vertices not yet visited int i, j, curNode; // allocate the graph gt = mallocGraph(N); // create the graph gt->vertex[0].x = 0; // robot gt->vertex[0].y = 6; gt->vertex[1].x = 10; // bottom obstacle ll gt->vertex[1].y = 0; gt->vertex[2].x = 12; // bottom obstacle lr gt->vertex[2].y = 0; gt->vertex[3].x = 12; // bottom obstacle ur gt->vertex[3].y = 7; gt->vertex[4].x = 10; // bottom obstacle ul gt->vertex[4].y = 7; gt->vertex[5].x = 4; // middle obstacle ll gt->vertex[5].y = 9; gt->vertex[6].x = 13; // middle obstacle lr gt->vertex[6].y = 9; gt->vertex[7].x = 13; // middle obstacle ur gt->vertex[7].y = 11; gt->vertex[8].x = 4; // middle obstacle ul gt->vertex[8].y = 11; gt->vertex[9].x = 15; // right obstacle ll gt->vertex[9].y = 5; gt->vertex[10].x = 17; // right obstacle lr gt->vertex[10].y = 5; gt->vertex[11].x = 17; // right obstacle ur gt->vertex[11].y = 12; gt->vertex[12].x = 15; // right obstacle ul gt->vertex[12].y = 12; gt->vertex[13].x = 18; // goal point gt->vertex[13].y = 11; // initialize the remainder of the graph vertices for(i=0;ivertex[i].d = 1e+7; // big number gt->vertex[i].p = -1; // No predecessor } gt->vertex[0].d = 0; // initialize source node // initialize the upper right half of the edge matrix gt->edge[0][1] = 1; gt->edge[0][4] = 1; gt->edge[0][5] = 1; gt->edge[0][6] = 1; gt->edge[0][8] = 1; gt->edge[1][0] = 1; gt->edge[1][2] = 1; gt->edge[1][4] = 1; gt->edge[1][5] = 1; gt->edge[2][10] = 1; gt->edge[2][9] = 1; gt->edge[2][12] = 1; gt->edge[2][6] = 1; gt->edge[2][3] = 1; gt->edge[3][9] = 1; gt->edge[3][12] = 1; gt->edge[3][6] = 1; gt->edge[3][5] = 1; gt->edge[3][4] = 1; gt->edge[4][6] = 1; gt->edge[4][5] = 1; gt->edge[5][6] = 1; gt->edge[5][8] = 1; gt->edge[6][9] = 1; gt->edge[6][12] = 1; gt->edge[6][7] = 1; gt->edge[7][9] = 1; gt->edge[7][12] = 1; gt->edge[7][8] = 1; gt->edge[8][12] = 1; gt->edge[9][10] = 1; gt->edge[9][12] = 1; gt->edge[10][13] = 1; gt->edge[10][11] = 1; gt->edge[11][13] = 1; gt->edge[11][12] = 1; // transpose it to fill in the lower left half of the matrix for(i=0;iedge[i][j] = gt->edge[j][i]; } } // now we can run Dijkstra's algorithm // S = set of visited vertices // Q = set of vertices not yet visited (should be a priority queue, but whatever) for(i=0;i