ARGoS 3
A parallel, multi-engine simulator for swarm robotics
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
13namespace 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}
REGISTER_QTOPENGL_ENTITY_OPERATION
#define REGISTER_QTOPENGL_ENTITY_OPERATION(ACTION, OPERATION, ENTITY)
Definition qtopengl_widget.h:50
Sqrt
#define Sqrt
Definition general.h:64
Real
float Real
Collects all ARGoS code.
Definition datatypes.h:39
argos
The namespace containing all the ARGoS related code.
Definition ci_actuator.h:12
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::GetControllableEntity
CControllableEntity & GetControllableEntity()
Definition miniquadrotor_entity.h:38
argos::CMiniQuadrotorEntity::GetEmbodiedEntity
CEmbodiedEntity & GetEmbodiedEntity()
Definition miniquadrotor_entity.h:42
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