void printDist(minHeap* h) { int s = h->size; int i = 0; while(s--) { i++; if (isPQ(h, i)) { s++; continue; } if (dist[i] == preDist[i]) { printf("Q[%d] : d[%c] = %d\n", i-1, printVertex(i), dist[i]); } else { printf("Q[%d] : d[%c] = %d -> d[%c] = %d\n", i-1, printVertex(i), preDist[i], printVertex(i), dist[i]); } } printf("\n\n"); }
void dijkstra(int start,int size) { minHeap* h = (minHeap*)malloc(sizeof(minHeap)); h->size = 0; h->array[0] = unused; dist[start] = 0; int count = 0; preDist[start] = 0; for (int i = 1; i <= size; i++) { push(h, i); } while (!isEmpty(h)) { int top = pop(h); printf("------------------------------------------------------\n"); printf("S[%d] : d[%c] = %d\n", count,printVertex(top), dist[top]); count++; printf("------------------------------------------------------\n"); for (int i = 1; i <= size; i++) { if (graph[top-1][i-1]!=inf) { //연결 if (dist[i] > dist[top] + graph[top-1][i-1]) { dist[i] = dist[top] + graph[top-1][i-1]; buildMinHeap(h); } } } printDist(h); for(int i=1; i<=size; i++){ preDist[i] = dist[i]; } } }
bool G2oSlamInterface::queryState(const std::vector<int>& nodes) { //return true; cout << "BEGIN" << endl; #if 1 if (nodes.size() == 0) { for (OptimizableGraph::VertexIDMap::const_iterator it = _optimizer->vertices().begin(); it != _optimizer->vertices().end(); ++it) { OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second); printVertex(v); } } else { for (size_t i = 0; i < nodes.size(); ++i) { OptimizableGraph::Vertex* v = _optimizer->vertex(nodes[i]); if (v) printVertex(v); } } #endif cout << "END" << endl << flush; return true; }
bool HogmanSlamInterface::queryState(const std::vector<int>& nodes) { cout << "BEGIN" << endl; if (_optimizer) { // 2D SLAM if (nodes.size() == 0) { for (Graph::VertexIDMap::const_iterator it = _optimizer->vertices().begin(); it != _optimizer->vertices().end(); it++){ PoseGraph2D::Vertex* v = static_cast<PoseGraph2D::Vertex*>(it->second); printVertex(v); } } else { for (size_t i = 0; i < nodes.size(); ++i) { PoseGraph2D::Vertex* v = static_cast<PoseGraph2D::Vertex*>(_optimizer->vertex(nodes[i])); if (v) printVertex(v); } } } else if (_optimizer3D) { // 3D SLAM if (nodes.size() == 0) { for (Graph::VertexIDMap::const_iterator it = _optimizer3D->vertices().begin(); it != _optimizer3D->vertices().end(); it++){ PoseGraph3D::Vertex* v = static_cast<PoseGraph3D::Vertex*>(it->second); printVertex(v); } } else { for (size_t i = 0; i < nodes.size(); ++i) { PoseGraph3D::Vertex* v = static_cast<PoseGraph3D::Vertex*>(_optimizer3D->vertex(nodes[i])); if (v) printVertex(v); } } } cout << "END" << endl << flush; return true; }
int main(){ struct vertex* head = NULL; GraphAddVertex(&head,6); GraphAddVertex(&head,5); GraphAddVertex(&head,4); GraphAddVertex(&head,3); GraphAddVertex(&head,2); GraphAddVertex(&head,1); printVertex(head); GraphAddEdge(head,1,2,1); struct edge* ed = head->edges; printf("%d\n",ed==NULL); printEdge(head); return 0; }