Warning: include(php/utility.php): Failed to open stream: No such file or directory in /home/argos/argos3/doc/api/embedded/a00858_source.php on line 2

Warning: include(): Failed opening 'php/utility.php' for inclusion (include_path='.:/usr/lib64/php') in /home/argos/argos3/doc/api/embedded/a00858_source.php on line 2
The ARGoS Website

qtopengl_camera.cpp
Go to the documentation of this file.
1 
7 #include "qtopengl_camera.h"
8 #include <QPoint>
9 #include <argos3/core/utility/math/quaternion.h>
10 #include <argos3/core/utility/logging/argos_log.h>
11 
12 namespace argos {
13 
14  static const Real MOVE_GAIN = 0.005f / ::exp(0.02f);
15  static const Real ROTATE_GAIN = 0.01f / ::exp(-0.02f);
16 
17  /****************************************/
18  /****************************************/
19 
21  if(NodeExists(t_tree, "camera")) {
22  try {
23  TConfigurationNode tCameraNode;
24  tCameraNode = GetNode(t_tree, "camera");
25  TConfigurationNodeIterator itSettingss;
26  SInt32 nIdx;
27  for(itSettingss = itSettingss.begin(&tCameraNode);
28  itSettingss != itSettingss.end();
29  ++itSettingss) {
30  GetNodeAttribute(*itSettingss, "idx", nIdx);
31  if(nIdx >=0 && nIdx <= 11) {
32  m_sSettings[nIdx].Init(*itSettingss);
33  }
34  else {
35  THROW_ARGOSEXCEPTION("Error initializing QTOpenGL camera settings: value given for 'idx' is out of bounds. Value = \"" << nIdx << "\", allowed [0-9].");
36  }
37  }
38  }
39  catch(CARGoSException& ex) {
40  THROW_ARGOSEXCEPTION_NESTED("Error initializing QTOpenGL camera settings", ex);
41  }
42  }
43  }
44 
45  /****************************************/
46  /****************************************/
47 
49  /* Get positional attributes */
50  GetNodeAttribute(t_tree, "position", Position);
51  GetNodeAttribute(t_tree, "look_at", Target);
52  /* Calculate the Forward vector */
53  Forward = (Target - Position).Normalize();
54  /* Was the up vector specified? */
55  if(!NodeAttributeExists(t_tree, "up")) {
56  /* Calculate the Left vector
57  It is located on a plane parallel to XY
58  It is perpendicular to the projection of Forward on that plane
59  */
60  if(Forward.GetX() != 0 || Forward.GetY() != 0) {
61  CVector2 cLeftXY(Forward.GetX(), Forward.GetY());
62  cLeftXY.Perpendicularize();
63  Left.Set(cLeftXY.GetX(), cLeftXY.GetY(), 0.0f);
64  Left.Normalize();
65  }
66  else {
67  LOGERR << "[WARNING] The given camera position is ambiguous, "
68  << "and a standard attitude has been used to calculate it. "
69  << "Consider specifying the \"up\" vector in the .argos file "
70  << "for more precise placement."
71  << std::endl;
72  Left.Set(0.0f, 1.0f, 0.0f);
73  }
74  /* Calculate the Up vector with a cross product */
75  Up = Forward;
77  }
78  else {
79  /* Get Up vector */
80  GetNodeAttribute(t_tree, "up", Up);
81  Up.Normalize();
82  /* Calculate the Left vector with a cross product */
83  Left = Up;
85  }
86  /* Get optional optics parameters */
87  Real fValue;
88  GetNodeAttributeOrDefault<Real>(t_tree, "lens_focal_length", fValue, 20.0f);
89  LensFocalLength = fValue * 0.001f;
91  }
92 
93  /****************************************/
94  /****************************************/
95 
97  /* Rotate around the local Y axis (Left)
98  The rotation matrix corresponding to this rotation is:
99 
100  | Fcos(a)-Usin(a) L Ucos(a)+Fsin(a) |
101 
102  where a is c_angle
103  L is the Left vector (local Y)
104  U is the Up vector (local Z)
105  F is the Forward vector (local X)
106  */
107  /* Calculate the new Up vector, given by:
108  Ucos(a)+Fsin(a)
109  */
110  /* Same, but faster than
111  cNewUp = Up * Cos(c_angle) + Forward * Sin(c_angle);
112  */
113  CVector3 cNewUp(Up);
114  cNewUp *= Cos(c_angle);
115  cNewUp += Forward * Sin(c_angle);
116  /* Check whether the rotation exceeds the limits */
118  /*
119  * The rotation exceeds the limits
120  * The camera Up vector would form an angle bigger than 90 degrees with
121  * the global Z axis
122  */
123  /* We force the Up vector to lie on the XY plane */
124  Up.SetZ(0.0f);
125  Up.Normalize();
126  if(Forward.GetZ() < 0.0f) {
127  /* Forward was looking down, set it to -Z */
128  Forward = -CVector3::Z;
129  }
130  else {
131  /* Forward was looking up, set it to Z */
132  Forward = CVector3::Z;
133  }
134  }
135  else {
136  /* The rotation is OK */
137  Up = cNewUp;
138  Up.Normalize();
139  /* Now calculate the new Forward vector with a cross product
140  NOTE: the Left vector, being the rotation axis, remains
141  unchanged!
142  */
143  Forward = Left;
144  Forward.CrossProduct(Up).Normalize();
145  }
146  }
147 
148  /****************************************/
149  /****************************************/
150 
152  /* Rotate around the local Z axis (Up)
153  The rotation matrix corresponding to this rotation is:
154 
155  | Fcos(a)+Lsin(a) -Fsin(a)+Lcos(a) U |
156 
157  where a is c_angle
158  L is the Left vector (local Y)
159  U is the Up vector (local Z)
160  F is the Forward vector (local X)
161  */
162 
163  /* Calculate the new Forward vector, given by:
164  Fcos(a)+Lsin(a)
165  */
166  /* Same, but faster than
167  Forward = Forward * Cos(c_angle) + Left * Sin(c_angle);
168  */
169  Forward *= Cos(c_angle);
170  Forward += Left * Sin(c_angle);
171  Forward.Normalize();
172  /* Now calculate the new Left vector with a cross product
173  NOTE: the Up vector, being the rotation axis, remains
174  unchanged!
175  */
176  Left = Up;
177  Left.CrossProduct(Forward).Normalize();
178  }
179 
180  /****************************************/
181  /****************************************/
182 
184  /* This rotation is performed around the global Z axis.
185  To this aim, along with the global Z axis we project the
186  Forward and Left axes onto the XY plane and use these as
187  additional axis to build up the rotation matrix.
188  With the matrix, we rotate the projected Forward and Left vectors.
189  We then transform the projected vectors into the final ones.
190  Finally, we cross-product the two vectors to obtain the new Up vector.
191  */
192  if(Forward.GetX() != 0 || Forward.GetY() != 0) {
193  /* Project Forward onto the XY axis */
194  CVector3 cForwardXY(Forward.GetX(), Forward.GetY(), 0.0f);
195  /* Save its length: it will be used to restore the Z coordinate correctly */
196  Real cForwardXYLength = cForwardXY.Length();
197  /* Normalize the projection */
198  cForwardXY.Normalize();
199 
200  /* Project Left onto the XY axis and normalize the result */
201  CVector3 cLeftXY = CVector3::Z;
202  cLeftXY.CrossProduct(cForwardXY).Normalize();
203 
204  /* The rotation matrix corresponding to this rotation is:
205 
206  | Fcos(a)+Lsin(a) -Fsin(a)+Lcos(a) Z |
207 
208  where a is c_angle
209  L is the Left vector projected on XY
210  F is the Forward vector projected on XY
211  Z is the global Z vector
212  */
213 
214  /* Calculate the new cForwardXY vector, given by:
215  Fcos(a)+Lsin(a)
216  We keep the unrotated cForwardXY vector, because we will
217  need it for rotating Left too.
218  */
219  /* Same, but faster than
220  CVector3 cNewForwardXY = cForwardXY * Cos(c_angle) + cLeftXY * Sin(c_angle);
221  */
222  CVector3 cNewForwardXY(cForwardXY);
223  cNewForwardXY *= Cos(c_angle);
224  cNewForwardXY += cLeftXY * Sin(c_angle);
225  cNewForwardXY.Normalize();
226  /* Update Forward from cNewForwardXY: we want to keep the Z coordinate
227  of Forward unchanged cause we rotated around the global Z, but we
228  need to update the X and Y coordinates. Right now, cNewForwardXY has
229  zero Z coordinate and length 1, which means that its X and Y are wrong.
230  To get the right values, we need to scale it back to the length of the
231  projection of Forward onto the XY plane. Once this is done, the X and Y
232  coordinates are right.
233  */
234  /* Scale the vector back to the right length */
235  cNewForwardXY *= cForwardXYLength;
236  /* Finally, update Forward */
237  Forward.SetX(cNewForwardXY.GetX());
238  Forward.SetY(cNewForwardXY.GetY());
239  Forward.Normalize();
240 
241  /* Calculate the new Left vector, given by:
242  -Fsin(a)+Lcos(a)
243  */
244  /* Same, but faster than
245  Left = cLeftXY * Cos(c_angle) - cForwardXY * Sin(c_angle);
246  */
247  Left = cLeftXY;
248  Left *= Cos(c_angle);
249  Left -= cForwardXY * Sin(c_angle);
250  Left.Normalize();
251 
252  /* To calculate the new Up vector, a cross-product is enough */
253  Up = Forward;
254  Up.CrossProduct(Left).Normalize();
255  }
256  }
257 
258  /****************************************/
259  /****************************************/
260 
262  Position += Forward * c_delta.GetX() + Left * c_delta.GetY() + Up * c_delta.GetZ();
263  Target = Position;
264  Target += Forward;
265  }
266 
267  /****************************************/
268  /****************************************/
269 
271  gluLookAt(
272  Position.GetX(),
273  Position.GetY(),
274  Position.GetZ(),
275  Target.GetX(),
276  Target.GetY(),
277  Target.GetZ(),
278  Up.GetX(),
279  Up.GetY(),
280  Up.GetZ());
281  }
282 
283  /****************************************/
284  /****************************************/
285 
287  YFieldOfView = ToDegrees(2.0f * ATan2(0.027f * 0.5f, LensFocalLength));
288  }
289 
290  /****************************************/
291  /****************************************/
292 
294  MotionSensitivity = MOVE_GAIN * ::exp(LensFocalLength);
295  RotationSensitivity = ROTATE_GAIN * ::exp(-LensFocalLength);
296  }
297 
298  /****************************************/
299  /****************************************/
300 
302  m_unActiveSettings(0) {
303  }
304 
305  /****************************************/
306  /****************************************/
307 
309  }
310 
311  /****************************************/
312  /****************************************/
313 
314  void CQTOpenGLCamera::Rotate(const QPoint& c_delta) {
315  m_sSettings[m_unActiveSettings]
316  .RotateUpDown(CRadians(m_sSettings[m_unActiveSettings].RotationSensitivity * c_delta.y()));
317  m_sSettings[m_unActiveSettings]
318  .RotateLeftRight2(CRadians(-m_sSettings[m_unActiveSettings].RotationSensitivity * c_delta.x()));
319  m_sSettings[m_unActiveSettings]
320  .Target = m_sSettings[m_unActiveSettings].Position;
321  m_sSettings[m_unActiveSettings]
322  .Target += m_sSettings[m_unActiveSettings].Forward;
323  }
324 
325  /****************************************/
326  /****************************************/
327 
328  void CQTOpenGLCamera::Move(SInt32 n_forwards_backwards,
329  SInt32 n_sideways,
330  SInt32 n_up_down) {
331  m_sSettings[m_unActiveSettings].Translate(
332  CVector3(m_sSettings[m_unActiveSettings].MotionSensitivity * n_forwards_backwards,
333  m_sSettings[m_unActiveSettings].MotionSensitivity * n_sideways,
334  m_sSettings[m_unActiveSettings].MotionSensitivity * n_up_down));
335  }
336 
337  /****************************************/
338  /****************************************/
339 
340 }
signed int SInt32
32-bit signed integer.
Definition: datatypes.h:93
void Move(SInt32 n_forwards_backwards, SInt32 n_sideways, SInt32 n_up_down)
A 3D vector class.
Definition: vector3.h:29
void Init(TConfigurationNode &t_tree)
Initialize from XML.
void Init(TConfigurationNode &t_tree)
CDegrees ToDegrees(const CRadians &c_radians)
Converts CRadians to CDegrees.
Definition: angles.h:489
void RotateLeftRight(const CRadians &c_angle)
Rotation around the local Z axis.
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
CRadians ATan2(const Real f_y, const Real f_x)
Computes the arctangent of the passed values.
Definition: angles.h:633
#define THROW_ARGOSEXCEPTION(message)
This macro throws an ARGoS exception with the passed message.
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector3.h:93
void CalculateSensitivity()
Calculate the sensitivity of the camera.
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector3.h:109
TConfigurationNode & GetNode(TConfigurationNode &t_node, const std::string &str_tag)
Given a tree root node, returns the first of its child nodes with the wanted name.
CVector3 Up
The local Z axis of the camera in the global reference frame.
Real Cos(const CRadians &c_radians)
Computes the cosine of the passed value in radians.
Definition: angles.h:595
void RotateLeftRight2(const CRadians &c_angle)
Rotation around the global Z axis.
CVector3 Position
The position of the camera in the global reference frame.
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
CVector2 & Perpendicularize()
Transforms this vector into its ortogonal.
Definition: vector2.h:222
#define THROW_ARGOSEXCEPTION_NESTED(message, nested)
This macro throws an ARGoS exception with the passed message and nesting the passed exception...
It defines the basic type CRadians, used to store an angle value in radians.
Definition: angles.h:42
Real Length() const
Returns the length of this vector.
Definition: vector3.h:205
void Set(const Real f_x, const Real f_y, const Real f_z)
Sets the vector contents from Cartesian coordinates.
Definition: vector3.h:143
A 2D vector class.
Definition: vector2.h:25
void SetX(const Real f_x)
Sets the x coordinate of this vector.
Definition: vector3.h:101
void Do()
Places this camera in the right position.
bool NodeExists(TConfigurationNode &t_node, const std::string &str_tag)
Given a tree root node, returns true if one of its child nodes has the wanted name.
bool NodeAttributeExists(TConfigurationNode &t_node, const std::string &str_attribute)
Returns true if the specified attribute of a node exists.
Real Sin(const CRadians &c_radians)
Computes the sine of the passed value in radians.
Definition: angles.h:586
void Rotate(const QPoint &c_delta)
CARGoSLog LOGERR(std::cerr, SLogColor(ARGOS_LOG_ATTRIBUTE_BRIGHT, ARGOS_LOG_COLOR_RED))
Definition: argos_log.h:180
ticpp::Iterator< ticpp::Element > TConfigurationNodeIterator
The iterator for the ARGoS configuration XML node.
void GetNodeAttribute(TConfigurationNode &t_node, const std::string &str_attribute, T &t_buffer)
Returns the value of a node's attribute.
The exception that wraps all errors in ARGoS.
void Translate(const CVector3 &c_delta)
c_delta is expressed in the camera local coordinates
static const CVector3 Z
The z axis.
Definition: vector3.h:40
CVector3 Forward
The local X axis of the camera in the global reference frame.
Real LensFocalLength
The focal length of the lens (if this was a real camera)
CRadians GetAngleWith(const CVector3 &c_other)
Returns the angle between this vector and the passed vector.
Definition: vector3.h:313
CVector3 & CrossProduct(const CVector3 &c_vector3)
Calculates the cross product between this vector and the passed one.
Definition: vector3.h:361
void RotateUpDown(const CRadians &c_angle)
Rotation around the local Y axis.
CVector3 Target
The direction of sight of the camera in the global reference frame.
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
Real GetZ() const
Returns the z coordinate of this vector.
Definition: vector3.h:125
void CalculateYFieldOfView()
Calculates the value of YFieldOfView.
CVector3 & Normalize()
Normalizes this vector.
Definition: vector3.h:215
CVector3 Left
The local Y axis of the camera in the global reference frame.
static const CRadians PI_OVER_TWO
Set to PI / 2.
Definition: angles.h:59