ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
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 }