ARGoS  3
A parallel, multi-engine simulator for swarm robotics
plugins/simulator/visualizations/qt-opengl/qtopengl_camera.cpp
Go to the documentation of this file.
00001 
00007 #include "qtopengl_camera.h"
00008 #include <QPoint>
00009 #include <argos3/core/utility/math/quaternion.h>
00010 #include <argos3/core/utility/logging/argos_log.h>
00011 
00012 namespace argos {
00013 
00014    static const Real MOVE_GAIN = 0.005f / ::exp(0.02f);
00015    static const Real ROTATE_GAIN = 0.01f / ::exp(-0.02f);
00016 
00017    /****************************************/
00018    /****************************************/
00019 
00020    void CQTOpenGLCamera::Init(TConfigurationNode& t_tree) {
00021       if(NodeExists(t_tree, "camera")) {
00022          try {
00023             TConfigurationNode tCameraNode;
00024             tCameraNode = GetNode(t_tree, "camera");
00025             TConfigurationNodeIterator itSettingss;
00026             SInt32 nIdx;
00027             for(itSettingss = itSettingss.begin(&tCameraNode);
00028                 itSettingss != itSettingss.end();
00029                 ++itSettingss) {
00030                GetNodeAttribute(*itSettingss, "idx", nIdx);
00031                if(nIdx >=0 && nIdx <= 11) {
00032                   m_sSettings[nIdx].Init(*itSettingss);
00033                }
00034                else {
00035                   THROW_ARGOSEXCEPTION("Error initializing QTOpenGL camera settings: value given for 'idx' is out of bounds. Value = \"" << nIdx << "\", allowed [0-9].");
00036                }
00037             }
00038          }
00039          catch(CARGoSException& ex) {
00040             THROW_ARGOSEXCEPTION_NESTED("Error initializing QTOpenGL camera settings", ex);
00041          }
00042       }
00043    }
00044 
00045    /****************************************/
00046    /****************************************/
00047 
00048    void CQTOpenGLCamera::SSettings::Init(TConfigurationNode& t_tree) {
00049       /* Get positional attributes */
00050       GetNodeAttribute(t_tree, "position", Position);
00051       GetNodeAttribute(t_tree, "look_at", Target);
00052       /* Calculate the Forward vector */
00053       Forward = (Target - Position).Normalize();
00054       /* Calculate the Left vector
00055          It is located on a plane parallel to XY
00056          It is perpendicular to the projection of Forward on that plane
00057       */
00058       if(Forward.GetX() != 0 && Forward.GetY() != 0) {
00059          CVector2 cLeftXY(Forward.GetX(), Forward.GetY());
00060          cLeftXY.Perpendicularize();
00061          Left.Set(cLeftXY.GetX(), cLeftXY.GetY(), 0.0f);
00062          Left.Normalize();
00063       }
00064       else {
00065          Left.Set(0.0f, 1.0f, 0.0f);
00066       }
00067       /* Calculate the Up vector with a cross product */
00068       Up = Forward;
00069       Up.CrossProduct(Left).Normalize();
00070       /* Get optional optics parameters */
00071       Real fValue;
00072       GetNodeAttributeOrDefault<Real>(t_tree, "lens_focal_length", fValue, 20.0f);
00073       LensFocalLength = fValue * 0.001f;
00074       CalculateYFieldOfView();
00075    }
00076 
00077    /****************************************/
00078    /****************************************/
00079 
00080    void CQTOpenGLCamera::SSettings::RotateUpDown(const CRadians& c_angle) {
00081       /* Rotate around the local Y axis (Left)
00082          The rotation matrix corresponding to this rotation is:
00083          
00084          | Fcos(a)-Usin(a)   L   Ucos(a)+Fsin(a) |
00085          
00086          where a is c_angle
00087          L is the Left vector (local Y)
00088          U is the Up vector (local Z)
00089          F is the Forward vector (local X)
00090       */
00091       /* Calculate the new Up vector, given by:
00092          Ucos(a)+Fsin(a)
00093       */
00094       /* Same, but faster than
00095          cNewUp = Up * Cos(c_angle) + Forward * Sin(c_angle);
00096       */
00097       CVector3 cNewUp(Up);
00098       cNewUp *= Cos(c_angle);
00099       cNewUp += Forward * Sin(c_angle);
00100       /* Check whether the rotation exceeds the limits */
00101       if(cNewUp.GetAngleWith(CVector3::Z) > CRadians::PI_OVER_TWO) {
00102          /*
00103           * The rotation exceeds the limits
00104           * The camera Up vector would form an angle bigger than 90 degrees with
00105           * the global Z axis
00106           */
00107          /* We force the Up vector to lie on the XY plane */
00108          Up.SetZ(0.0f);
00109          Up.Normalize();
00110          if(Forward.GetZ() < 0.0f) {
00111             /* Forward was looking down, set it to -Z */
00112             Forward = -CVector3::Z;
00113          }
00114          else {
00115             /* Forward was looking up, set it to Z */
00116             Forward = CVector3::Z;
00117          }
00118       }
00119       else {
00120          /* The rotation is OK */
00121          Up = cNewUp;
00122          Up.Normalize();
00123          /* Now calculate the new Forward vector with a cross product
00124             NOTE: the Left vector, being the rotation axis, remains
00125             unchanged!
00126          */
00127          Forward = Left;
00128          Forward.CrossProduct(Up).Normalize();
00129       }
00130    }
00131 
00132    /****************************************/
00133    /****************************************/
00134 
00135    void CQTOpenGLCamera::SSettings::RotateLeftRight(const CRadians& c_angle) {
00136       /* Rotate around the local Z axis (Up)
00137          The rotation matrix corresponding to this rotation is:
00138          
00139          | Fcos(a)+Lsin(a)   -Fsin(a)+Lcos(a)   U |
00140          
00141          where a is c_angle
00142          L is the Left vector (local Y)
00143          U is the Up vector (local Z)
00144          F is the Forward vector (local X)
00145       */
00146 
00147       /* Calculate the new Forward vector, given by:
00148          Fcos(a)+Lsin(a)
00149       */
00150       /* Same, but faster than
00151          Forward = Forward * Cos(c_angle) + Left * Sin(c_angle);
00152       */
00153       Forward *= Cos(c_angle);
00154       Forward += Left * Sin(c_angle);
00155       Forward.Normalize();
00156       /* Now calculate the new Left vector with a cross product
00157          NOTE: the Up vector, being the rotation axis, remains
00158          unchanged!
00159       */
00160       Left = Up;
00161       Left.CrossProduct(Forward).Normalize();
00162    }
00163 
00164    /****************************************/
00165    /****************************************/
00166 
00167    void CQTOpenGLCamera::SSettings::RotateLeftRight2(const CRadians& c_angle) {
00168       /* This rotation is performed around the global Z axis.
00169          To this aim, along with the global Z axis we project the
00170          Forward and Left axes onto the XY plane and use these as
00171          additional axis to build up the rotation matrix.
00172          With the matrix, we rotate the projected Forward and Left vectors.
00173          We then transform the projected vectors into the final ones.
00174          Finally, we cross-product the two vectors to obtain the new Up vector.
00175       */
00176       if(Forward.GetX() != 0 || Forward.GetY() != 0) {
00177          /* Project Forward onto the XY axis */
00178          CVector3 cForwardXY(Forward.GetX(), Forward.GetY(), 0.0f);
00179          /* Save its length: it will be used to restore the Z coordinate correctly */
00180          Real cForwardXYLength = cForwardXY.Length();
00181          /* Normalize the projection */
00182          cForwardXY.Normalize();
00183          
00184          /* Project Left onto the XY axis and normalize the result */
00185          CVector3 cLeftXY = CVector3::Z;
00186          cLeftXY.CrossProduct(cForwardXY).Normalize();
00187          
00188          /* The rotation matrix corresponding to this rotation is:
00189             
00190             | Fcos(a)+Lsin(a)   -Fsin(a)+Lcos(a)   Z |
00191             
00192             where a is c_angle
00193             L is the Left vector projected on XY
00194             F is the Forward vector projected on XY
00195             Z is the global Z vector
00196          */
00197          
00198          /* Calculate the new cForwardXY vector, given by:
00199             Fcos(a)+Lsin(a)
00200             We keep the unrotated cForwardXY vector, because we will
00201             need it for rotating Left too.
00202          */
00203          /* Same, but faster than
00204             CVector3 cNewForwardXY = cForwardXY * Cos(c_angle) + cLeftXY * Sin(c_angle);
00205          */
00206          CVector3 cNewForwardXY(cForwardXY);
00207          cNewForwardXY *= Cos(c_angle);
00208          cNewForwardXY += cLeftXY * Sin(c_angle);
00209          cNewForwardXY.Normalize();
00210          /* Update Forward from cNewForwardXY: we want to keep the Z coordinate
00211             of Forward unchanged cause we rotated around the global Z, but we
00212             need to update the X and Y coordinates. Right now, cNewForwardXY has
00213             zero Z coordinate and length 1, which means that its X and Y are wrong.
00214             To get the right values, we need to scale it back to the length of the
00215             projection of Forward onto the XY plane. Once this is done, the X and Y
00216             coordinates are right.
00217          */
00218          /* Scale the vector back to the right length */
00219          cNewForwardXY *= cForwardXYLength;
00220          /* Finally, update Forward */
00221          Forward.SetX(cNewForwardXY.GetX());
00222          Forward.SetY(cNewForwardXY.GetY());
00223          Forward.Normalize();
00224          
00225          /* Calculate the new Left vector, given by:
00226             -Fsin(a)+Lcos(a)
00227          */
00228          /* Same, but faster than
00229             Left = cLeftXY * Cos(c_angle) - cForwardXY * Sin(c_angle);
00230          */
00231          Left = cLeftXY;
00232          Left *= Cos(c_angle);
00233          Left -= cForwardXY * Sin(c_angle);
00234          Left.Normalize();
00235          
00236          /* To calculate the new Up vector, a cross-product is enough */
00237          Up = Forward;
00238          Up.CrossProduct(Left).Normalize();
00239       }
00240    }
00241 
00242    /****************************************/
00243    /****************************************/
00244 
00245    void CQTOpenGLCamera::SSettings::Translate(const CVector3& c_delta) {
00246       Position += Forward * c_delta.GetX() + Left * c_delta.GetY() + Up * c_delta.GetZ();
00247       Target = Position;
00248       Target += Forward;
00249    }
00250 
00251    /****************************************/
00252    /****************************************/
00253 
00254    void CQTOpenGLCamera::SSettings::Do() {
00255       gluLookAt(
00256          Position.GetX(),
00257          Position.GetY(),
00258          Position.GetZ(),
00259          Target.GetX(),
00260          Target.GetY(),
00261          Target.GetZ(),
00262          Up.GetX(),
00263          Up.GetY(),
00264          Up.GetZ());
00265    }
00266 
00267    /****************************************/
00268    /****************************************/
00269 
00270    void CQTOpenGLCamera::SSettings::CalculateYFieldOfView() {
00271       YFieldOfView = ToDegrees(2.0f * ATan2(0.027f * 0.5f, LensFocalLength));
00272    }
00273 
00274    /****************************************/
00275    /****************************************/
00276 
00277    void CQTOpenGLCamera::SSettings::CalculateSensitivity() {
00278       MotionSensitivity = MOVE_GAIN * ::exp(LensFocalLength);
00279       RotationSensitivity = ROTATE_GAIN * ::exp(-LensFocalLength);
00280    }
00281 
00282    /****************************************/
00283    /****************************************/
00284 
00285    CQTOpenGLCamera::CQTOpenGLCamera() :
00286       m_unActiveSettings(0) {
00287    }
00288 
00289    /****************************************/
00290    /****************************************/
00291 
00292    CQTOpenGLCamera::~CQTOpenGLCamera() {
00293    }
00294    
00295    /****************************************/
00296    /****************************************/
00297 
00298    void CQTOpenGLCamera::Rotate(const QPoint& c_delta) {
00299       m_sSettings[m_unActiveSettings]
00300          .RotateUpDown(CRadians(m_sSettings[m_unActiveSettings].RotationSensitivity * c_delta.y()));
00301       m_sSettings[m_unActiveSettings]
00302          .RotateLeftRight2(CRadians(-m_sSettings[m_unActiveSettings].RotationSensitivity * c_delta.x()));
00303       m_sSettings[m_unActiveSettings]
00304          .Target = m_sSettings[m_unActiveSettings].Position;
00305       m_sSettings[m_unActiveSettings]
00306          .Target += m_sSettings[m_unActiveSettings].Forward;
00307    }
00308    
00309    /****************************************/
00310    /****************************************/
00311 
00312    CVector3 CQTOpenGLCamera::GetMousePosInWorld(SInt32 n_x,
00313                                                 SInt32 n_y) const {
00314       /* Get current viewport */
00315       GLint nViewport[4];
00316       glGetIntegerv(GL_VIEWPORT, nViewport);
00317       /* Get OpenGL matrices */
00318       GLdouble fModelViewMatrix[16];
00319       GLdouble fProjectionMatrix[16];
00320       glGetDoublev(GL_MODELVIEW_MATRIX, fModelViewMatrix);
00321       glGetDoublev(GL_PROJECTION_MATRIX, fProjectionMatrix);
00322       /*
00323        * Convert mouse position in window into a 3D representation
00324        */
00325       /* The x coordinate stays the same */
00326       GLfloat fWinX = n_x;
00327       /* The y coordinate of the window is top-left; in OpenGL is bottom-left */
00328       GLfloat fWinY = nViewport[3] - n_y;
00329       /* Read the z coordinate from the depth buffer in the back buffer */
00330       GLfloat fWinZ;
00331       glReadBuffer(GL_BACK);
00332       glReadPixels(n_x, (GLint)fWinY, 1, 1, GL_DEPTH_COMPONENT, GL_FLOAT, &fWinZ);
00333       /* Get the actual position in the world */
00334       GLdouble fWorldX, fWorldY, fWorldZ;
00335       gluUnProject(fWinX, fWinY, fWinZ,
00336                    fModelViewMatrix, fProjectionMatrix, nViewport,
00337                    &fWorldX, &fWorldY, &fWorldZ);
00338       /*
00339        * Swap coordinates when creating the ray
00340        * In ARGoS, the up vector is the Z-axis, while in OpenGL it is the Y-axis
00341        */
00342       return CVector3(fWorldX, fWorldZ, fWorldY);
00343    }
00344 
00345    /****************************************/
00346    /****************************************/
00347 
00348    CRay3 CQTOpenGLCamera::ProjectRayFromMousePosIntoWorld(SInt32 n_x,
00349                                                           SInt32 n_y) const {
00350       /* Get current viewport */
00351       GLint nViewport[4];
00352       glGetIntegerv(GL_VIEWPORT, nViewport);
00353       /* Get OpenGL matrices */
00354       GLdouble fModelViewMatrix[16];
00355       GLdouble fProjectionMatrix[16];
00356       glGetDoublev(GL_MODELVIEW_MATRIX, fModelViewMatrix);
00357       glGetDoublev(GL_PROJECTION_MATRIX, fProjectionMatrix);
00358       /*
00359        * Convert mouse position in window into OpenGL representation
00360        */
00361       /* The x coordinate stays the same */
00362       GLfloat fWinX = n_x;
00363       /* The y coordinate of the window is top-left; in OpenGL is bottom-left */
00364       GLfloat fWinY = nViewport[3] - n_y;
00365       /*
00366        * Get the position of the ray start in the world
00367        * The ray starts at the near clipping plane (depth = 0.0f)
00368        */
00369       GLdouble fRayStartX, fRayStartY, fRayStartZ;
00370       gluUnProject(fWinX, fWinY, 0.0f,
00371                    fModelViewMatrix, fProjectionMatrix, nViewport,
00372                    &fRayStartX, &fRayStartY, &fRayStartZ);
00373       /*
00374        * Get the position of the ray end in the world
00375        * The ray starts at the far clipping plane (depth = 1.0f)
00376        */
00377       GLdouble fRayEndX, fRayEndY, fRayEndZ;
00378       gluUnProject(fWinX, fWinY, 1.0f,
00379                    fModelViewMatrix, fProjectionMatrix, nViewport,
00380                    &fRayEndX, &fRayEndY, &fRayEndZ);
00381       return CRay3(CVector3(fRayStartX, fRayStartY, fRayStartZ),
00382                    CVector3(fRayEndX, fRayEndY, fRayEndZ));
00383    }
00384 
00385    /****************************************/
00386    /****************************************/
00387 
00388    void CQTOpenGLCamera::Move(SInt32 n_forwards_backwards,
00389                               SInt32 n_sideways,
00390                               SInt32 n_up_down) {
00391       m_sSettings[m_unActiveSettings].Translate(
00392          CVector3(m_sSettings[m_unActiveSettings].MotionSensitivity * n_forwards_backwards,
00393                   m_sSettings[m_unActiveSettings].MotionSensitivity * n_sideways,
00394                   m_sSettings[m_unActiveSettings].MotionSensitivity * n_up_down));
00395    }
00396 
00397    /****************************************/
00398    /****************************************/
00399 
00400 }