00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #include "pose_graph_vis3d.h"
00020
00021 #include "axes.h"
00022 #include "vrml_output.h"
00023 #include "primitives.h"
00024 #include "stuff/macros.h"
00025 #include "graph/posegraph3d.h"
00026
00027 #define NUM_DRAW_LISTS (2)
00028
00029 namespace AISNavigation {
00030
00031 using namespace std;
00032
00033 PoseGraph3DVis::PoseGraph3DVis() :
00034 _graph(0), _hirarchy(0),
00035 _updateDrawList(false), _useDrawList(false), _drawList(0), _listAllocated(false)
00036 {
00037 _colors.push_back(Color(1, 0, 0));
00038 _colors.push_back(Color(0, 1, 0));
00039 _colors.push_back(Color(0, 0, 1));
00040 _colors.push_back(Color(1, 1, 0));
00041 _colors.push_back(Color(1, 0, 1));
00042 _colors.push_back(Color(0, 1, 1));
00043 for (int i = 0; i < 100; ++i) {
00044 double r,g,b;
00045 GET_COLOR(i, 100, r, g, b);
00046 _colors.push_back(Color(r, g, b));
00047 }
00048 _drawOptions.resize(NUM_DRAW_LISTS, true);
00049 }
00050
00051 PoseGraph3DVis::~PoseGraph3DVis()
00052 {
00053 if (_listAllocated)
00054 glDeleteLists(_drawList, NUM_DRAW_LISTS);
00055 }
00056
00057 void PoseGraph3DVis::writeVrml(std::ostream& os) const
00058 {
00059 if (!_graph)
00060 return;
00061 writeVrmlFileHeader(os);
00062 os << Axes(1.0) << endl;
00063 writeStart(os);
00064 vrmlLoadIdentity();
00065 if (_drawOptions[0])
00066 writeGraph(os);
00067 if (_drawOptions[1])
00068 writeHirarchy(os);
00069 writeEnd(os);
00070 }
00071
00072 void PoseGraph3DVis::draw() const
00073 {
00074 if (!_graph)
00075 return;
00076
00077
00078 GLboolean hasLight = glIsEnabled(GL_LIGHTING);
00079 if (hasLight)
00080 glDisable(GL_LIGHTING);
00081
00082 if (_useDrawList) {
00083 if (!_listAllocated) {
00084 _drawList = glGenLists(NUM_DRAW_LISTS);
00085 _listAllocated = true;
00086 _updateDrawList = true;
00087 }
00088 if (_updateDrawList) {
00089 _updateDrawList = false;
00090
00091 if (_drawOptions[0])
00092 glNewList(_drawList, GL_COMPILE_AND_EXECUTE);
00093 else
00094 glNewList(_drawList, GL_COMPILE);
00095 drawGraph();
00096 glEndList();
00097
00098
00099 if (_drawOptions[1])
00100 glNewList(_drawList+1, GL_COMPILE_AND_EXECUTE);
00101 else
00102 glNewList(_drawList+1, GL_COMPILE);
00103 drawHirarchy();
00104 glEndList();
00105 } else {
00106 for (GLuint l = 0; l < NUM_DRAW_LISTS; ++l)
00107 if (_drawOptions[l])
00108 glCallList(_drawList + l);
00109 }
00110 } else {
00111 if (_drawOptions[0])
00112 drawGraph();
00113 if (_drawOptions[1])
00114 drawHirarchy();
00115 }
00116
00117 if (hasLight)
00118 glEnable(GL_LIGHTING);
00119 }
00120
00121 void PoseGraph3DVis::setGraph(PoseGraph3D* graph)
00122 {
00123 _graph = graph;
00124 if (_useDrawList)
00125 _updateDrawList = true;
00126 }
00127
00128 void PoseGraph3DVis::drawGraph() const
00129 {
00130
00131 for (PoseGraph3D::VertexIDMap::const_iterator it = _graph->vertices().begin();
00132 it != _graph->vertices().end(); ++it) {
00133 const PoseGraph3D::Vertex* vi = static_cast<PoseGraph3D::Vertex*>(it->second);
00134 const Vector3& t = vi->transformation.translation();
00135 AxisAngle axis(vi->transformation.rotation());
00136 double angle = axis.norm();
00137 axis.normalize();
00138 glPushMatrix();
00139 glTranslatef(t.x(), t.y(), t.z());
00140 glRotatef(RAD2DEG(angle), axis.x(), axis.y(), axis.z());
00141 drawPoseBox();
00142 glPopMatrix();
00143 }
00144
00145
00146 glColor3f(0.7f, 0.7f, 0.7f);
00147 glBegin(GL_LINES);
00148 for (PoseGraph3D::EdgeSet::const_iterator it = _graph->edges().begin();
00149 it != _graph->edges().end(); ++it) {
00150 const PoseGraph3D::Vertex* vi = static_cast<PoseGraph3D::Vertex*>((*it)->from());
00151 const PoseGraph3D::Vertex* vj = static_cast<PoseGraph3D::Vertex*>((*it)->to());
00152 const Vector3& t1 = vi->transformation.translation();
00153 const Vector3& t2 = vj->transformation.translation();
00154 glVertex3f(t1.x(), t1.y(), t1.z());
00155 glVertex3f(t2.x(), t2.y(), t2.z());
00156 }
00157 glEnd();
00158 }
00159
00160 void PoseGraph3DVis::setHirarchy(std::vector<PoseGraph3DVis::HEdgeVector>* hirarchy)
00161 {
00162 _hirarchy = hirarchy;
00163 }
00164
00165 void PoseGraph3DVis::drawHirarchy() const
00166 {
00167 if (!_hirarchy || !_graph)
00168 return;
00169
00170 int l = 0;
00171 glBegin(GL_LINES);
00172 for (std::vector<HEdgeVector>::const_iterator level = _hirarchy->begin(); level != _hirarchy->end(); ++level, ++l) {
00173 const HEdgeVector& hEdges = *level;
00174 glColor3f(_colors[l].r, _colors[l].g, _colors[l].b);
00175 for (HEdgeVector::const_iterator it = hEdges.begin(); it != hEdges.end(); ++it) {
00176 const PoseGraph3D::Vertex* vi = static_cast<PoseGraph3D::Vertex*>(_graph->vertex(it->id1));
00177 const PoseGraph3D::Vertex* vj = static_cast<PoseGraph3D::Vertex*>(_graph->vertex(it->id2));
00178 const Vector3& t1 = vi->transformation.translation();
00179 const Vector3& t2 = vj->transformation.translation();
00180 glVertex3f(t1.x(), t1.y(), t1.z());
00181 glVertex3f(t2.x(), t2.y(), t2.z());
00182 }
00183 }
00184 glEnd();
00185 }
00186
00187 void PoseGraph3DVis::writeGraph(std::ostream& os) const
00188 {
00189 os << "# PoseGraph3DVis \n";
00190
00191 for (PoseGraph3D::VertexIDMap::const_iterator it = _graph->vertices().begin();
00192 it != _graph->vertices().end(); ++it) {
00193 const PoseGraph3D::Vertex* vi = static_cast<PoseGraph3D::Vertex*>(it->second);
00194 const Vector3& t = vi->transformation.translation();
00195 AxisAngle axis(vi->transformation.rotation());
00196 double angle = axis.norm();
00197 axis.normalize();
00198 vrmlPushMatrix();
00199 vrmlTranslatef(t.x(), t.y(), t.z());
00200 vrmlRotatef(RAD2DEG(angle), axis.x(), axis.y(), axis.z());
00201 writePoseBox(os);
00202 vrmlPopMatrix();
00203 }
00204
00205
00206 std::list<Vector3> line_points;
00207 vrmlColor3f(0.5f, 0.5f, 0.5f);
00208 for (PoseGraph3D::EdgeSet::const_iterator it = _graph->edges().begin();
00209 it != _graph->edges().end(); ++it) {
00210 const PoseGraph3D::Vertex* vi = static_cast<PoseGraph3D::Vertex*>((*it)->from());
00211 const PoseGraph3D::Vertex* vj = static_cast<PoseGraph3D::Vertex*>((*it)->to());
00212 const Vector3& t1 = vi->transformation.translation();
00213 const Vector3& t2 = vj->transformation.translation();
00214 line_points.push_back(t1);
00215 line_points.push_back(t2);
00216 }
00217 writeLines(os, line_points.begin(), line_points.end());
00218 os << "# PoseGraph3DVis Nodes end\n";
00219 }
00220
00221 void PoseGraph3DVis::writeHirarchy(std::ostream& os) const
00222 {
00223 if (!_hirarchy || !_graph)
00224 return;
00225
00226 int l = 0;
00227 std::list<Vector3> line_points;
00228 for (std::vector<HEdgeVector>::const_iterator level = _hirarchy->begin(); level != _hirarchy->end(); ++level, ++l) {
00229 const HEdgeVector& hEdges = *level;
00230 vrmlColor3f(_colors[l].r, _colors[l].g, _colors[l].b);
00231 for (HEdgeVector::const_iterator it = hEdges.begin(); it != hEdges.end(); ++it) {
00232 const PoseGraph3D::Vertex* vi = static_cast<PoseGraph3D::Vertex*>(_graph->vertex(it->id1));
00233 const PoseGraph3D::Vertex* vj = static_cast<PoseGraph3D::Vertex*>(_graph->vertex(it->id2));
00234 const Vector3& t1 = vi->transformation.translation();
00235 const Vector3& t2 = vj->transformation.translation();
00236 line_points.push_back(t1);
00237 line_points.push_back(t2);
00238 }
00239 }
00240 writeLines(os, line_points.begin(), line_points.end());
00241 }
00242
00243 void PoseGraph3DVis::setDrawGraph(bool draw)
00244 {
00245 _drawOptions[0] = draw;
00246 }
00247
00248 void PoseGraph3DVis::setDrawHirarchy(bool draw)
00249 {
00250 _drawOptions[1] = draw;
00251 }
00252
00253 }