qtopengl_epuck.cpp
Go to the documentation of this file.
1 
7 #include "qtopengl_epuck.h"
8 #include "epuck_entity.h"
9 #include <argos3/core/simulator/entity/embodied_entity.h>
10 #include <argos3/core/utility/math/vector2.h>
11 #include <argos3/core/utility/math/vector3.h>
12 #include <argos3/plugins/simulator/entities/led_equipped_entity.h>
13 #include <argos3/plugins/simulator/visualizations/qt-opengl/qtopengl_widget.h>
14 
15 namespace argos {
16 
17  /****************************************/
18  /****************************************/
19 
20  /* All measures are in meters */
21 
22  static const Real WHEEL_DIAMETER = 0.041f;
23  static const Real WHEEL_RADIUS = WHEEL_DIAMETER * 0.5f;
24  static const Real WHEEL_WIDTH = 0.01f;
25  static const Real HALF_WHEEL_WIDTH = WHEEL_WIDTH * 0.5f;
26  static const Real INTERWHEEL_DISTANCE = 0.053f;
27  static const Real HALF_INTERWHEEL_DISTANCE = INTERWHEEL_DISTANCE * 0.5f;
28 
29  static const Real CHASSIS_ELEVATION = 0.005f; // to be checked!
30  static const Real HALF_CHASSIS_LENGTH = 0.0275f; // to be checked!
31  static const Real HALF_CHASSIS_WIDTH = HALF_INTERWHEEL_DISTANCE - HALF_WHEEL_WIDTH;
32 
33  static const Real BODY_RADIUS = 0.035f;
34  static const Real BODY_ELEVATION = WHEEL_DIAMETER + CHASSIS_ELEVATION; // to be checked!
35  static const Real BODY_HEIGHT = 0.03f; // to be checked!
36 
37  static const Real LED_ELEVATION = BODY_ELEVATION + BODY_HEIGHT;
38  static const Real LED_HEIGHT = 0.01; // to be checked!
39  static const Real LED_UPPER_RING_INNER_RADIUS = 0.8 * BODY_RADIUS;
40 
41  /****************************************/
42  /****************************************/
43 
45  m_unVertices(40),
46  m_fLEDAngleSlice(360.0f / 8.0f) {
47  /* Reserve the needed display lists */
48  m_unLists = glGenLists(4);
49 
50  /* Assign indices for better referencing (later) */
51  m_unWheelList = m_unLists;
52  m_unChassisList = m_unLists + 1;
53  m_unBodyList = m_unLists + 2;
54  m_unLEDList = m_unLists + 3;
55 
56  /* Create the wheel display list */
57  glNewList(m_unWheelList, GL_COMPILE);
58  RenderWheel();
59  glEndList();
60 
61  /* Create the body display list */
62  glNewList(m_unBodyList, GL_COMPILE);
63  RenderBody();
64  glEndList();
65 
66  /* Create the chassis display list */
67  glNewList(m_unChassisList, GL_COMPILE);
68  RenderChassis();
69  glEndList();
70 
71  /* Create the LED display list */
72  glNewList(m_unLEDList, GL_COMPILE);
73  RenderLED();
74  glEndList();
75  }
76 
77  /****************************************/
78  /****************************************/
79 
81  glDeleteLists(m_unLists, 4);
82  }
83 
84  /****************************************/
85  /****************************************/
86 
88  /* Place the chassis */
89  glCallList(m_unChassisList);
90  /* Place the body */
91  glCallList(m_unBodyList);
92  /* Place the wheels */
93  glPushMatrix();
94  glTranslatef(0.0f, HALF_INTERWHEEL_DISTANCE, 0.0f);
95  glCallList(m_unWheelList);
96  glPopMatrix();
97  glPushMatrix();
98  glTranslatef(0.0f, -HALF_INTERWHEEL_DISTANCE, 0.0f);
99  glCallList(m_unWheelList);
100  glPopMatrix();
101  /* Place the LEDs */
102  glPushMatrix();
103  CLEDEquippedEntity& cLEDEquippedEntity = c_entity.GetLEDEquippedEntity();
104  for(UInt32 i = 0; i < 8; i++) {
105  const CColor& cColor = cLEDEquippedEntity.GetLED(i).GetColor();
106  glRotatef(-m_fLEDAngleSlice, 0.0f, 0.0f, 1.0f);
107  SetLEDMaterial(cColor.GetRed(),
108  cColor.GetGreen(),
109  cColor.GetBlue());
110  glCallList(m_unLEDList);
111  }
112  glPopMatrix();
113  }
114 
115  /****************************************/
116  /****************************************/
117 
119  const GLfloat pfColor[] = { 0.0f, 1.0f, 0.0f, 1.0f };
120  const GLfloat pfSpecular[] = { 0.9f, 0.9f, 0.9f, 1.0f };
121  const GLfloat pfShininess[] = { 100.0f };
122  const GLfloat pfEmission[] = { 0.0f, 0.0f, 0.0f, 1.0f };
123  glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, pfColor);
124  glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, pfSpecular);
125  glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS, pfShininess);
126  glMaterialfv(GL_FRONT_AND_BACK, GL_EMISSION, pfEmission);
127  }
128 
129  /****************************************/
130  /****************************************/
131 
133  const GLfloat pfColor[] = { 1.0f, 0.0f, 0.0f, 1.0f };
134  const GLfloat pfSpecular[] = { 0.9f, 0.9f, 0.9f, 1.0f };
135  const GLfloat pfShininess[] = { 100.0f };
136  const GLfloat pfEmission[] = { 0.0f, 0.0f, 0.0f, 1.0f };
137  glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, pfColor);
138  glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, pfSpecular);
139  glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS, pfShininess);
140  glMaterialfv(GL_FRONT_AND_BACK, GL_EMISSION, pfEmission);
141  }
142 
143  /****************************************/
144  /****************************************/
145 
147  const GLfloat pfColor[] = { 0.0f, 0.0f, 1.0f, 1.0f };
148  const GLfloat pfSpecular[] = { 0.5f, 0.5f, 1.0f, 1.0f };
149  const GLfloat pfShininess[] = { 10.0f };
150  const GLfloat pfEmission[] = { 0.0f, 0.0f, 0.0f, 1.0f };
151  glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, pfColor);
152  glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, pfSpecular);
153  glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS, pfShininess);
154  glMaterialfv(GL_FRONT_AND_BACK, GL_EMISSION, pfEmission);
155  }
156 
157  /****************************************/
158  /****************************************/
159 
160  void CQTOpenGLEPuck::SetLEDMaterial(GLfloat f_red,
161  GLfloat f_green,
162  GLfloat f_blue) {
163  const GLfloat pfColor[] = { f_red, f_green, f_blue, 1.0f };
164  const GLfloat pfSpecular[] = { 0.0f, 0.0f, 0.0f, 1.0f };
165  const GLfloat pfShininess[] = { 0.0f };
166  const GLfloat pfEmission[] = { f_red, f_green, f_blue, 1.0f };
167  glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, pfColor);
168  glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, pfSpecular);
169  glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS, pfShininess);
170  glMaterialfv(GL_FRONT_AND_BACK, GL_EMISSION, pfEmission);
171  }
172 
173  /****************************************/
174  /****************************************/
175 
177  /* Set material */
179  /* Right side */
180  CVector2 cVertex(WHEEL_RADIUS, 0.0f);
181  CRadians cAngle(CRadians::TWO_PI / m_unVertices);
182  CVector3 cNormal(-1.0f, -1.0f, 0.0f);
183  cNormal.Normalize();
184  glBegin(GL_POLYGON);
185  for(GLuint i = 0; i <= m_unVertices; i++) {
186  glNormal3f(cNormal.GetX(), cNormal.GetY(), cNormal.GetZ());
187  glVertex3f(cVertex.GetX(), -HALF_WHEEL_WIDTH, WHEEL_RADIUS + cVertex.GetY());
188  cVertex.Rotate(cAngle);
189  cNormal.RotateY(cAngle);
190  }
191  glEnd();
192  /* Left side */
193  cVertex.Set(WHEEL_RADIUS, 0.0f);
194  cNormal.Set(-1.0f, 1.0f, 0.0f);
195  cNormal.Normalize();
196  cAngle = -cAngle;
197  glBegin(GL_POLYGON);
198  for(GLuint i = 0; i <= m_unVertices; i++) {
199  glNormal3f(cNormal.GetX(), cNormal.GetY(), cNormal.GetZ());
200  glVertex3f(cVertex.GetX(), HALF_WHEEL_WIDTH, WHEEL_RADIUS + cVertex.GetY());
201  cVertex.Rotate(cAngle);
202  cNormal.RotateY(cAngle);
203  }
204  glEnd();
205  /* Tire */
206  cNormal.Set(1.0f, 0.0f, 0.0f);
207  cVertex.Set(WHEEL_RADIUS, 0.0f);
208  cAngle = -cAngle;
209  glBegin(GL_QUAD_STRIP);
210  for(GLuint i = 0; i <= m_unVertices; i++) {
211  glNormal3f(cNormal.GetX(), cNormal.GetY(), cNormal.GetZ());
212  glVertex3f(cVertex.GetX(), -HALF_WHEEL_WIDTH, WHEEL_RADIUS + cVertex.GetY());
213  glVertex3f(cVertex.GetX(), HALF_WHEEL_WIDTH, WHEEL_RADIUS + cVertex.GetY());
214  cVertex.Rotate(cAngle);
215  cNormal.RotateY(cAngle);
216  }
217  glEnd();
218  }
219 
220  /****************************************/
221  /****************************************/
222 
224  /* Set material */
226  /* This part covers the bottom face (parallel to XY) */
227  glBegin(GL_QUADS);
228  /* Bottom face */
229  glNormal3f(0.0f, 0.0f, -1.0f);
230  glVertex3f( HALF_CHASSIS_LENGTH, HALF_CHASSIS_WIDTH, CHASSIS_ELEVATION);
231  glVertex3f( HALF_CHASSIS_LENGTH, -HALF_CHASSIS_WIDTH, CHASSIS_ELEVATION);
232  glVertex3f(-HALF_CHASSIS_LENGTH, -HALF_CHASSIS_WIDTH, CHASSIS_ELEVATION);
233  glVertex3f(-HALF_CHASSIS_LENGTH, HALF_CHASSIS_WIDTH, CHASSIS_ELEVATION);
234  glEnd();
235  /* This part covers the faces (South, East, North, West) */
236  glBegin(GL_QUAD_STRIP);
237  /* Starting side */
238  glNormal3f(-1.0f, 0.0f, 0.0f);
239  glVertex3f(-HALF_CHASSIS_LENGTH, -HALF_CHASSIS_WIDTH, CHASSIS_ELEVATION + WHEEL_DIAMETER);
240  glVertex3f(-HALF_CHASSIS_LENGTH, -HALF_CHASSIS_WIDTH, CHASSIS_ELEVATION);
241  /* South face */
242  glVertex3f( HALF_CHASSIS_LENGTH, -HALF_CHASSIS_WIDTH, CHASSIS_ELEVATION + WHEEL_DIAMETER);
243  glVertex3f( HALF_CHASSIS_LENGTH, -HALF_CHASSIS_WIDTH, CHASSIS_ELEVATION);
244  /* East face */
245  glNormal3f(0.0f, -1.0f, 0.0f);
246  glVertex3f( HALF_CHASSIS_LENGTH, HALF_CHASSIS_WIDTH, CHASSIS_ELEVATION + WHEEL_DIAMETER);
247  glVertex3f( HALF_CHASSIS_LENGTH, HALF_CHASSIS_WIDTH, CHASSIS_ELEVATION);
248  /* North face */
249  glNormal3f(1.0f, 0.0f, 0.0f);
250  glVertex3f(-HALF_CHASSIS_LENGTH, HALF_CHASSIS_WIDTH, CHASSIS_ELEVATION + WHEEL_DIAMETER);
251  glVertex3f(-HALF_CHASSIS_LENGTH, HALF_CHASSIS_WIDTH, CHASSIS_ELEVATION);
252  /* West face */
253  glNormal3f(0.0f, 1.0f, 0.0f);
254  glVertex3f(-HALF_CHASSIS_LENGTH, -HALF_CHASSIS_WIDTH, CHASSIS_ELEVATION + WHEEL_DIAMETER);
255  glVertex3f(-HALF_CHASSIS_LENGTH, -HALF_CHASSIS_WIDTH, CHASSIS_ELEVATION);
256  glEnd();
257  }
258 
259  /****************************************/
260  /****************************************/
261 
263  /* Set material */
265  CVector2 cVertex(BODY_RADIUS, 0.0f);
266  CRadians cAngle(-CRadians::TWO_PI / m_unVertices);
267  /* Bottom part */
268  glBegin(GL_POLYGON);
269  glNormal3f(0.0f, 0.0f, -1.0f);
270  for(GLuint i = 0; i <= m_unVertices; i++) {
271  glVertex3f(cVertex.GetX(), cVertex.GetY(), BODY_ELEVATION);
272  cVertex.Rotate(cAngle);
273  }
274  glEnd();
275  /* Side surface */
276  cAngle = -cAngle;
277  CVector2 cNormal(1.0f, 0.0f);
278  cVertex.Set(BODY_RADIUS, 0.0f);
279  glBegin(GL_QUAD_STRIP);
280  for(GLuint i = 0; i <= m_unVertices; i++) {
281  glNormal3f(cNormal.GetX(), cNormal.GetY(), 0.0f);
282  glVertex3f(cVertex.GetX(), cVertex.GetY(), BODY_ELEVATION + BODY_HEIGHT);
283  glVertex3f(cVertex.GetX(), cVertex.GetY(), BODY_ELEVATION);
284  cVertex.Rotate(cAngle);
285  cNormal.Rotate(cAngle);
286  }
287  glEnd();
288  /* Top part */
289  glBegin(GL_POLYGON);
290  cVertex.Set(LED_UPPER_RING_INNER_RADIUS, 0.0f);
291  glNormal3f(0.0f, 0.0f, 1.0f);
292  for(GLuint i = 0; i <= m_unVertices; i++) {
293  glVertex3f(cVertex.GetX(), cVertex.GetY(), BODY_ELEVATION + BODY_HEIGHT + LED_HEIGHT);
294  cVertex.Rotate(cAngle);
295  }
296  glEnd();
297  /* Triangle to set the direction */
298  SetLEDMaterial(1.0f, 1.0f, 0.0f);
299  glBegin(GL_TRIANGLES);
300  glVertex3f( BODY_RADIUS * 0.7, 0.0f, BODY_ELEVATION + BODY_HEIGHT + LED_HEIGHT + 0.001f);
301  glVertex3f(-BODY_RADIUS * 0.7, BODY_RADIUS * 0.3, BODY_ELEVATION + BODY_HEIGHT + LED_HEIGHT + 0.001f);
302  glVertex3f(-BODY_RADIUS * 0.7, -BODY_RADIUS * 0.3, BODY_ELEVATION + BODY_HEIGHT + LED_HEIGHT + 0.001f);
303  glEnd();
304  }
305 
306  /****************************************/
307  /****************************************/
308 
310  /* Side surface */
311  CVector2 cVertex(BODY_RADIUS, 0.0f);
312  CRadians cAngle(CRadians::TWO_PI / m_unVertices);
313  CVector2 cNormal(1.0f, 0.0f);
314  glBegin(GL_QUAD_STRIP);
315  for(GLuint i = 0; i <= m_unVertices / 8; i++) {
316  glNormal3f(cNormal.GetX(), cNormal.GetY(), 0.0f);
317  glVertex3f(cVertex.GetX(), cVertex.GetY(), LED_ELEVATION + LED_HEIGHT);
318  glVertex3f(cVertex.GetX(), cVertex.GetY(), LED_ELEVATION);
319  cVertex.Rotate(cAngle);
320  cNormal.Rotate(cAngle);
321  }
322  glEnd();
323  /* Top surface */
324  cVertex.Set(BODY_RADIUS, 0.0f);
325  CVector2 cVertex2(LED_UPPER_RING_INNER_RADIUS, 0.0f);
326  glBegin(GL_QUAD_STRIP);
327  glNormal3f(0.0f, 0.0f, 1.0f);
328  for(GLuint i = 0; i <= m_unVertices / 8; i++) {
329  glVertex3f(cVertex2.GetX(), cVertex2.GetY(), BODY_ELEVATION + BODY_HEIGHT + LED_HEIGHT);
330  glVertex3f(cVertex.GetX(), cVertex.GetY(), BODY_ELEVATION + BODY_HEIGHT + LED_HEIGHT);
331  cVertex.Rotate(cAngle);
332  cVertex2.Rotate(cAngle);
333  }
334  glEnd();
335  }
336 
337  /****************************************/
338  /****************************************/
339 
341  public:
342  void ApplyTo(CQTOpenGLWidget& c_visualization,
343  CEPuckEntity& c_entity) {
344  static CQTOpenGLEPuck m_cModel;
345  c_visualization.DrawRays(c_entity.GetControllableEntity());
346  c_visualization.DrawEntity(c_entity.GetEmbodiedEntity());
347  m_cModel.Draw(c_entity);
348  }
349  };
350 
352  public:
353  void ApplyTo(CQTOpenGLWidget& c_visualization,
354  CEPuckEntity& c_entity) {
355  c_visualization.DrawBoundingBox(c_entity.GetEmbodiedEntity());
356  }
357  };
358 
359  REGISTER_QTOPENGL_ENTITY_OPERATION(CQTOpenGLOperationDrawNormal, CQTOpenGLOperationDrawEPuckNormal, CEPuckEntity);
360 
361  REGISTER_QTOPENGL_ENTITY_OPERATION(CQTOpenGLOperationDrawSelected, CQTOpenGLOperationDrawEPuckSelected, CEPuckEntity);
362 
363  /****************************************/
364  /****************************************/
365 
366 }
CLEDEntity & GetLED(UInt32 un_index)
Returns an LED by numeric index.
void RenderWheel()
Renders a wheel.
void RenderLED()
A single LED of the ring.
A 3D vector class.
Definition: vector3.h:29
CVector3 & RotateY(const CRadians &c_angle)
Rotates this vector wrt the y axis.
Definition: vector3.h:245
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector2.h:78
void SetRedPlasticMaterial()
Sets a red plastic material.
void ApplyTo(CQTOpenGLWidget &c_visualization, CEPuckEntity &c_entity)
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector3.h:93
void SetLEDMaterial(GLfloat f_red, GLfloat f_green, GLfloat f_blue)
Sets a colored LED material.
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector3.h:109
static const CRadians TWO_PI
Set to PI * 2.
Definition: angles.h:54
CEmbodiedEntity & GetEmbodiedEntity()
Definition: epuck_entity.h:55
UInt8 GetGreen() const
Returns the green channel of the color.
Definition: color.h:90
const CColor & GetColor() const
Returns the current color of the LED.
Definition: led_entity.h:58
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector2.h:94
It defines the basic type CRadians, used to store an angle value in radians.
Definition: angles.h:42
void SetCircuitBoardMaterial()
Sets a circuit board material.
unsigned int UInt32
32-bit unsigned integer.
Definition: datatypes.h:97
A container of CLEDEntity.
void Set(const Real f_x, const Real f_y, const Real f_z)
Sets the vector contents from Cartesian coordinates.
Definition: vector3.h:143
void RenderBody()
Renders the body.
A 2D vector class.
Definition: vector2.h:25
void ApplyTo(CQTOpenGLWidget &c_visualization, CEPuckEntity &c_entity)
CLEDEquippedEntity & GetLEDEquippedEntity()
Definition: epuck_entity.h:63
UInt8 GetRed() const
Returns the red channel of the color.
Definition: color.h:79
virtual void Draw(CEPuckEntity &c_entity)
The basic color type.
Definition: color.h:25
void SetGreenPlasticMaterial()
Sets a green plastic material.
void DrawEntity(CPositionalEntity &c_entity)
Draws a positional entity.
void DrawBoundingBox(CEmbodiedEntity &c_entity)
Draws the bounding box of an embodied entity.
UInt8 GetBlue() const
Returns the blue channel of the color.
Definition: color.h:101
REGISTER_QTOPENGL_ENTITY_OPERATION(CQTOpenGLOperationDrawNormal, CQTOpenGLOperationDrawEPuckNormal, CEPuckEntity)
void Set(Real f_x, Real f_y)
Sets the vector contents from Cartesian coordinates.
Definition: vector2.h:111
void DrawRays(CControllableEntity &c_entity)
Draws a ray.
void RenderChassis()
Renders the chassis.
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
Real GetZ() const
Returns the z coordinate of this vector.
Definition: vector3.h:125
CVector3 & Normalize()
Normalizes this vector.
Definition: vector3.h:215
CVector2 & Rotate(const CRadians &c_angle)
Rotates this vector by the wanted angle.
Definition: vector2.h:169
CControllableEntity & GetControllableEntity()
Definition: epuck_entity.h:51