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
00050 GetNodeAttribute(t_tree, "position", Position);
00051 GetNodeAttribute(t_tree, "look_at", Target);
00052
00053 Forward = (Target - Position).Normalize();
00054
00055 if(!NodeAttributeExists(t_tree, "up")) {
00056
00057
00058
00059
00060 if(Forward.GetX() != 0 || Forward.GetY() != 0) {
00061 CVector2 cLeftXY(Forward.GetX(), Forward.GetY());
00062 cLeftXY.Perpendicularize();
00063 Left.Set(cLeftXY.GetX(), cLeftXY.GetY(), 0.0f);
00064 Left.Normalize();
00065 }
00066 else {
00067 LOGERR << "[WARNING] The given camera position is ambiguous, "
00068 << "and a standard attitude has been used to calculate it. "
00069 << "Consider specifying the \"up\" vector in the .argos file "
00070 << "for more precise placement."
00071 << std::endl;
00072 Left.Set(0.0f, 1.0f, 0.0f);
00073 }
00074
00075 Up = Forward;
00076 Up.CrossProduct(Left).Normalize();
00077 }
00078 else {
00079
00080 GetNodeAttribute(t_tree, "up", Up);
00081 Up.Normalize();
00082
00083 Left = Up;
00084 Left.CrossProduct(Forward).Normalize();
00085 }
00086
00087 Real fValue;
00088 GetNodeAttributeOrDefault<Real>(t_tree, "lens_focal_length", fValue, 20.0f);
00089 LensFocalLength = fValue * 0.001f;
00090 CalculateYFieldOfView();
00091 }
00092
00093
00094
00095
00096 void CQTOpenGLCamera::SSettings::RotateUpDown(const CRadians& c_angle) {
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113 CVector3 cNewUp(Up);
00114 cNewUp *= Cos(c_angle);
00115 cNewUp += Forward * Sin(c_angle);
00116
00117 if(cNewUp.GetAngleWith(CVector3::Z) > CRadians::PI_OVER_TWO) {
00118
00119
00120
00121
00122
00123
00124 Up.SetZ(0.0f);
00125 Up.Normalize();
00126 if(Forward.GetZ() < 0.0f) {
00127
00128 Forward = -CVector3::Z;
00129 }
00130 else {
00131
00132 Forward = CVector3::Z;
00133 }
00134 }
00135 else {
00136
00137 Up = cNewUp;
00138 Up.Normalize();
00139
00140
00141
00142
00143 Forward = Left;
00144 Forward.CrossProduct(Up).Normalize();
00145 }
00146 }
00147
00148
00149
00150
00151 void CQTOpenGLCamera::SSettings::RotateLeftRight(const CRadians& c_angle) {
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169 Forward *= Cos(c_angle);
00170 Forward += Left * Sin(c_angle);
00171 Forward.Normalize();
00172
00173
00174
00175
00176 Left = Up;
00177 Left.CrossProduct(Forward).Normalize();
00178 }
00179
00180
00181
00182
00183 void CQTOpenGLCamera::SSettings::RotateLeftRight2(const CRadians& c_angle) {
00184
00185
00186
00187
00188
00189
00190
00191
00192 if(Forward.GetX() != 0 || Forward.GetY() != 0) {
00193
00194 CVector3 cForwardXY(Forward.GetX(), Forward.GetY(), 0.0f);
00195
00196 Real cForwardXYLength = cForwardXY.Length();
00197
00198 cForwardXY.Normalize();
00199
00200
00201 CVector3 cLeftXY = CVector3::Z;
00202 cLeftXY.CrossProduct(cForwardXY).Normalize();
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222 CVector3 cNewForwardXY(cForwardXY);
00223 cNewForwardXY *= Cos(c_angle);
00224 cNewForwardXY += cLeftXY * Sin(c_angle);
00225 cNewForwardXY.Normalize();
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235 cNewForwardXY *= cForwardXYLength;
00236
00237 Forward.SetX(cNewForwardXY.GetX());
00238 Forward.SetY(cNewForwardXY.GetY());
00239 Forward.Normalize();
00240
00241
00242
00243
00244
00245
00246
00247 Left = cLeftXY;
00248 Left *= Cos(c_angle);
00249 Left -= cForwardXY * Sin(c_angle);
00250 Left.Normalize();
00251
00252
00253 Up = Forward;
00254 Up.CrossProduct(Left).Normalize();
00255 }
00256 }
00257
00258
00259
00260
00261 void CQTOpenGLCamera::SSettings::Translate(const CVector3& c_delta) {
00262 Position += Forward * c_delta.GetX() + Left * c_delta.GetY() + Up * c_delta.GetZ();
00263 Target = Position;
00264 Target += Forward;
00265 }
00266
00267
00268
00269
00270 void CQTOpenGLCamera::SSettings::Do() {
00271 gluLookAt(
00272 Position.GetX(),
00273 Position.GetY(),
00274 Position.GetZ(),
00275 Target.GetX(),
00276 Target.GetY(),
00277 Target.GetZ(),
00278 Up.GetX(),
00279 Up.GetY(),
00280 Up.GetZ());
00281 }
00282
00283
00284
00285
00286 void CQTOpenGLCamera::SSettings::CalculateYFieldOfView() {
00287 YFieldOfView = ToDegrees(2.0f * ATan2(0.027f * 0.5f, LensFocalLength));
00288 }
00289
00290
00291
00292
00293 void CQTOpenGLCamera::SSettings::CalculateSensitivity() {
00294 MotionSensitivity = MOVE_GAIN * ::exp(LensFocalLength);
00295 RotationSensitivity = ROTATE_GAIN * ::exp(-LensFocalLength);
00296 }
00297
00298
00299
00300
00301 CQTOpenGLCamera::CQTOpenGLCamera() :
00302 m_unActiveSettings(0) {
00303 }
00304
00305
00306
00307
00308 CQTOpenGLCamera::~CQTOpenGLCamera() {
00309 }
00310
00311
00312
00313
00314 void CQTOpenGLCamera::Rotate(const QPoint& c_delta) {
00315 m_sSettings[m_unActiveSettings]
00316 .RotateUpDown(CRadians(m_sSettings[m_unActiveSettings].RotationSensitivity * c_delta.y()));
00317 m_sSettings[m_unActiveSettings]
00318 .RotateLeftRight2(CRadians(-m_sSettings[m_unActiveSettings].RotationSensitivity * c_delta.x()));
00319 m_sSettings[m_unActiveSettings]
00320 .Target = m_sSettings[m_unActiveSettings].Position;
00321 m_sSettings[m_unActiveSettings]
00322 .Target += m_sSettings[m_unActiveSettings].Forward;
00323 }
00324
00325
00326
00327
00328 void CQTOpenGLCamera::Move(SInt32 n_forwards_backwards,
00329 SInt32 n_sideways,
00330 SInt32 n_up_down) {
00331 m_sSettings[m_unActiveSettings].Translate(
00332 CVector3(m_sSettings[m_unActiveSettings].MotionSensitivity * n_forwards_backwards,
00333 m_sSettings[m_unActiveSettings].MotionSensitivity * n_sideways,
00334 m_sSettings[m_unActiveSettings].MotionSensitivity * n_up_down));
00335 }
00336
00337
00338
00339
00340 }