vector3.h
Go to the documentation of this file.
1 
7 #ifndef VECTOR3_H
8 #define VECTOR3_H
9 
10 namespace argos {
11  class CVector3;
12  class CVector2;
13  class CQuaternion;
14  class CRotationMatrix3;
15 }
16 
17 #include <argos3/core/utility/math/general.h>
18 #include <argos3/core/utility/math/angles.h>
19 #include <argos3/core/utility/math/vector2.h>
20 #include <argos3/core/utility/string_utilities.h>
21 #include <iostream>
22 #include <cmath>
23 
24 namespace argos {
25 
29  class CVector3 {
30 
31  public:
32 
34  static const CVector3 X;
35 
37  static const CVector3 Y;
38 
40  static const CVector3 Z;
41 
43  static const CVector3 ZERO;
44 
45  public:
46 
53  m_fX(0.0),
54  m_fY(0.0),
55  m_fZ(0.0) {
56  }
57 
67  Real f_y,
68  Real f_z) :
69  m_fX(f_x),
70  m_fY(f_y),
71  m_fZ(f_z) {
72  }
73 
83  inline CVector3(Real f_length,
84  const CRadians& c_inclination,
85  const CRadians& c_azimuth) {
86  FromSphericalCoords(f_length, c_inclination, c_azimuth);
87  }
88 
93  inline Real GetX() const {
94  return m_fX;
95  }
96 
101  inline void SetX(const Real f_x) {
102  m_fX = f_x;
103  }
104 
109  inline Real GetY() const {
110  return m_fY;
111  }
112 
117  inline void SetY(const Real f_y) {
118  m_fY = f_y;
119  }
120 
125  inline Real GetZ() const {
126  return m_fZ;
127  }
128 
133  inline void SetZ(const Real f_z) {
134  m_fZ = f_z;
135  }
136 
143  inline void Set(const Real f_x,
144  const Real f_y,
145  const Real f_z) {
146  m_fX = f_x;
147  m_fY = f_y;
148  m_fZ = f_z;
149  }
150 
159  const CRadians& c_inclination,
160  const CRadians& c_azimuth) {
161  Real fInclinationSin, fInclinationCos;
162  Real fAzimuthSin, fAzimuthCos;
163 #ifdef ARGOS_SINCOS
164  SinCos(c_inclination, fInclinationSin, fInclinationCos);
165  SinCos(c_azimuth, fAzimuthSin, fAzimuthCos);
166 #else
167  fInclinationSin = Sin(c_inclination);
168  fInclinationCos = Cos(c_inclination);
169  fAzimuthSin = Sin(c_azimuth);
170  fAzimuthCos = Cos(c_azimuth);
171 #endif
172  m_fX = f_length * fInclinationSin * fAzimuthCos;
173  m_fY = f_length * fInclinationSin * fAzimuthSin;
174  m_fZ = f_length * fInclinationCos;
175  return *this;
176  }
177 
185  inline void ToSphericalCoords(Real& f_radius,
186  CRadians& c_inclination,
187  CRadians& c_azimuth) const {
188  f_radius = Length();
189  c_inclination = ACos(m_fZ / f_radius);
190  c_azimuth = ATan2(m_fY, m_fX);
191  }
192 
197  inline Real SquareLength() const {
198  return Square(m_fX) + Square(m_fY) + Square(m_fZ);
199  }
200 
205  inline Real Length() const {
206  return Sqrt(SquareLength());
207  }
208 
215  inline CVector3& Normalize() {
216  *this /= Length();
217  return *this;
218  }
219 
225  inline CVector3& RotateX(const CRadians& c_angle) {
226  Real fSin, fCos;
227 #ifdef ARGOS_SINCOS
228  SinCos(c_angle, fSin, fCos);
229 #else
230  fSin = Sin(c_angle);
231  fCos = Cos(c_angle);
232 #endif
233  Real fNewY = m_fY * fCos - m_fZ * fSin;
234  Real fNewZ = m_fY * fSin + m_fZ * fCos;
235  m_fY = fNewY;
236  m_fZ = fNewZ;
237  return *this;
238  }
239 
245  inline CVector3& RotateY(const CRadians& c_angle) {
246  Real fSin, fCos;
247 #ifdef ARGOS_SINCOS
248  SinCos(c_angle, fSin, fCos);
249 #else
250  fSin = Sin(c_angle);
251  fCos = Cos(c_angle);
252 #endif
253  Real fNewX = m_fX * fCos + m_fZ * fSin;
254  Real fNewZ = m_fZ * fCos - m_fX * fSin;
255  m_fX = fNewX;
256  m_fZ = fNewZ;
257  return *this;
258  }
259 
265  inline CVector3& RotateZ(const CRadians& c_angle) {
266  Real fSin, fCos;
267 #ifdef ARGOS_SINCOS
268  SinCos(c_angle, fSin, fCos);
269 #else
270  fSin = Sin(c_angle);
271  fCos = Cos(c_angle);
272 #endif
273  Real fNewX = m_fX * fCos - m_fY * fSin;
274  Real fNewY = m_fX * fSin + m_fY * fCos;
275  m_fX = fNewX;
276  m_fY = fNewY;
277  return *this;
278  }
279 
292  inline CVector3& RotateZ(const CVector2& c_vector) {
293  Real fNewX = m_fX * c_vector.GetX() - m_fY * c_vector.GetY();
294  Real fNewY = m_fX * c_vector.GetY() + m_fY * c_vector.GetX();
295  m_fX = fNewX;
296  m_fY = fNewY;
297  return *this;
298  }
299 
306  CVector3& Rotate(const CQuaternion& c_quaternion);
307 
313  inline CRadians GetAngleWith(const CVector3& c_other) {
314  return CRadians(ACos(DotProduct(c_other) /
315  (Length() *
316  c_other.Length())));
317  }
318 
323  inline CRadians GetXAngle() const {
324  return ATan2(m_fZ, m_fY);
325  }
326 
331  inline CRadians GetYAngle() const {
332  return ATan2(m_fX, m_fZ);
333  }
334 
339  inline CRadians GetZAngle() const {
340  return ATan2(m_fY, m_fX);
341  }
342 
348  inline Real DotProduct(const CVector3& c_vector3) const {
349  return
350  m_fX * c_vector3.m_fX +
351  m_fY * c_vector3.m_fY +
352  m_fZ * c_vector3.m_fZ;
353  }
354 
361  inline CVector3& CrossProduct(const CVector3& c_vector3) {
362  Real fNewX, fNewY, fNewZ;
363  fNewX = m_fY * c_vector3.m_fZ - m_fZ * c_vector3.m_fY;
364  fNewY = m_fZ * c_vector3.m_fX - m_fX * c_vector3.m_fZ;
365  fNewZ = m_fX * c_vector3.m_fY - m_fY * c_vector3.m_fX;
366  m_fX = fNewX;
367  m_fY = fNewY;
368  m_fZ = fNewZ;
369  return *this;
370  }
371 
377  inline CVector2& ProjectOntoXY(CVector2& c_proj) const {
378  c_proj.Set(m_fX,m_fY);
379  return c_proj;
380  }
381 
387  inline CVector2& ProjectOntoYZ(CVector2& c_proj) const {
388  c_proj.Set(m_fY,m_fZ);
389  return c_proj;
390  }
391 
397  inline CVector2& ProjectOntoXZ(CVector2& c_proj) const {
398  c_proj.Set(m_fX,m_fZ);
399  return c_proj;
400  }
401 
407  inline CVector3& Negate() {
408  m_fX = -m_fX;
409  m_fY = -m_fY;
410  m_fZ = -m_fZ;
411  return *this;
412  }
413 
421  inline Real operator[](UInt32 un_index) const {
422  switch(un_index) {
423  case 0: return m_fX;
424  case 1: return m_fY;
425  case 2: return m_fZ;
426  default: THROW_ARGOSEXCEPTION("Real Vector3::operator[]: index " << un_index << " out of bounds");
427  }
428  }
429 
437  inline Real& operator[](UInt32 un_index) {
438  switch(un_index) {
439  case 0: return m_fX;
440  case 1: return m_fY;
441  case 2: return m_fZ;
442  default: THROW_ARGOSEXCEPTION("Real Vector3::operator[]: index " << un_index << " out of bounds");
443  }
444  }
445 
452  inline bool operator==(const CVector3& c_vector3) const {
453  return m_fX == c_vector3.m_fX && m_fY == c_vector3.m_fY && m_fZ == c_vector3.m_fZ;
454  }
455 
462  inline bool operator!=(const CVector3& c_vector3) const {
463  return !((*this) == c_vector3);
464  }
465 
473  inline bool operator<(const CVector3& c_vector3) const {
474  return m_fX < c_vector3.m_fX && m_fY < c_vector3.m_fY && m_fZ < c_vector3.m_fZ;
475  }
476 
484  inline bool operator<=(const CVector3& c_vector3) const {
485  return m_fX <= c_vector3.m_fX && m_fY <= c_vector3.m_fY && m_fZ <= c_vector3.m_fZ;
486  }
487 
495  inline bool operator>(const CVector3& c_vector3) const {
496  return m_fX > c_vector3.m_fX && m_fY > c_vector3.m_fY && m_fZ > c_vector3.m_fZ;
497  }
498 
506  inline bool operator>=(const CVector3& c_vector3) const {
507  return m_fX >= c_vector3.m_fX && m_fY >= c_vector3.m_fY && m_fZ >= c_vector3.m_fZ;
508  }
509 
514  inline CVector3 operator-() const {
515  return CVector3(-m_fX, -m_fY, -m_fZ);
516  }
517 
523  inline CVector3& operator+=(const CVector3& c_vector3) {
524  m_fX += c_vector3.m_fX;
525  m_fY += c_vector3.m_fY;
526  m_fZ += c_vector3.m_fZ;
527  return *this;
528  }
529 
535  inline CVector3& operator-=(const CVector3& c_vector3) {
536  m_fX -= c_vector3.m_fX;
537  m_fY -= c_vector3.m_fY;
538  m_fZ -= c_vector3.m_fZ;
539  return *this;
540  }
541 
547  inline CVector3& operator*=(Real f_value) {
548  m_fX *= f_value;
549  m_fY *= f_value;
550  m_fZ *= f_value;
551  return *this;
552  }
553 
559  inline CVector3& operator/=(Real f_value) {
560  m_fX /= f_value;
561  m_fY /= f_value;
562  m_fZ /= f_value;
563  return *this;
564  }
565 
571  inline CVector3 operator+(const CVector3& c_vector3) const {
572  CVector3 cResult(*this);
573  cResult += c_vector3;
574  return cResult;
575  }
576 
582  inline CVector3 operator-(const CVector3& c_vector3) const {
583  CVector3 cResult(*this);
584  cResult -= c_vector3;
585  return cResult;
586  }
587 
593  inline CVector3 operator*(Real f_value) const {
594  CVector3 cResult(*this);
595  cResult *= f_value;
596  return cResult;
597  }
598 
604  inline CVector3 operator/(const Real f_value) const {
605  CVector3 cResult(*this);
606  cResult /= f_value;
607  return cResult;
608  }
609 
616  inline friend CVector3 operator*(Real f_value,
617  const CVector3& c_vector3) {
618  return c_vector3 * f_value;
619  }
620 
627  inline friend std::ostream& operator<<(std::ostream& c_os,
628  const CVector3& c_vector3) {
629  c_os << c_vector3.m_fX << ","
630  << c_vector3.m_fY << ","
631  << c_vector3.m_fZ;
632  return c_os;
633  }
634 
641  inline friend std::istream& operator>>(std::istream& c_is,
642  CVector3& c_vector3) {
643  Real fValues[3];
644  ParseValues<Real>(c_is, 3, fValues, ',');
645  c_vector3.Set(fValues[0], fValues[1], fValues[2]);
646  return c_is;
647  }
648 
649  private:
650 
652  Real m_fX;
653 
655  Real m_fY;
656 
658  Real m_fZ;
659 
660  friend class CRotationMatrix3;
662 
663  };
664 
665  /****************************************/
666  /****************************************/
667 
674  inline Real SquareDistance(const CVector3& c_v1, const CVector3& c_v2) {
675  return (c_v1 - c_v2).SquareLength();
676  }
677 
684  inline Real Distance(const CVector3& c_v1, const CVector3& c_v2) {
685  return (c_v1 - c_v2).Length();
686  }
687 
688 /****************************************/
689 /****************************************/
690 
691 }
692 
693 #endif
Real Distance(const CVector2 &c_v1, const CVector2 &c_v2)
Computes the distance between the passed vectors.
Definition: vector2.h:418
T Square(const T &t_v)
Returns the square of the value of the passed argument.
Definition: general.h:128
CVector3 operator/(const Real f_value) const
Returns a new vector containing the division between this vector and the passed value.
Definition: vector3.h:604
A 3D vector class.
Definition: vector3.h:29
CVector3 operator-() const
Returns a negated copy of this vector.
Definition: vector3.h:514
#define Sqrt
Definition: general.h:64
CVector2 & ProjectOntoXY(CVector2 &c_proj) const
Calculates the projection of this vector onto the xy plane.
Definition: vector3.h:377
CVector3 & RotateY(const CRadians &c_angle)
Rotates this vector wrt the y axis.
Definition: vector3.h:245
CVector3 & RotateZ(const CRadians &c_angle)
Rotates this vector wrt the z axis.
Definition: vector3.h:265
friend CVector3 operator*(Real f_value, const CVector3 &c_vector3)
Returns a new vector containing the multiplication between the passed value and the passed vector...
Definition: vector3.h:616
static const CVector3 X
The x axis.
Definition: vector3.h:34
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: vector2.h:78
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector3.h:93
CVector3 & Rotate(const CQuaternion &c_quaternion)
Rotates this vector by the given quaternion.
Definition: vector3.cpp:25
bool operator>=(const CVector3 &c_vector3) const
Returns true if this vector is greater than or equal to the passed one.
Definition: vector3.h:506
CVector3 & RotateZ(const CVector2 &c_vector)
Rotates this vector wrt the z axis.
Definition: vector3.h:292
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector3.h:109
CVector3 & FromSphericalCoords(Real f_length, const CRadians &c_inclination, const CRadians &c_azimuth)
Sets the vector contents from spherical coordinates.
Definition: vector3.h:158
Real SquareLength() const
Returns the square length of this vector.
Definition: vector3.h:197
Real Cos(const CRadians &c_radians)
Computes the cosine of the passed value in radians.
Definition: angles.h:595
friend std::ostream & operator<<(std::ostream &c_os, const CVector3 &c_vector3)
Serializes the contents of the passed vector onto a stream.
Definition: vector3.h:627
void ToSphericalCoords(Real &f_radius, CRadians &c_inclination, CRadians &c_azimuth) const
Returns the vector contents as spherical coordinates.
Definition: vector3.h:185
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector2.h:94
CRadians GetXAngle() const
Returns the angle between this vector and the x axis.
Definition: vector3.h:323
bool operator<(const CVector3 &c_vector3) const
Returns true if this vector is smaller than the passed one.
Definition: vector3.h:473
CVector3 & operator-=(const CVector3 &c_vector3)
Subtracts the passed vector from this vector.
Definition: vector3.h:535
CVector3 & operator/=(Real f_value)
Divides this vector by the given value.
Definition: vector3.h:559
It defines the basic type CRadians, used to store an angle value in radians.
Definition: angles.h:42
CVector3(Real f_length, const CRadians &c_inclination, const CRadians &c_azimuth)
Class constructor.
Definition: vector3.h:83
Real Length() const
Returns the length of this vector.
Definition: vector3.h:205
CVector2 & ProjectOntoXZ(CVector2 &c_proj) const
Calculates the projection of this vector onto the xz plane.
Definition: vector3.h:397
unsigned int UInt32
32-bit unsigned integer.
Definition: datatypes.h:97
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
CVector3 & operator*=(Real f_value)
Multiplies this vector by the given value.
Definition: vector3.h:547
void SetX(const Real f_x)
Sets the x coordinate of this vector.
Definition: vector3.h:101
Real Sin(const CRadians &c_radians)
Computes the sine of the passed value in radians.
Definition: angles.h:586
CVector3 operator+(const CVector3 &c_vector3) const
Returns a new vector containing the sum between this vector and the passed one.
Definition: vector3.h:571
Real SquareDistance(const CVector2 &c_v1, const CVector2 &c_v2)
Computes the square distance between the passed vectors.
Definition: vector2.h:408
CVector3 & operator+=(const CVector3 &c_vector3)
Sums the passed vector to this vector.
Definition: vector3.h:523
CVector3 & RotateX(const CRadians &c_angle)
Rotates this vector wrt the x axis.
Definition: vector3.h:225
Real & operator[](UInt32 un_index)
Returns a Cartesian coordinate of this vector.
Definition: vector3.h:437
bool operator!=(const CVector3 &c_vector3) const
Returns true if this vector and the passed one are not equal.
Definition: vector3.h:462
CVector3(Real f_x, Real f_y, Real f_z)
Class constructor.
Definition: vector3.h:66
static const CVector3 ZERO
The zero vector (0,0,0)
Definition: vector3.h:43
void SinCos(const CRadians &c_radians, Real &f_sin, Real &f_cos)
Computes the sine and cosine of the passed value in radians.
Definition: angles.h:574
static const CVector3 Z
The z axis.
Definition: vector3.h:40
static const CVector3 Y
The y axis.
Definition: vector3.h:37
void SetY(const Real f_y)
Sets the y coordinate of this vector.
Definition: vector3.h:117
Real DotProduct(const CVector3 &c_vector3) const
Returns the dot product between this vector and the passed one.
Definition: vector3.h:348
CRadians GetAngleWith(const CVector3 &c_other)
Returns the angle between this vector and the passed vector.
Definition: vector3.h:313
CVector3()
Class constructor.
Definition: vector3.h:52
friend std::istream & operator>>(std::istream &c_is, CVector3 &c_vector3)
Deserializes the contents of a stream and stores them into the passed vector.
Definition: vector3.h:641
CVector3 & CrossProduct(const CVector3 &c_vector3)
Calculates the cross product between this vector and the passed one.
Definition: vector3.h:361
CRadians GetYAngle() const
Returns the angle between this vector and the y axis.
Definition: vector3.h:331
bool operator>(const CVector3 &c_vector3) const
Returns true if this vector is greater than the passed one.
Definition: vector3.h:495
void Set(Real f_x, Real f_y)
Sets the vector contents from Cartesian coordinates.
Definition: vector2.h:111
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 SetZ(const Real f_z)
Sets the z coordinate of this vector.
Definition: vector3.h:133
CRadians ACos(Real f_value)
Computes the arccosine of the passed value.
Definition: angles.h:622
CVector3 & Negate()
Negates this vector.
Definition: vector3.h:407
CVector3 & Normalize()
Normalizes this vector.
Definition: vector3.h:215
CVector3 operator*(Real f_value) const
Returns a new vector containing the multiplication between this vector and the passed value...
Definition: vector3.h:593
bool operator<=(const CVector3 &c_vector3) const
Returns true if this vector is smaller than or equal to the passed one.
Definition: vector3.h:484
Real operator[](UInt32 un_index) const
Returns a Cartesian coordinate of this vector.
Definition: vector3.h:421
bool operator==(const CVector3 &c_vector3) const
Returns true if this vector and the passed one are equal.
Definition: vector3.h:452
CVector3 operator-(const CVector3 &c_vector3) const
Returns a new vector containing the subtraction between this vector and the passed one...
Definition: vector3.h:582
CRadians GetZAngle() const
Returns the angle between this vector and the z axis.
Definition: vector3.h:339
CVector2 & ProjectOntoYZ(CVector2 &c_proj) const
Calculates the projection of this vector onto the yz plane.
Definition: vector3.h:387