qtopengl_miniquadrotor.cpp
Go to the documentation of this file.
1 
7 #include "qtopengl_miniquadrotor.h"
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 
42  CQTOpenGLMiniQuadrotor::CQTOpenGLMiniQuadrotor() :
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 
55  CQTOpenGLMiniQuadrotor::~CQTOpenGLMiniQuadrotor() {
56  glDeleteLists(m_unLists, 1);
57  }
58 
59  /****************************************/
60  /****************************************/
61 
62  void CQTOpenGLMiniQuadrotor::Draw(CMiniQuadrotorEntity& c_entity) {
63  /* Place the body */
64  glCallList(m_unLists);
65  }
66 
67  /****************************************/
68  /****************************************/
69 
70  void CQTOpenGLMiniQuadrotor::SetBodyMaterial() {
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 
84  void CQTOpenGLMiniQuadrotor::SetBoardMaterial() {
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 
98  void CQTOpenGLMiniQuadrotor::SetBladeMaterial() {
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 
112  void CQTOpenGLMiniQuadrotor::MakeBody() {
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 
148  void CQTOpenGLMiniQuadrotor::MakeBoard() {
149  SetBoardMaterial();
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 
194  void CQTOpenGLMiniQuadrotor::MakeArm() {
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 
240  void CQTOpenGLMiniQuadrotor::MakePropeller() {
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 */
276  SetBladeMaterial();
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 
293  class CQTOpenGLOperationDrawMiniQuadrotorNormal : public CQTOpenGLOperationDrawNormal {
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 
304  class CQTOpenGLOperationDrawMiniQuadrotorSelected : public CQTOpenGLOperationDrawSelected {
305  public:
306  void ApplyTo(CQTOpenGLWidget& c_visualization,
307  CMiniQuadrotorEntity& c_entity) {
308  c_visualization.DrawBoundingBox(c_entity.GetEmbodiedEntity());
309  }
310  };
311 
312  REGISTER_QTOPENGL_ENTITY_OPERATION(CQTOpenGLOperationDrawNormal, CQTOpenGLOperationDrawMiniQuadrotorNormal, CMiniQuadrotorEntity);
313 
314  REGISTER_QTOPENGL_ENTITY_OPERATION(CQTOpenGLOperationDrawSelected, CQTOpenGLOperationDrawMiniQuadrotorSelected, CMiniQuadrotorEntity);
315 
316  /****************************************/
317  /****************************************/
318 
319 }
Real
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
Sqrt
#define Sqrt
Definition: general.h:64
argos
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
argos::REGISTER_QTOPENGL_ENTITY_OPERATION
REGISTER_QTOPENGL_ENTITY_OPERATION(CQTOpenGLOperationDrawNormal, CQTOpenGLOperationDrawEPuckNormal, CEPuckEntity)
argos::CRadians
It defines the basic type CRadians, used to store an angle value in radians.
Definition: angles.h:42
argos::CRadians::TWO_PI
static const CRadians TWO_PI
Set to PI * 2.
Definition: angles.h:54
argos::CVector2
A 2D vector class.
Definition: vector2.h:27
argos::CVector2::GetY
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector2.h:110
argos::CVector2::Rotate
CVector2 & Rotate(const CRadians &c_angle)
Rotates this vector by the wanted angle.
Definition: vector2.h:194
argos::CVector2::Set
void Set(Real f_x, Real f_y)
Sets the vector contents from Cartesian coordinates.
Definition: vector2.h:127
argos::CVector2::GetX
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector2.h:94
argos::CMiniQuadrotorEntity::GetEmbodiedEntity
CEmbodiedEntity & GetEmbodiedEntity()
Definition: miniquadrotor_entity.h:42
argos::CMiniQuadrotorEntity::GetControllableEntity
CControllableEntity & GetControllableEntity()
Definition: miniquadrotor_entity.h:38
argos::CQTOpenGLOperationDrawMiniQuadrotorNormal::ApplyTo
void ApplyTo(CQTOpenGLWidget &c_visualization, CMiniQuadrotorEntity &c_entity)
Definition: qtopengl_miniquadrotor.cpp:295
argos::CQTOpenGLOperationDrawMiniQuadrotorSelected::ApplyTo
void ApplyTo(CQTOpenGLWidget &c_visualization, CMiniQuadrotorEntity &c_entity)
Definition: qtopengl_miniquadrotor.cpp:306
argos::CQTOpenGLMiniQuadrotor::MakePropeller
void MakePropeller()
One of the four propellers.
Definition: qtopengl_miniquadrotor.cpp:240
argos::CQTOpenGLMiniQuadrotor::Draw
virtual void Draw(CMiniQuadrotorEntity &c_entity)
Definition: qtopengl_miniquadrotor.cpp:62
argos::CQTOpenGLMiniQuadrotor::SetBoardMaterial
void SetBoardMaterial()
Sets the board material.
Definition: qtopengl_miniquadrotor.cpp:84
argos::CQTOpenGLMiniQuadrotor::MakeArm
void MakeArm()
On of the two trasversal arms to which propellers are attached.
Definition: qtopengl_miniquadrotor.cpp:194
argos::CQTOpenGLMiniQuadrotor::SetBladeMaterial
void SetBladeMaterial()
Sets the blade material.
Definition: qtopengl_miniquadrotor.cpp:98
argos::CQTOpenGLMiniQuadrotor::SetBodyMaterial
void SetBodyMaterial()
Sets the body material.
Definition: qtopengl_miniquadrotor.cpp:70
argos::CQTOpenGLWidget::DrawRays
void DrawRays(CControllableEntity &c_entity)
Draws a ray.
Definition: qtopengl_widget.cpp:394
argos::CQTOpenGLWidget::DrawEntity
void DrawEntity(CPositionalEntity &c_entity)
Draws a positional entity.
Definition: qtopengl_widget.cpp:358
argos::CQTOpenGLWidget::DrawBoundingBox
void DrawBoundingBox(CEmbodiedEntity &c_entity)
Draws the bounding box of an embodied entity.
Definition: qtopengl_widget.cpp:428