qtopengl_miniquadrotor.cpp
Go to the documentation of this file.
1 
8 #include "miniquadrotor_entity.h"
9 #include <argos3/core/utility/math/vector2.h>
10 #include <argos3/core/utility/math/vector3.h>
11 #include <argos3/plugins/simulator/visualizations/qt-opengl/qtopengl_widget.h>
12 
13 namespace argos {
14 
15  /****************************************/
16  /****************************************/
17 
18  /* All measures are in meters */
19 
20  static const Real BOARD_HALF_DIAGONAL = 0.029f;
21  static const Real BOARD_HEIGHT = 0.005f;
22 
23  static const Real ARM_HALF_LENGTH = 0.063f;
24  static const Real ARM_HEIGHT = 0.003f;
25  static const Real ARM_HALF_WIDTH = 0.0015f;
26  static const Real ARM_BOTTOM = 0.001f;
27  static const Real ARM_TOP = ARM_BOTTOM + ARM_HEIGHT;
28 
29  static const Real PROPELLER_ROD_RADIUS = 0.013f;
30  static const Real PROPELLER_ROD_HEIGHT = 0.011f;
31  static const Real PROPELLER_ROD_BOTTOM = ARM_TOP;
32  static const Real PROPELLER_ROD_TOP = PROPELLER_ROD_BOTTOM + PROPELLER_ROD_HEIGHT;
33 
34  static const Real PROPELLER_BLADE_RADIUS = 0.041f;
35  static const Real PROPELLER_BLADE_ELEVATION = 0.0105f;
36 
37  static const Real ONE_OVER_SR2 = 1.0f / Sqrt(2.0f);
38 
39  /****************************************/
40  /****************************************/
41 
43  m_unVertices(40) {
44  /* Reserve the needed display lists */
45  m_unLists = glGenLists(1);
46  /* Create the body display list */
47  glNewList(m_unLists, GL_COMPILE);
48  MakeBody();
49  glEndList();
50  }
51 
52  /****************************************/
53  /****************************************/
54 
56  glDeleteLists(m_unLists, 1);
57  }
58 
59  /****************************************/
60  /****************************************/
61 
63  /* Place the body */
64  glCallList(m_unLists);
65  }
66 
67  /****************************************/
68  /****************************************/
69 
71  const GLfloat pfColor[] = { 0.00f, 0.00f, 0.00f, 1.00f };
72  const GLfloat pfSpecular[] = { 0.90f, 0.90f, 0.90f, 1.00f };
73  const GLfloat pfShininess[] = { 100.00f };
74  const GLfloat pfEmission[] = { 0.00f, 0.00f, 0.00f, 1.00f };
75  glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, pfColor);
76  glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, pfSpecular);
77  glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS, pfShininess);
78  glMaterialfv(GL_FRONT_AND_BACK, GL_EMISSION, pfEmission);
79  }
80 
81  /****************************************/
82  /****************************************/
83 
85  const GLfloat pfColor[] = { 0.00f, 1.00f, 0.00f, 1.00f };
86  const GLfloat pfSpecular[] = { 0.90f, 0.90f, 0.90f, 1.00f };
87  const GLfloat pfShininess[] = { 100.00f };
88  const GLfloat pfEmission[] = { 0.00f, 0.00f, 0.00f, 1.00f };
89  glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, pfColor);
90  glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, pfSpecular);
91  glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS, pfShininess);
92  glMaterialfv(GL_FRONT_AND_BACK, GL_EMISSION, pfEmission);
93  }
94 
95  /****************************************/
96  /****************************************/
97 
99  const GLfloat pfColor[] = { 0.75f, 0.75f, 0.75f, 1.00f };
100  const GLfloat pfSpecular[] = { 0.90f, 0.90f, 0.90f, 1.00f };
101  const GLfloat pfShininess[] = { 100.00f };
102  const GLfloat pfEmission[] = { 0.00f, 0.00f, 0.00f, 1.00f };
103  glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, pfColor);
104  glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, pfSpecular);
105  glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS, pfShininess);
106  glMaterialfv(GL_FRONT_AND_BACK, GL_EMISSION, pfEmission);
107  }
108 
109  /****************************************/
110  /****************************************/
111 
113  /* Board */
114  MakeBoard();
115  /* N->S arm */
116  MakeArm();
117  /* W->E arm */
118  glPushMatrix();
119  glRotated(90.0f, 0.0f, 0.0f, 1.0f);
120  MakeArm();
121  glPopMatrix();
122  /* Propellers */
123  /* N propeller */
124  glPushMatrix();
125  glTranslated(0.0f, ARM_HALF_LENGTH - PROPELLER_ROD_RADIUS, 0.0f);
126  MakePropeller();
127  glPopMatrix();
128  /* E propeller */
129  glPushMatrix();
130  glTranslated(ARM_HALF_LENGTH - PROPELLER_ROD_RADIUS, 0.0f, 0.0f);
131  MakePropeller();
132  glPopMatrix();
133  /* S propeller */
134  glPushMatrix();
135  glTranslated(0.0f, -ARM_HALF_LENGTH + PROPELLER_ROD_RADIUS, 0.0f);
136  MakePropeller();
137  glPopMatrix();
138  /* W propeller */
139  glPushMatrix();
140  glTranslated(-ARM_HALF_LENGTH + PROPELLER_ROD_RADIUS, 0.0f, 0.0f);
141  MakePropeller();
142  glPopMatrix();
143  }
144 
145  /****************************************/
146  /****************************************/
147 
150  glBegin(GL_QUADS);
151  /* Bottom face, parallel to XY */
152  glNormal3d(0.0f, 0.0f, -1.0f);
153  glVertex3d( 0.0f, BOARD_HALF_DIAGONAL, 0.0f );
154  glVertex3d( BOARD_HALF_DIAGONAL, 0.0f, 0.0f );
155  glVertex3d( 0.0f, -BOARD_HALF_DIAGONAL, 0.0f );
156  glVertex3d(-BOARD_HALF_DIAGONAL, 0.0f, 0.0f );
157  /* Top face, parallel to XY */
158  glNormal3d(0.0f, 0.0f, 1.0f);
159  glVertex3d( 0.0f, BOARD_HALF_DIAGONAL, BOARD_HEIGHT);
160  glVertex3d(-BOARD_HALF_DIAGONAL, 0.0f, BOARD_HEIGHT);
161  glVertex3d( 0.0f, -BOARD_HALF_DIAGONAL, BOARD_HEIGHT);
162  glVertex3d( BOARD_HALF_DIAGONAL, 0.0f, BOARD_HEIGHT);
163  /* This part covers the faces (NE, SE, SW, NW) */
164  /* NE face */
165  glNormal3d( ONE_OVER_SR2, ONE_OVER_SR2, 0.0f );
166  glVertex3d( 0.0f, BOARD_HALF_DIAGONAL, 0.0f );
167  glVertex3d( 0.0f, BOARD_HALF_DIAGONAL, BOARD_HEIGHT);
168  glVertex3d( BOARD_HALF_DIAGONAL, 0.0f, BOARD_HEIGHT);
169  glVertex3d( BOARD_HALF_DIAGONAL, 0.0f, 0.0f );
170  /* SE face */
171  glNormal3d( ONE_OVER_SR2, -ONE_OVER_SR2, 0.0f );
172  glVertex3d( BOARD_HALF_DIAGONAL, 0.0f, 0.0f );
173  glVertex3d( BOARD_HALF_DIAGONAL, 0.0f, BOARD_HEIGHT);
174  glVertex3d( 0.0f, -BOARD_HALF_DIAGONAL, BOARD_HEIGHT);
175  glVertex3d( 0.0f, -BOARD_HALF_DIAGONAL, 0.0f );
176  /* SW face */
177  glNormal3d( -ONE_OVER_SR2, -ONE_OVER_SR2, 0.0f );
178  glVertex3d( 0.0f, -BOARD_HALF_DIAGONAL, 0.0f );
179  glVertex3d( 0.0f, -BOARD_HALF_DIAGONAL, BOARD_HEIGHT);
180  glVertex3d(-BOARD_HALF_DIAGONAL, 0.0f, BOARD_HEIGHT);
181  glVertex3d(-BOARD_HALF_DIAGONAL, 0.0f, 0.0f );
182  /* NW face */
183  glNormal3d( -ONE_OVER_SR2, ONE_OVER_SR2, 0.0f );
184  glVertex3d(-BOARD_HALF_DIAGONAL, 0.0f, 0.0f );
185  glVertex3d(-BOARD_HALF_DIAGONAL, 0.0f, BOARD_HEIGHT);
186  glVertex3d( 0.0f, BOARD_HALF_DIAGONAL, BOARD_HEIGHT);
187  glVertex3d( 0.0f, BOARD_HALF_DIAGONAL, 0.0f );
188  glEnd();
189  }
190 
191  /****************************************/
192  /****************************************/
193 
195  SetBodyMaterial();
196  glBegin(GL_QUADS);
197  /* Bottom face, parallel to XY */
198  glNormal3d( 0.0f, 0.0f, -1.0f );
199  glVertex3d( ARM_HALF_WIDTH, ARM_HALF_LENGTH, ARM_BOTTOM);
200  glVertex3d( ARM_HALF_WIDTH, -ARM_HALF_LENGTH, ARM_BOTTOM);
201  glVertex3d(-ARM_HALF_WIDTH, -ARM_HALF_LENGTH, ARM_BOTTOM);
202  glVertex3d(-ARM_HALF_WIDTH, ARM_HALF_LENGTH, ARM_BOTTOM);
203  /* Top face, parallel to XY */
204  glNormal3d( 0.0f, 0.0f, 1.0f );
205  glVertex3d( ARM_HALF_WIDTH, ARM_HALF_LENGTH, ARM_TOP );
206  glVertex3d(-ARM_HALF_WIDTH, ARM_HALF_LENGTH, ARM_TOP );
207  glVertex3d(-ARM_HALF_WIDTH, -ARM_HALF_LENGTH, ARM_TOP );
208  glVertex3d( ARM_HALF_WIDTH, -ARM_HALF_LENGTH, ARM_TOP );
209  /* This part covers the faces (N, E, S, W) */
210  /* North face */
211  glNormal3d( 1.0f, 0.0f, 0.0f );
212  glVertex3d( ARM_HALF_WIDTH, ARM_HALF_LENGTH, ARM_TOP );
213  glVertex3d( ARM_HALF_WIDTH, ARM_HALF_LENGTH, ARM_BOTTOM);
214  glVertex3d(-ARM_HALF_WIDTH, ARM_HALF_LENGTH, ARM_BOTTOM);
215  glVertex3d(-ARM_HALF_WIDTH, ARM_HALF_LENGTH, ARM_TOP );
216  /* East face */
217  glNormal3d( 0.0f, 1.0f, 0.0f );
218  glVertex3d( ARM_HALF_WIDTH, ARM_HALF_LENGTH, ARM_TOP );
219  glVertex3d( ARM_HALF_WIDTH, -ARM_HALF_LENGTH, ARM_TOP );
220  glVertex3d( ARM_HALF_WIDTH, -ARM_HALF_LENGTH, ARM_BOTTOM);
221  glVertex3d( ARM_HALF_WIDTH, ARM_HALF_LENGTH, ARM_BOTTOM);
222  /* South face */
223  glNormal3d(-1.0f, 0.0f, 0.0f );
224  glVertex3d( ARM_HALF_WIDTH, -ARM_HALF_LENGTH, ARM_TOP );
225  glVertex3d(-ARM_HALF_WIDTH, -ARM_HALF_LENGTH, ARM_TOP );
226  glVertex3d(-ARM_HALF_WIDTH, -ARM_HALF_LENGTH, ARM_BOTTOM);
227  glVertex3d( ARM_HALF_WIDTH, -ARM_HALF_LENGTH, ARM_BOTTOM);
228  /* West face */
229  glNormal3d( 0.0f, -1.0f, 0.0f );
230  glVertex3d(-ARM_HALF_WIDTH, ARM_HALF_LENGTH, ARM_TOP );
231  glVertex3d(-ARM_HALF_WIDTH, ARM_HALF_LENGTH, ARM_BOTTOM);
232  glVertex3d(-ARM_HALF_WIDTH, -ARM_HALF_LENGTH, ARM_BOTTOM);
233  glVertex3d(-ARM_HALF_WIDTH, -ARM_HALF_LENGTH, ARM_TOP );
234  glEnd();
235  }
236 
237  /****************************************/
238  /****************************************/
239 
241  SetBodyMaterial();
242  /* Rod side surface */
243  CVector2 cNormal(1.0f, 0.0f);
244  CVector2 cVertex;
245  CRadians cAngle(CRadians::TWO_PI / m_unVertices);
246  glBegin(GL_QUAD_STRIP);
247  for(GLuint i = 0; i <= m_unVertices; i++) {
248  glNormal3d(cNormal.GetX(), cNormal.GetY(), 0.0f);
249  cVertex = cNormal;
250  cVertex *= PROPELLER_ROD_RADIUS;
251  glVertex3d(cVertex.GetX(), cVertex.GetY(), PROPELLER_ROD_TOP);
252  glVertex3d(cVertex.GetX(), cVertex.GetY(), PROPELLER_ROD_BOTTOM);
253  cNormal.Rotate(cAngle);
254  }
255  glEnd();
256  /* Rod top disk */
257  cVertex.Set(PROPELLER_ROD_RADIUS, 0.0f);
258  glBegin(GL_POLYGON);
259  glNormal3d(0.0f, 0.0f, 1.0f);
260  for(GLuint i = 0; i <= m_unVertices; i++) {
261  glVertex3d(cVertex.GetX(), cVertex.GetY(), PROPELLER_ROD_TOP);
262  cVertex.Rotate(cAngle);
263  }
264  glEnd();
265  /* Rod bottom disk */
266  cVertex.Set(PROPELLER_ROD_RADIUS, 0.0f);
267  cAngle = -cAngle;
268  glBegin(GL_POLYGON);
269  glNormal3d(0.0f, 0.0f, -1.0f);
270  for(GLuint i = 0; i <= m_unVertices; i++) {
271  glVertex3d(cVertex.GetX(), cVertex.GetY(), PROPELLER_ROD_BOTTOM);
272  cVertex.Rotate(cAngle);
273  }
274  glEnd();
275  /* Rotating blade disks */
277  cVertex.Set(PROPELLER_BLADE_RADIUS, 0.0f);
278  cAngle = -cAngle;
279  glDisable(GL_CULL_FACE);
280  glBegin(GL_POLYGON);
281  glNormal3d(0.0f, 0.0f, 1.0f);
282  for(GLuint i = 0; i <= m_unVertices; i++) {
283  glVertex3d(cVertex.GetX(), cVertex.GetY(), PROPELLER_BLADE_ELEVATION);
284  cVertex.Rotate(cAngle);
285  }
286  glEnd();
287  glEnable(GL_CULL_FACE);
288  }
289 
290  /****************************************/
291  /****************************************/
292 
294  public:
295  void ApplyTo(CQTOpenGLWidget& c_visualization,
296  CMiniQuadrotorEntity& c_entity) {
297  static CQTOpenGLMiniQuadrotor m_cModel;
298  c_visualization.DrawRays(c_entity.GetControllableEntity());
299  c_visualization.DrawEntity(c_entity.GetEmbodiedEntity());
300  m_cModel.Draw(c_entity);
301  }
302  };
303 
305  public:
306  void ApplyTo(CQTOpenGLWidget& c_visualization,
307  CMiniQuadrotorEntity& c_entity) {
308  c_visualization.DrawBoundingBox(c_entity.GetEmbodiedEntity());
309  }
310  };
311 
313 
315 
316  /****************************************/
317  /****************************************/
318 
319 }
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
#define Sqrt
Definition: general.h:64
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
REGISTER_QTOPENGL_ENTITY_OPERATION(CQTOpenGLOperationDrawNormal, CQTOpenGLOperationDrawEPuckNormal, CEPuckEntity)
It defines the basic type CRadians, used to store an angle value in radians.
Definition: angles.h:42
static const CRadians TWO_PI
Set to PI * 2.
Definition: angles.h:54
A 2D vector class.
Definition: vector2.h:27
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector2.h:110
CVector2 & Rotate(const CRadians &c_angle)
Rotates this vector by the wanted angle.
Definition: vector2.h:194
void Set(Real f_x, Real f_y)
Sets the vector contents from Cartesian coordinates.
Definition: vector2.h:127
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector2.h:94
CEmbodiedEntity & GetEmbodiedEntity()
CControllableEntity & GetControllableEntity()
void ApplyTo(CQTOpenGLWidget &c_visualization, CMiniQuadrotorEntity &c_entity)
void ApplyTo(CQTOpenGLWidget &c_visualization, CMiniQuadrotorEntity &c_entity)
void MakePropeller()
One of the four propellers.
virtual void Draw(CMiniQuadrotorEntity &c_entity)
void SetBoardMaterial()
Sets the board material.
void MakeArm()
On of the two trasversal arms to which propellers are attached.
void SetBladeMaterial()
Sets the blade material.
void SetBodyMaterial()
Sets the body material.
void DrawRays(CControllableEntity &c_entity)
Draws a ray.
void DrawEntity(CPositionalEntity &c_entity)
Draws a positional entity.
void DrawBoundingBox(CEmbodiedEntity &c_entity)
Draws the bounding box of an embodied entity.