RenderPlannerData.cpp
1 /*********************************************************************
2 * Rice University Software Distribution License
3 *
4 * Copyright (c) 2010, Rice University
5 * All Rights Reserved.
6 *
7 * For a full description see the file named LICENSE.
8 *
9 *********************************************************************/
10 
11 /* Author: Ioan Sucan */
12 
13 #include "omplapp/graphics/detail/RenderPlannerData.h"
14 #include <ompl/base/spaces/SE2StateSpace.h>
15 #include <ompl/base/spaces/SE3StateSpace.h>
16 
17 #ifdef __APPLE__
18 #include <OpenGL/gl.h>
19 #else
20 #include <GL/gl.h>
21 #endif
22 
23 namespace ompl
24 {
25  namespace app
26  {
27 
28  static void renderState(const base::SE2StateSpace::StateType &state)
29  {
30  glVertex3d(state.getX(), state.getY(), 0.0);
31  }
32 
33  static void renderState(const base::SE3StateSpace::StateType &state)
34  {
35  glVertex3d(state.getX(), state.getY(), state.getZ());
36  }
37 
38  static void renderEdge(const base::SE2StateSpace::StateType &state1,
39  const base::SE2StateSpace::StateType &state2)
40  {
41  renderState(state1);
42  renderState(state2);
43  }
44 
45  static void renderEdge(const base::SE3StateSpace::StateType &state1,
46  const base::SE3StateSpace::StateType &state2)
47  {
48  renderState(state1);
49  renderState(state2);
50  }
51 
52  static void setStateColor(int tag)
53  {
54  static const int NC = 7;
55  static float colors[NC][4] =
56  {
57  {1.0f, 0.0f, 0.0f, 0.6f},
58  {0.0f, 1.0f, 0.0f, 0.6f},
59  {0.0f, 0.0f, 1.0f, 0.6f},
60  {1.0f, 1.0f, 0.0f, 0.6f},
61  {0.0f, 1.0f, 1.0f, 0.6f},
62  {1.0f, 0.0f, 1.0f, 0.6f},
63  {1.0f, 1.0f, 1.0f, 0.6f},
64  };
65 
66  int c = abs(tag) % NC;
67  glColor4f(colors[c][0], colors[c][1], colors[c][2], colors[c][3]);
68  }
69 
70  int RenderPlannerData(const base::PlannerData &pd, const aiVector3D &translate, MotionModel m, const GeometricStateExtractor &gse, unsigned int count)
71  {
72  static int result = -1;
73 
74  if (result != -1)
75  glDeleteLists(result, 2);
76  result = glGenLists(2);
77 
78  aiMatrix4x4 t;
79  aiMatrix4x4::Translation(-translate, t);
80  aiTransposeMatrix4(&t);
81 
82  // render vertices
83  glNewList(result, GL_COMPILE);
84  glPushMatrix();
85  glMultMatrixf((float*)&t);
86  glDisable(GL_LIGHTING);
87  glDisable(GL_COLOR_MATERIAL);
88  glPointSize(2.0f);
89  glBegin(GL_POINTS);
90  if (m == Motion_2D)
91  for (std::size_t i = 0; i < pd.numVertices(); ++i)
92  {
93  const base::PlannerDataVertex& vtx = pd.getVertex(i);
94  setStateColor(vtx.getTag());
95  for (unsigned int r = 0 ; r < count ; ++r)
96  {
97  const base::State *st = gse(vtx.getState(), r);
98  const auto* se2st = static_cast<const base::SE2StateSpace::StateType*>(st);
99  renderState (*se2st);
100  }
101  }
102  else
103  for (std::size_t i = 0; i < pd.numVertices(); ++i)
104  {
105  const base::PlannerDataVertex& vtx = pd.getVertex(i);
106  setStateColor(vtx.getTag());
107  for (unsigned int r = 0 ; r < count ; ++r)
108  {
109  const base::State *st = gse(vtx.getState(), r);
110  const auto* se3st = static_cast<const base::SE3StateSpace::StateType*>(st);
111  renderState (*se3st);
112  }
113  }
114  glEnd();
115  glPopMatrix();
116  glEndList();
117 
118  // render edges
119  glNewList(result+1, GL_COMPILE);
120  glPushMatrix();
121  glMultMatrixf((float*)&t);
122  glDisable(GL_LIGHTING);
123  glDisable(GL_COLOR_MATERIAL);
124  glPointSize(2.0f);
125  glBegin(GL_LINES);
126  std::vector<unsigned int> edgeList;
127  unsigned int numEdges;
128  if (m == Motion_2D)
129  for (std::size_t i = 0; i < pd.numVertices(); ++i)
130  {
131  const base::PlannerDataVertex& vtx = pd.getVertex(i);
132  const auto* vi =
133  static_cast<const base::SE2StateSpace::StateType*>(gse(vtx.getState(), 0));
134  setStateColor(vtx.getTag());
135  numEdges = pd.getEdges(i, edgeList);
136  for (unsigned int j = 0; j < numEdges; ++j)
137  {
138  const auto* vj =
139  static_cast<const base::SE2StateSpace::StateType*>(gse(pd.getVertex(edgeList[j]).getState(), 0));
140  renderEdge(*vi, *vj);
141  }
142  }
143  else
144  for (std::size_t i = 0; i < pd.numVertices(); ++i)
145  {
146  const base::PlannerDataVertex& vtx = pd.getVertex(i);
147  const auto* vi =
148  static_cast<const base::SE3StateSpace::StateType*>(gse(vtx.getState(), 0));
149  setStateColor(vtx.getTag());
150  numEdges = pd.getEdges(i, edgeList);
151  for (unsigned int j = 0; j < numEdges; ++j)
152  {
153  const auto* vj =
154  static_cast<const base::SE3StateSpace::StateType*>(gse(pd.getVertex(edgeList[j]).getState(), 0));
155  renderEdge(*vi, *vj);
156  }
157  }
158  glEnd();
159  glPopMatrix();
160  glEndList();
161 
162  return result;
163  }
164 
165  }
166 }
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
A state in SE(2): (x, y, yaw)
Definition: SE2StateSpace.h:53
A state in SE(3): position = (x, y, z), quaternion = (x, y, z, w)
Definition: SE3StateSpace.h:53
int RenderPlannerData(const base::PlannerData &pd, const aiVector3D &translate, MotionModel m, const GeometricStateExtractor &gse, unsigned int count)
Render the planner states in pd, after shifting them by translate, using the motion model m...
unsigned int numVertices() const
Retrieve the number of vertices in this structure.
const PlannerDataVertex & getVertex(unsigned int index) const
Retrieve a reference to the vertex object with the given index. If this vertex does not exist...
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:58
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:577
MotionModel
Specify whether bodies are moving in 2D or bodies moving in 3D.
Definition of an abstract state.
Definition: State.h:49
virtual const State * getState() const
Retrieve the state associated with this vertex.
Definition: PlannerData.h:80
unsigned int getEdges(unsigned int v, std::vector< unsigned int > &edgeList) const
Returns a list of the vertex indexes directly connected to vertex with index v (outgoing edges)...
virtual int getTag() const
Returns the integer tag associated with this vertex.
Definition: PlannerData.h:70