8 #include <argos3/core/utility/math/vector2.h>
9 #include <argos3/core/simulator/entity/embodied_entity.h>
10 #include <argos3/plugins/robots/prototype/simulator/prototype_entity.h>
11 #include <argos3/plugins/robots/prototype/simulator/prototype_link_equipped_entity.h>
12 #include <argos3/plugins/simulator/entities/directional_led_equipped_entity.h>
13 #include <argos3/plugins/simulator/entities/led_equipped_entity.h>
14 #include <argos3/plugins/simulator/entities/magnet_equipped_entity.h>
15 #include <argos3/plugins/simulator/entities/tag_equipped_entity.h>
16 #include <argos3/plugins/simulator/visualizations/qt-opengl/qtopengl_widget.h>
23 static const Real LED_RADIUS = 0.0025f;
24 static const Real FIELD_SCALE_FACTOR = 0.0005f;
26 const GLfloat
SPECULAR[] = { 0.0f, 0.0f, 0.0f, 1.0f };
28 const GLfloat
EMISSION[] = { 0.0f, 0.0f, 0.0f, 1.0f };
36 m_unBaseList = glGenLists(6);
38 m_unBoxList = m_unBaseList;
39 m_unCylinderList = m_unBaseList + 1;
40 m_unSphereList = m_unBaseList + 2;
41 m_unLEDList = m_unBaseList + 3;
42 m_unPoleList = m_unBaseList + 4;
43 m_unTagList = m_unBaseList + 5;
45 glNewList(m_unBoxList, GL_COMPILE);
49 glNewList(m_unCylinderList, GL_COMPILE);
53 glNewList(m_unSphereList, GL_COMPILE);
57 glNewList(m_unLEDList, GL_COMPILE);
61 glNewList(m_unPoleList, GL_COMPILE);
66 glNewList(m_unTagList, GL_COMPILE);
75 glDeleteLists(m_unBaseList, 6);
81 void CQTOpenGLPrototype::MakeLED() {
85 glBegin(GL_TRIANGLE_STRIP);
89 cPoint = LED_RADIUS * cNormal;
90 glNormal3f(cNormal.GetX(), cNormal.GetY(), cNormal.GetZ());
93 cPoint = LED_RADIUS * cNormal;
94 glNormal3f(cNormal.GetX(), cNormal.GetY(), cNormal.GetZ());
97 cPoint = LED_RADIUS * cNormal;
98 glNormal3f(cNormal.GetX(), cNormal.GetY(), cNormal.GetZ());
101 cPoint = LED_RADIUS * cNormal;
102 glNormal3f(cNormal.GetX(), cNormal.GetY(), cNormal.GetZ());
103 glVertex3f(cPoint.
GetX(), cPoint.
GetY(), cPoint.
GetZ());
116 glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, BODY_COLOR);
122 const CVector3& cPosition = pcLink->GetAnchor().Position;
124 const CQuaternion& cOrientation = pcLink->GetAnchor().Orientation;
128 glTranslatef(cPosition.
GetX(), cPosition.
GetY(), cPosition.
GetZ());
129 glRotatef(
ToDegrees(cXAngle).GetValue(), 1.0f, 0.0f, 0.0f);
130 glRotatef(
ToDegrees(cYAngle).GetValue(), 0.0f, 1.0f, 0.0f);
131 glRotatef(
ToDegrees(cZAngle).GetValue(), 0.0f, 0.0f, 1.0f);
132 glScalef(pcLink->GetExtents().GetX(),
133 pcLink->GetExtents().GetY(),
134 pcLink->GetExtents().GetZ());
136 switch(pcLink->GetGeometry()) {
137 case CPrototypeLinkEntity::EGeometry::BOX:
138 glCallList(m_unBoxList);
140 case CPrototypeLinkEntity::EGeometry::CYLINDER:
141 glCallList(m_unCylinderList);
143 case CPrototypeLinkEntity::EGeometry::SPHERE:
144 glCallList(m_unSphereList);
161 GLfloat pfColor[] = { 0.0f, 0.0f, 0.0f, 1.0f };
162 const GLfloat pfSpecular[] = { 0.0f, 0.0f, 0.0f, 1.0f };
163 const GLfloat pfShininess[] = { 100.0f };
164 const GLfloat pfEmission[] = { 0.0f, 0.0f, 0.0f, 1.0f };
165 glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, pfSpecular);
166 glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS, pfShininess);
167 glMaterialfv(GL_FRONT_AND_BACK, GL_EMISSION, pfEmission);
174 const CColor& cColor = s_instance.LED.GetColor();
175 pfColor[0] = cColor.
GetRed() / 255.0f;
176 pfColor[1] = cColor.
GetGreen() / 255.0f;
177 pfColor[2] = cColor.
GetBlue() / 255.0f;
178 glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, pfColor);
180 const CVector3& cPosition = s_instance.LED.GetPosition();
181 glTranslatef(cPosition.
GetX(), cPosition.
GetY(), cPosition.
GetZ());
183 glCallList(m_unLEDList);
189 GLfloat pfColor[] = { 0.0f, 0.0f, 0.0f, 1.0f };
190 const GLfloat pfSpecular[] = { 0.0f, 0.0f, 0.0f, 1.0f };
191 const GLfloat pfShininess[] = { 100.0f };
192 const GLfloat pfEmission[] = { 0.0f, 0.0f, 0.0f, 1.0f };
193 glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, pfSpecular);
194 glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS, pfShininess);
195 glMaterialfv(GL_FRONT_AND_BACK, GL_EMISSION, pfEmission);
200 const CColor& cColor = ps_actuator->LED.GetColor();
201 pfColor[0] = cColor.
GetRed() / 255.0f;
202 pfColor[1] = cColor.
GetGreen() / 255.0f;
203 pfColor[2] = cColor.
GetBlue() / 255.0f;
204 glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, pfColor);
206 const CVector3& cPosition = ps_actuator->LED.GetPosition();
207 glTranslatef(cPosition.
GetX(), cPosition.
GetY(), cPosition.
GetZ());
209 glCallList(m_unLEDList);
220 const CVector3& cTagPosition = s_instance.Tag.GetPosition();
221 const CQuaternion& cTagOrientation = s_instance.Tag.GetOrientation();
223 Real fScaling = s_instance.Tag.GetSideLength();
225 glTranslatef(cTagPosition.
GetX(),
227 cTagPosition.
GetZ());
228 glRotatef(
ToDegrees(cX).GetValue(), 1.0f, 0.0f, 0.0f);
229 glRotatef(
ToDegrees(cY).GetValue(), 0.0f, 1.0f, 0.0f);
230 glRotatef(
ToDegrees(cZ).GetValue(), 0.0f, 0.0f, 1.0f);
231 glScalef(fScaling, fScaling, 1.0f);
232 glCallList(m_unTagList);
243 CRadians cFieldOrientationZ, cFieldOrientationY, cFieldOrientationX;
246 cFieldOrigin = s_instance.Offset;
247 cFieldOrigin.
Rotate(s_instance.Anchor.Orientation);
248 cFieldOrigin += s_instance.Anchor.Position;
249 const CVector3& cField = s_instance.Magnet.GetField();
250 cFieldOrientation = s_instance.Anchor.Orientation *
256 glTranslatef(cFieldOrigin.
GetX(),
258 cFieldOrigin.
GetZ());
259 glRotatef(
ToDegrees(cFieldOrientationX).GetValue(), 1.0f, 0.0f, 0.0f);
260 glRotatef(
ToDegrees(cFieldOrientationY).GetValue(), 0.0f, 1.0f, 0.0f);
261 glRotatef(
ToDegrees(cFieldOrientationZ).GetValue(), 0.0f, 0.0f, 1.0f);
262 glScalef(1.0f, 1.0f, cField.
Length() * FIELD_SCALE_FACTOR);
263 glCallList(m_unPoleList);
273 void CQTOpenGLPrototype::MakeBox() {
274 glEnable(GL_NORMALIZE);
276 glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, SPECULAR);
277 glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS, SHININESS);
278 glMaterialfv(GL_FRONT_AND_BACK, GL_EMISSION, EMISSION);
282 glNormal3f(0.0f, 0.0f, -1.0f);
283 glVertex3f( 0.5f, 0.5f, 0.0f);
284 glVertex3f( 0.5f, -0.5f, 0.0f);
285 glVertex3f(-0.5f, -0.5f, 0.0f);
286 glVertex3f(-0.5f, 0.5f, 0.0f);
288 glNormal3f(0.0f, 0.0f, 1.0f);
289 glVertex3f(-0.5f, -0.5f, 1.0f);
290 glVertex3f( 0.5f, -0.5f, 1.0f);
291 glVertex3f( 0.5f, 0.5f, 1.0f);
292 glVertex3f(-0.5f, 0.5f, 1.0f);
297 glNormal3f(0.0f, -1.0f, 0.0f);
298 glVertex3f(-0.5f, -0.5f, 1.0f);
299 glVertex3f(-0.5f, -0.5f, 0.0f);
300 glVertex3f( 0.5f, -0.5f, 0.0f);
301 glVertex3f( 0.5f, -0.5f, 1.0f);
303 glNormal3f(1.0f, 0.0f, 0.0f);
304 glVertex3f( 0.5f, -0.5f, 1.0f);
305 glVertex3f( 0.5f, -0.5f, 0.0f);
306 glVertex3f( 0.5f, 0.5f, 0.0f);
307 glVertex3f( 0.5f, 0.5f, 1.0f);
309 glNormal3f(0.0f, 1.0f, 0.0f);
310 glVertex3f( 0.5f, 0.5f, 1.0f);
311 glVertex3f( 0.5f, 0.5f, 0.0f);
312 glVertex3f(-0.5f, 0.5f, 0.0f);
313 glVertex3f(-0.5f, 0.5f, 1.0f);
315 glNormal3f(-1.0f, 0.0f, 0.0f);
316 glVertex3f(-0.5f, 0.5f, 1.0f);
317 glVertex3f(-0.5f, 0.5f, 0.0f);
318 glVertex3f(-0.5f, -0.5f, 0.0f);
319 glVertex3f(-0.5f, -0.5f, 1.0f);
321 glDisable(GL_NORMALIZE);
327 void CQTOpenGLPrototype::MakeCylinder() {
328 glEnable(GL_NORMALIZE);
330 glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, SPECULAR);
331 glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS, SHININESS);
332 glMaterialfv(GL_FRONT_AND_BACK, GL_EMISSION, EMISSION);
334 CVector2 cVertex(0.5f, 0.0f);
336 glBegin(GL_QUAD_STRIP);
337 for(GLuint i = 0; i <= m_unVertices; i++) {
338 glNormal3f(cVertex.GetX(), cVertex.GetY(), 0.0f);
339 glVertex3f(cVertex.GetX(), cVertex.GetY(), 1.0f);
340 glVertex3f(cVertex.GetX(), cVertex.GetY(), 0.0f);
341 cVertex.Rotate(cAngle);
345 cVertex.Set(0.5f, 0.0f);
347 glNormal3f(0.0f, 0.0f, 1.0f);
348 for(GLuint i = 0; i <= m_unVertices; i++) {
349 glVertex3f(cVertex.GetX(), cVertex.GetY(), 1.0f);
350 cVertex.Rotate(cAngle);
354 cVertex.Set(0.5f, 0.0f);
357 glNormal3f(0.0f, 0.0f, -1.0f);
358 for(GLuint i = 0; i <= m_unVertices; i++) {
359 glVertex3f(cVertex.GetX(), cVertex.GetY(), 0.0f);
360 cVertex.Rotate(cAngle);
363 glDisable(GL_NORMALIZE);
369 void CQTOpenGLPrototype::MakeSphere() {
370 glEnable(GL_NORMALIZE);
371 glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, SPECULAR);
372 glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS, SHININESS);
373 glMaterialfv(GL_FRONT_AND_BACK, GL_EMISSION, EMISSION);
374 CVector3 cNormal, cPoint;
376 glBegin(GL_TRIANGLE_STRIP);
377 for(CRadians cInclination; cInclination <=
CRadians::PI; cInclination += cSlice) {
379 cPoint.FromSphericalCoords(0.5f, cInclination, cAzimuth);
380 glNormal3f(cPoint.GetX(), cPoint.GetY(), cPoint.GetZ() + 0.5f);
381 glVertex3f(cPoint.GetX(), cPoint.GetY(), cPoint.GetZ() + 0.5f);
382 cPoint.FromSphericalCoords(0.5f, cInclination + cSlice, cAzimuth);
383 glNormal3f(cPoint.GetX(), cPoint.GetY(), cPoint.GetZ() + 0.5f);
384 glVertex3f(cPoint.GetX(), cPoint.GetY(), cPoint.GetZ() + 0.5f);
385 cPoint.FromSphericalCoords(0.5f, cInclination, cAzimuth + cSlice);
386 glNormal3f(cPoint.GetX(), cPoint.GetY(), cPoint.GetZ() + 0.5f);
387 glVertex3f(cPoint.GetX(), cPoint.GetY(), cPoint.GetZ() + 0.5f);
388 cPoint.FromSphericalCoords(0.5f, cInclination + cSlice, cAzimuth + cSlice);
389 glNormal3f(cPoint.GetX(), cPoint.GetY(), cPoint.GetZ() + 0.5f);
390 glVertex3f(cPoint.GetX(), cPoint.GetY(), cPoint.GetZ() + 0.5f);
394 glDisable(GL_NORMALIZE);
400 void CQTOpenGLPrototype::MakeTagTexture() {
401 glGenTextures(1, &m_unTagTex);
402 glBindTexture(GL_TEXTURE_2D, m_unTagTex);
403 glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, 4, 4, 0, GL_RGB, GL_FLOAT, m_fTagTexture);
409 void CQTOpenGLPrototype::MakeTag() {
410 glEnable(GL_NORMALIZE);
411 glDisable(GL_LIGHTING);
412 glEnable(GL_TEXTURE_2D);
413 glBindTexture(GL_TEXTURE_2D, m_unTagTex);
414 glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
415 glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
416 glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
417 glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
419 glNormal3f(0.0f, 0.0f, 1.0f);
420 glTexCoord2f(1.0f, 1.0f); glVertex2f( 0.5f, 0.5f);
421 glTexCoord2f(0.03f, 1.0f); glVertex2f(-0.5f, 0.5f);
422 glTexCoord2f(0.03f, 0.03f); glVertex2f(-0.5f, -0.5f);
423 glTexCoord2f(1.0f, 0.03f); glVertex2f( 0.5f, -0.5f);
425 glDisable(GL_TEXTURE_2D);
426 glEnable(GL_LIGHTING);
427 glDisable(GL_NORMALIZE);
433 void CQTOpenGLPrototype::MakePoles() {
434 glEnable(GL_NORMALIZE);
435 glDisable(GL_LIGHTING);
439 glColor3f(1.0, 0.0, 0.0);
440 glVertex3f(0.0f, 0.0f, 0.0f);
441 glVertex3f(0.0f, 0.0f, 0.5f);
443 glColor3f(0.0, 0.0, 1.0);
444 glVertex3f(0.0f, 0.0f, 0.0f);
445 glVertex3f(0.0f, 0.0f, -0.5f);
448 glEnable(GL_LIGHTING);
449 glDisable(GL_NORMALIZE);
479 REGISTER_ENTITY_OPERATION(CQTOpenGLOperationDrawNormal, CQTOpenGLWidget, CQTOpenGLOperationDrawPrototypeNormal,
void, CPrototypeEntity);
481 REGISTER_ENTITY_OPERATION(CQTOpenGLOperationDrawSelected, CQTOpenGLWidget, CQTOpenGLOperationDrawPrototypeSelected,
void, CPrototypeEntity);
SInstance::TVector & GetInstances()
Returns the tags.
const GLfloat SHININESS[]
CDegrees ToDegrees(const CRadians &c_radians)
Converts CRadians to CDegrees.
CEmbodiedEntity & GetEmbodiedEntity()
virtual ~CQTOpenGLPrototype()
void ApplyTo(CQTOpenGLWidget &c_visualization, CPrototypeEntity &c_entity)
float Real
Collects all ARGoS code.
Real GetX() const
Returns the x coordinate of this vector.
CVector3 & Rotate(const CQuaternion &c_quaternion)
Rotates this vector by the given quaternion.
virtual void DrawEntity(CPrototypeEntity &c_entity)
bool HasControllableEntity() const
REGISTER_ENTITY_OPERATION(CQTOpenGLOperationDrawNormal, CQTOpenGLWidget, CQTOpenGLOperationDrawPrototypeNormal, void, CPrototypeEntity)
Real GetY() const
Returns the y coordinate of this vector.
static const CRadians TWO_PI
Set to PI * 2.
CVector3 & FromSphericalCoords(Real f_length, const CRadians &c_inclination, const CRadians &c_azimuth)
Sets the vector contents from spherical coordinates.
bool HasDirectionalLEDEquippedEntity() const
CMagnetEquippedEntity & GetMagnetEquippedEntity()
UInt8 GetGreen() const
Returns the green channel of the color.
CPrototypeLinkEntity::TVector & GetLinks()
bool HasLEDEquippedEntity() const
A container of CTagEntity.
It defines the basic type CRadians, used to store an angle value in radians.
Real Length() const
Returns the length of this vector.
CDirectionalLEDEquippedEntity & GetDirectionalLEDEquippedEntity()
CControllableEntity & GetControllableEntity()
const GLfloat BODY_COLOR[]
static const CRadians PI
The PI constant.
void ToEulerAngles(CRadians &c_z_angle, CRadians &c_y_angle, CRadians &c_x_angle) const
UInt8 GetRed() const
Returns the red channel of the color.
CPrototypeLinkEquippedEntity & GetLinkEquippedEntity()
static const CVector3 Z
The z axis.
SActuator::TList & GetLEDs()
Returns all the LEDs.
virtual void DrawDevices(CPrototypeEntity &c_entity)
void ApplyTo(CQTOpenGLWidget &c_visualization, CPrototypeEntity &c_entity)
bool HasMagnetEquippedEntity() const
void DrawBoundingBox(CEmbodiedEntity &c_entity)
Draws the bounding box of an embodied entity.
UInt8 GetBlue() const
Returns the blue channel of the color.
bool HasTagEquippedEntity() const
A container of CMagnetEntity.
void DrawRays(CControllableEntity &c_entity)
Draws a ray.
A container of CDirectionalLEDEntity.
The namespace containing all the ARGoS related code.
Real GetZ() const
Returns the z coordinate of this vector.
SInstance::TVector & GetInstances()
Returns all the LEDs.
CTagEquippedEntity & GetTagEquippedEntity()
CLEDEquippedEntity & GetLEDEquippedEntity()
SInstance::TVector & GetInstances()