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>
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;
29 static const Real CHASSIS_ELEVATION = 0.005f;
30 static const Real HALF_CHASSIS_LENGTH = 0.0275f;
31 static const Real HALF_CHASSIS_WIDTH = HALF_INTERWHEEL_DISTANCE - HALF_WHEEL_WIDTH;
33 static const Real BODY_RADIUS = 0.035f;
34 static const Real BODY_ELEVATION = WHEEL_DIAMETER + CHASSIS_ELEVATION;
35 static const Real BODY_HEIGHT = 0.03f;
37 static const Real LED_ELEVATION = BODY_ELEVATION + BODY_HEIGHT;
38 static const Real LED_HEIGHT = 0.01;
39 static const Real LED_UPPER_RING_INNER_RADIUS = 0.8 * BODY_RADIUS;
46 m_fLEDAngleSlice(360.0f / 8.0f) {
48 m_unLists = glGenLists(4);
51 m_unWheelList = m_unLists;
52 m_unChassisList = m_unLists + 1;
53 m_unBodyList = m_unLists + 2;
54 m_unLEDList = m_unLists + 3;
57 glNewList(m_unWheelList, GL_COMPILE);
62 glNewList(m_unBodyList, GL_COMPILE);
67 glNewList(m_unChassisList, GL_COMPILE);
72 glNewList(m_unLEDList, GL_COMPILE);
81 glDeleteLists(m_unLists, 4);
89 glCallList(m_unChassisList);
91 glCallList(m_unBodyList);
94 glTranslatef(0.0f, HALF_INTERWHEEL_DISTANCE, 0.0f);
95 glCallList(m_unWheelList);
98 glTranslatef(0.0f, -HALF_INTERWHEEL_DISTANCE, 0.0f);
99 glCallList(m_unWheelList);
104 for(
UInt32 i = 0; i < 8; i++) {
106 glRotatef(-m_fLEDAngleSlice, 0.0f, 0.0f, 1.0f);
110 glCallList(m_unLEDList);
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);
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);
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);
163 const GLfloat fEmissionFactor = 10.0f;
164 const GLfloat pfColor[] = { f_red, f_green, f_blue, 1.0f };
165 const GLfloat pfSpecular[] = { 0.0f, 0.0f, 0.0f, 1.0f };
166 const GLfloat pfShininess[] = { 0.0f };
167 const GLfloat pfEmission[] = { f_red * fEmissionFactor, f_green * fEmissionFactor, f_blue * fEmissionFactor, 1.0f };
168 glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, pfColor);
169 glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, pfSpecular);
170 glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS, pfShininess);
171 glMaterialfv(GL_FRONT_AND_BACK, GL_EMISSION, pfEmission);
181 CVector2 cVertex(WHEEL_RADIUS, 0.0f);
183 CVector3 cNormal(-1.0f, -1.0f, 0.0f);
186 for(GLuint i = 0; i <= m_unVertices; i++) {
187 glNormal3f(cNormal.
GetX(), cNormal.
GetY(), cNormal.
GetZ());
188 glVertex3f(cVertex.
GetX(), -HALF_WHEEL_WIDTH, WHEEL_RADIUS + cVertex.
GetY());
194 cVertex.
Set(WHEEL_RADIUS, 0.0f);
195 cNormal.
Set(-1.0f, 1.0f, 0.0f);
199 for(GLuint i = 0; i <= m_unVertices; i++) {
200 glNormal3f(cNormal.
GetX(), cNormal.
GetY(), cNormal.
GetZ());
201 glVertex3f(cVertex.
GetX(), HALF_WHEEL_WIDTH, WHEEL_RADIUS + cVertex.
GetY());
207 cNormal.
Set(1.0f, 0.0f, 0.0f);
208 cVertex.
Set(WHEEL_RADIUS, 0.0f);
210 glBegin(GL_QUAD_STRIP);
211 for(GLuint i = 0; i <= m_unVertices; i++) {
212 glNormal3f(cNormal.
GetX(), cNormal.
GetY(), cNormal.
GetZ());
213 glVertex3f(cVertex.
GetX(), -HALF_WHEEL_WIDTH, WHEEL_RADIUS + cVertex.
GetY());
214 glVertex3f(cVertex.
GetX(), HALF_WHEEL_WIDTH, WHEEL_RADIUS + cVertex.
GetY());
230 glNormal3f(0.0f, 0.0f, -1.0f);
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 glVertex3f(-HALF_CHASSIS_LENGTH, HALF_CHASSIS_WIDTH, CHASSIS_ELEVATION);
237 glBegin(GL_QUAD_STRIP);
239 glNormal3f(-1.0f, 0.0f, 0.0f);
240 glVertex3f(-HALF_CHASSIS_LENGTH, -HALF_CHASSIS_WIDTH, CHASSIS_ELEVATION + WHEEL_DIAMETER);
241 glVertex3f(-HALF_CHASSIS_LENGTH, -HALF_CHASSIS_WIDTH, CHASSIS_ELEVATION);
243 glVertex3f( HALF_CHASSIS_LENGTH, -HALF_CHASSIS_WIDTH, CHASSIS_ELEVATION + WHEEL_DIAMETER);
244 glVertex3f( HALF_CHASSIS_LENGTH, -HALF_CHASSIS_WIDTH, CHASSIS_ELEVATION);
246 glNormal3f(0.0f, -1.0f, 0.0f);
247 glVertex3f( HALF_CHASSIS_LENGTH, HALF_CHASSIS_WIDTH, CHASSIS_ELEVATION + WHEEL_DIAMETER);
248 glVertex3f( HALF_CHASSIS_LENGTH, HALF_CHASSIS_WIDTH, CHASSIS_ELEVATION);
250 glNormal3f(1.0f, 0.0f, 0.0f);
251 glVertex3f(-HALF_CHASSIS_LENGTH, HALF_CHASSIS_WIDTH, CHASSIS_ELEVATION + WHEEL_DIAMETER);
252 glVertex3f(-HALF_CHASSIS_LENGTH, HALF_CHASSIS_WIDTH, CHASSIS_ELEVATION);
254 glNormal3f(0.0f, 1.0f, 0.0f);
255 glVertex3f(-HALF_CHASSIS_LENGTH, -HALF_CHASSIS_WIDTH, CHASSIS_ELEVATION + WHEEL_DIAMETER);
256 glVertex3f(-HALF_CHASSIS_LENGTH, -HALF_CHASSIS_WIDTH, CHASSIS_ELEVATION);
266 CVector2 cVertex(BODY_RADIUS, 0.0f);
270 glNormal3f(0.0f, 0.0f, -1.0f);
271 for(GLuint i = 0; i <= m_unVertices; i++) {
272 glVertex3f(cVertex.
GetX(), cVertex.
GetY(), BODY_ELEVATION);
279 cVertex.
Set(BODY_RADIUS, 0.0f);
280 glBegin(GL_QUAD_STRIP);
281 for(GLuint i = 0; i <= m_unVertices; i++) {
282 glNormal3f(cNormal.
GetX(), cNormal.
GetY(), 0.0f);
283 glVertex3f(cVertex.
GetX(), cVertex.
GetY(), BODY_ELEVATION + BODY_HEIGHT);
284 glVertex3f(cVertex.
GetX(), cVertex.
GetY(), BODY_ELEVATION);
291 cVertex.
Set(LED_UPPER_RING_INNER_RADIUS, 0.0f);
292 glNormal3f(0.0f, 0.0f, 1.0f);
293 for(GLuint i = 0; i <= m_unVertices; i++) {
294 glVertex3f(cVertex.
GetX(), cVertex.
GetY(), BODY_ELEVATION + BODY_HEIGHT + LED_HEIGHT);
300 glBegin(GL_TRIANGLES);
301 glVertex3f( BODY_RADIUS * 0.7, 0.0f, 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 glVertex3f(-BODY_RADIUS * 0.7, -BODY_RADIUS * 0.3, BODY_ELEVATION + BODY_HEIGHT + LED_HEIGHT + 0.001f);
312 CVector2 cVertex(BODY_RADIUS, 0.0f);
315 glBegin(GL_QUAD_STRIP);
316 for(GLuint i = 0; i <= m_unVertices / 8; i++) {
317 glNormal3f(cNormal.
GetX(), cNormal.
GetY(), 0.0f);
318 glVertex3f(cVertex.
GetX(), cVertex.
GetY(), LED_ELEVATION + LED_HEIGHT);
319 glVertex3f(cVertex.
GetX(), cVertex.
GetY(), LED_ELEVATION);
325 cVertex.
Set(BODY_RADIUS, 0.0f);
326 CVector2 cVertex2(LED_UPPER_RING_INNER_RADIUS, 0.0f);
327 glBegin(GL_QUAD_STRIP);
328 glNormal3f(0.0f, 0.0f, 1.0f);
329 for(GLuint i = 0; i <= m_unVertices / 8; i++) {
330 glVertex3f(cVertex2.
GetX(), cVertex2.
GetY(), BODY_ELEVATION + BODY_HEIGHT + LED_HEIGHT);
331 glVertex3f(cVertex.
GetX(), cVertex.
GetY(), BODY_ELEVATION + BODY_HEIGHT + LED_HEIGHT);
348 m_cModel.
Draw(c_entity);