Gazebo Math

API Reference

6.15.1
gz/math/Vector2.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 #ifndef GZ_MATH_VECTOR2_HH_
18 #define GZ_MATH_VECTOR2_HH_
19 
20 #include <algorithm>
21 #include <cmath>
22 #include <limits>
23 
24 #include <gz/math/Helpers.hh>
25 #include <gz/math/config.hh>
26 
27 namespace ignition
28 {
29  namespace math
30  {
31  // Inline bracket to help doxygen filtering.
32  inline namespace IGNITION_MATH_VERSION_NAMESPACE {
33  //
36  template<typename T>
37  class Vector2
38  {
40  public: static const Vector2<T> Zero;
41 
43  public: static const Vector2<T> One;
44 
46  public: static const Vector2 NaN;
47 
49  public: Vector2()
50  {
51  this->data[0] = 0;
52  this->data[1] = 0;
53  }
54 
58  public: Vector2(const T &_x, const T &_y)
59  {
60  this->data[0] = _x;
61  this->data[1] = _y;
62  }
63 
66  public: Vector2(const Vector2<T> &_v)
67  {
68  this->data[0] = _v[0];
69  this->data[1] = _v[1];
70  }
71 
73  public: virtual ~Vector2() {}
74 
77  public: T Sum() const
78  {
79  return this->data[0] + this->data[1];
80  }
81 
85  public: double Distance(const Vector2 &_pt) const
86  {
87  return sqrt((this->data[0]-_pt[0])*(this->data[0]-_pt[0]) +
88  (this->data[1]-_pt[1])*(this->data[1]-_pt[1]));
89  }
90 
93  public: T Length() const
94  {
95  return static_cast<T>(sqrt(this->SquaredLength()));
96  }
97 
100  public: T SquaredLength() const
101  {
102  return
103  this->data[0] * this->data[0] +
104  this->data[1] * this->data[1];
105  }
106 
108  public: void Normalize()
109  {
110  T d = this->Length();
111 
112  if (!equal<T>(d, static_cast<T>(0.0)))
113  {
114  this->data[0] /= d;
115  this->data[1] /= d;
116  }
117  }
118 
121  public: Vector2 Normalized() const
122  {
123  Vector2<T> result = *this;
124  result.Normalize();
125  return result;
126  }
127 
130  public: Vector2 Round()
131  {
132  this->data[0] = static_cast<T>(std::nearbyint(this->data[0]));
133  this->data[1] = static_cast<T>(std::nearbyint(this->data[1]));
134  return *this;
135  }
136 
139  public: Vector2 Rounded() const
140  {
141  Vector2<T> result = *this;
142  result.Round();
143  return result;
144  }
145 
149  public: void Set(T _x, T _y)
150  {
151  this->data[0] = _x;
152  this->data[1] = _y;
153  }
154 
158  public: T Dot(const Vector2<T> &_v) const
159  {
160  return (this->data[0] * _v[0]) + (this->data[1] * _v[1]);
161  }
162 
165  public: Vector2 Abs() const
166  {
167  return Vector2(std::abs(this->data[0]),
168  std::abs(this->data[1]));
169  }
170 
179  public: T AbsDot(const Vector2<T> &_v) const
180  {
181  return std::abs(this->data[0] * _v[0]) +
182  std::abs(this->data[1] * _v[1]);
183  }
184 
186  public: inline void Correct()
187  {
188  // std::isfinite works with floating point values,
189  // need to explicit cast to avoid ambiguity in vc++.
190  if (!std::isfinite(static_cast<double>(this->data[0])))
191  this->data[0] = 0;
192  if (!std::isfinite(static_cast<double>(this->data[1])))
193  this->data[1] = 0;
194  }
195 
199  public: void Max(const Vector2<T> &_v)
200  {
201  this->data[0] = std::max(_v[0], this->data[0]);
202  this->data[1] = std::max(_v[1], this->data[1]);
203  }
204 
208  public: void Min(const Vector2<T> &_v)
209  {
210  this->data[0] = std::min(_v[0], this->data[0]);
211  this->data[1] = std::min(_v[1], this->data[1]);
212  }
213 
216  public: T Max() const
217  {
218  return std::max(this->data[0], this->data[1]);
219  }
220 
223  public: T Min() const
224  {
225  return std::min(this->data[0], this->data[1]);
226  }
227 
231  public: Vector2 &operator=(const Vector2 &_v)
232  {
233  this->data[0] = _v[0];
234  this->data[1] = _v[1];
235 
236  return *this;
237  }
238 
242  public: const Vector2 &operator=(T _v)
243  {
244  this->data[0] = _v;
245  this->data[1] = _v;
246 
247  return *this;
248  }
249 
253  public: Vector2 operator+(const Vector2 &_v) const
254  {
255  return Vector2(this->data[0] + _v[0], this->data[1] + _v[1]);
256  }
257 
260  // \return this
261  public: const Vector2 &operator+=(const Vector2 &_v)
262  {
263  this->data[0] += _v[0];
264  this->data[1] += _v[1];
265 
266  return *this;
267  }
268 
272  public: inline Vector2<T> operator+(const T _s) const
273  {
274  return Vector2<T>(this->data[0] + _s,
275  this->data[1] + _s);
276  }
277 
282  public: friend inline Vector2<T> operator+(const T _s,
283  const Vector2<T> &_v)
284  {
285  return _v + _s;
286  }
287 
291  public: const Vector2<T> &operator+=(const T _s)
292  {
293  this->data[0] += _s;
294  this->data[1] += _s;
295 
296  return *this;
297  }
298 
301  public: inline Vector2 operator-() const
302  {
303  return Vector2(-this->data[0], -this->data[1]);
304  }
305 
309  public: Vector2 operator-(const Vector2 &_v) const
310  {
311  return Vector2(this->data[0] - _v[0], this->data[1] - _v[1]);
312  }
313 
317  public: const Vector2 &operator-=(const Vector2 &_v)
318  {
319  this->data[0] -= _v[0];
320  this->data[1] -= _v[1];
321 
322  return *this;
323  }
324 
328  public: inline Vector2<T> operator-(const T _s) const
329  {
330  return Vector2<T>(this->data[0] - _s,
331  this->data[1] - _s);
332  }
333 
338  public: friend inline Vector2<T> operator-(const T _s,
339  const Vector2<T> &_v)
340  {
341  return {_s - _v.X(), _s - _v.Y()};
342  }
343 
347  public: const Vector2<T> &operator-=(T _s)
348  {
349  this->data[0] -= _s;
350  this->data[1] -= _s;
351 
352  return *this;
353  }
354 
359  public: const Vector2 operator/(const Vector2 &_v) const
360  {
361  return Vector2(this->data[0] / _v[0], this->data[1] / _v[1]);
362  }
363 
368  public: const Vector2 &operator/=(const Vector2 &_v)
369  {
370  this->data[0] /= _v[0];
371  this->data[1] /= _v[1];
372 
373  return *this;
374  }
375 
379  public: const Vector2 operator/(T _v) const
380  {
381  return Vector2(this->data[0] / _v, this->data[1] / _v);
382  }
383 
387  public: const Vector2 &operator/=(T _v)
388  {
389  this->data[0] /= _v;
390  this->data[1] /= _v;
391 
392  return *this;
393  }
394 
398  public: const Vector2 operator*(const Vector2 &_v) const
399  {
400  return Vector2(this->data[0] * _v[0], this->data[1] * _v[1]);
401  }
402 
407  public: const Vector2 &operator*=(const Vector2 &_v)
408  {
409  this->data[0] *= _v[0];
410  this->data[1] *= _v[1];
411 
412  return *this;
413  }
414 
418  public: const Vector2 operator*(T _v) const
419  {
420  return Vector2(this->data[0] * _v, this->data[1] * _v);
421  }
422 
427  public: friend inline const Vector2 operator*(const T _s,
428  const Vector2 &_v)
429  {
430  return Vector2(_v * _s);
431  }
432 
436  public: const Vector2 &operator*=(T _v)
437  {
438  this->data[0] *= _v;
439  this->data[1] *= _v;
440 
441  return *this;
442  }
443 
449  public: bool Equal(const Vector2 &_v, const T &_tol) const
450  {
451  return equal<T>(this->data[0], _v[0], _tol)
452  && equal<T>(this->data[1], _v[1], _tol);
453  }
454 
459  public: bool operator==(const Vector2 &_v) const
460  {
461  return this->Equal(_v, static_cast<T>(1e-6));
462  }
463 
466  public: bool operator!=(const Vector2 &_v) const
467  {
468  return !(*this == _v);
469  }
470 
473  public: bool IsFinite() const
474  {
475  // std::isfinite works with floating point values,
476  // need to explicit cast to avoid ambiguity in vc++.
477  return std::isfinite(static_cast<double>(this->data[0])) &&
478  std::isfinite(static_cast<double>(this->data[1]));
479  }
480 
484  public: T &operator[](const std::size_t _index)
485  {
486  return this->data[clamp(_index, IGN_ZERO_SIZE_T, IGN_ONE_SIZE_T)];
487  }
488 
492  public: T operator[](const std::size_t _index) const
493  {
494  return this->data[clamp(_index, IGN_ZERO_SIZE_T, IGN_ONE_SIZE_T)];
495  }
496 
499  public: inline T X() const
500  {
501  return this->data[0];
502  }
503 
506  public: inline T Y() const
507  {
508  return this->data[1];
509  }
510 
513  public: inline T &X()
514  {
515  return this->data[0];
516  }
517 
520  public: inline T &Y()
521  {
522  return this->data[1];
523  }
524 
527  public: inline void X(const T &_v)
528  {
529  this->data[0] = _v;
530  }
531 
534  public: inline void Y(const T &_v)
535  {
536  this->data[1] = _v;
537  }
538 
543  public: friend std::ostream
544  &operator<<(std::ostream &_out, const Vector2<T> &_pt)
545  {
546  _out << _pt[0] << " " << _pt[1];
547  return _out;
548  }
549 
554  public: bool operator<(const Vector2<T> &_pt) const
555  {
556  return this->data[0] < _pt[0] || this->data[1] < _pt[1];
557  }
558 
563  public: friend std::istream
565  {
566  T x, y;
567  // Skip white spaces
568  _in.setf(std::ios_base::skipws);
569  _in >> x >> y;
570  if (!_in.fail())
571  {
572  _pt.Set(x, y);
573  }
574  return _in;
575  }
576 
578  private: T data[2];
579  };
580 
581  template<typename T>
582  const Vector2<T> Vector2<T>::Zero(0, 0);
583 
584  template<typename T>
585  const Vector2<T> Vector2<T>::One(1, 1);
586 
587  template<typename T>
588  const Vector2<T> Vector2<T>::NaN(
591 
595  }
596  }
597 }
598 #endif
Vector2 operator+(const Vector2 &_v) const
Addition operator.
Definition: gz/math/Vector2.hh:253
virtual ~Vector2()
Destructor.
Definition: gz/math/Vector2.hh:73
const Vector2 operator/(const Vector2 &_v) const
Division operator.
Definition: gz/math/Vector2.hh:359
Vector2 Round()
Round to near whole number, return the result.
Definition: gz/math/Vector2.hh:130
bool operator!=(const Vector2 &_v) const
Not equal to operator.
Definition: gz/math/Vector2.hh:466
Two dimensional (x, y) vector.
Definition: gz/math/Vector2.hh:37
Definition: gz/math/AdditivelySeparableScalarField3.hh:27
friend Vector2< T > operator+(const T _s, const Vector2< T > &_v)
Addition operators.
Definition: gz/math/Vector2.hh:282
const Vector2< T > & operator-=(T _s)
Subtraction assignment operator.
Definition: gz/math/Vector2.hh:347
Vector2 operator-() const
Negation operator.
Definition: gz/math/Vector2.hh:301
double Distance(const Vector2 &_pt) const
Calc distance to the given point.
Definition: gz/math/Vector2.hh:85
Vector2(const T &_x, const T &_y)
Constructor.
Definition: gz/math/Vector2.hh:58
T AbsDot(const Vector2< T > &_v) const
Return the absolute dot product of this vector and another vector. This is similar to the Dot functio...
Definition: gz/math/Vector2.hh:179
const Vector2 & operator*=(T _v)
Multiplication assignment operator.
Definition: gz/math/Vector2.hh:436
T & operator[](const std::size_t _index)
Array subscript operator.
Definition: gz/math/Vector2.hh:484
friend std::istream & operator>>(std::istream &_in, Vector2< T > &_pt)
Stream extraction operator.
Definition: gz/math/Vector2.hh:564
static const Vector2 NaN
math::Vector2(NaN, NaN, NaN)
Definition: gz/math/Vector2.hh:46
Vector2 Rounded() const
Get a rounded version of this vector.
Definition: gz/math/Vector2.hh:139
bool operator==(const Vector2 &_v) const
Equal to operator.
Definition: gz/math/Vector2.hh:459
Vector2< int > Vector2i
Definition: gz/math/Vector2.hh:592
T operator[](const std::size_t _index) const
Const-qualified array subscript operator.
Definition: gz/math/Vector2.hh:492
T Min() const
Get the minimum value in the vector.
Definition: gz/math/Vector2.hh:223
const Vector2 operator*(const Vector2 &_v) const
Multiplication operators.
Definition: gz/math/Vector2.hh:398
bool Equal(const Vector2 &_v, const T &_tol) const
Equality test with tolerance.
Definition: gz/math/Vector2.hh:449
static const Vector2< T > One
math::Vector2(1, 1)
Definition: gz/math/Vector2.hh:43
const Vector2 operator/(T _v) const
Division operator.
Definition: gz/math/Vector2.hh:379
Vector2< double > Vector2d
Definition: gz/math/Vector2.hh:593
T Length() const
Returns the length (magnitude) of the vector.
Definition: gz/math/Vector2.hh:93
Vector2 operator-(const Vector2 &_v) const
Subtraction operator.
Definition: gz/math/Vector2.hh:309
T Max() const
Get the maximum value in the vector.
Definition: gz/math/Vector2.hh:216
T X() const
Return the x value.
Definition: gz/math/Vector2.hh:499
friend std::ostream & operator<<(std::ostream &_out, const Vector2< T > &_pt)
Stream extraction operator.
Definition: gz/math/Vector2.hh:544
bool IsFinite() const
See if a point is finite (e.g., not nan)
Definition: gz/math/Vector2.hh:473
void Min(const Vector2< T > &_v)
Set this vector's components to the minimum of itself and the passed in vector.
Definition: gz/math/Vector2.hh:208
T isfinite(T... args)
T Sum() const
Return the sum of the values.
Definition: gz/math/Vector2.hh:77
Vector2 Abs() const
Get the absolute value of the vector.
Definition: gz/math/Vector2.hh:165
void Normalize()
Normalize the vector length.
Definition: gz/math/Vector2.hh:108
static const Vector2< T > Zero
math::Vector2(0, 0)
Definition: gz/math/Vector2.hh:40
STL class.
Vector2< T > operator-(const T _s) const
Subtraction operators.
Definition: gz/math/Vector2.hh:328
const Vector2 & operator/=(T _v)
Division operator.
Definition: gz/math/Vector2.hh:387
friend Vector2< T > operator-(const T _s, const Vector2< T > &_v)
Subtraction operators.
Definition: gz/math/Vector2.hh:338
T SquaredLength() const
Returns the square of the length (magnitude) of the vector.
Definition: gz/math/Vector2.hh:100
T clamp(T _v, T _min, T _max)
Simple clamping function.
Definition: gz/math/Helpers.hh:406
T setf(T... args)
const Vector2 & operator/=(const Vector2 &_v)
Division operator.
Definition: gz/math/Vector2.hh:368
Vector2< T > operator+(const T _s) const
Addition operators.
Definition: gz/math/Vector2.hh:272
Vector2 Normalized() const
Returns a normalized vector.
Definition: gz/math/Vector2.hh:121
const friend Vector2 operator*(const T _s, const Vector2 &_v)
Scalar left multiplication operators.
Definition: gz/math/Vector2.hh:427
void Set(T _x, T _y)
Set the contents of the vector.
Definition: gz/math/Vector2.hh:149
T Y() const
Return the y value.
Definition: gz/math/Vector2.hh:506
void X(const T &_v)
Set the x value.
Definition: gz/math/Vector2.hh:527
T min(T... args)
Vector2 & operator=(const Vector2 &_v)
Assignment operator.
Definition: gz/math/Vector2.hh:231
T fail(T... args)
void Correct()
Corrects any nan values.
Definition: gz/math/Vector2.hh:186
Vector2< float > Vector2f
Definition: gz/math/Vector2.hh:594
void Max(const Vector2< T > &_v)
Set this vector's components to the maximum of itself and the passed in vector.
Definition: gz/math/Vector2.hh:199
static const size_t IGN_ZERO_SIZE_T
size_t type with a value of 0
Definition: gz/math/Helpers.hh:227
T nearbyint(T... args)
static const size_t IGN_ONE_SIZE_T
size_t type with a value of 1
Definition: gz/math/Helpers.hh:230
const Vector2 & operator=(T _v)
Assignment operator.
Definition: gz/math/Vector2.hh:242
Vector2()
Default Constructor.
Definition: gz/math/Vector2.hh:49
const Vector2 & operator-=(const Vector2 &_v)
Subtraction assignment operator.
Definition: gz/math/Vector2.hh:317
T Dot(const Vector2< T > &_v) const
Get the dot product of this vector and _v.
Definition: gz/math/Vector2.hh:158
void Y(const T &_v)
Set the y value.
Definition: gz/math/Vector2.hh:534
bool operator<(const Vector2< T > &_pt) const
Less than operator.
Definition: gz/math/Vector2.hh:554
T max(T... args)
T & X()
Return a mutable x value.
Definition: gz/math/Vector2.hh:513
STL class.
const Vector2 & operator*=(const Vector2 &_v)
Multiplication assignment operator.
Definition: gz/math/Vector2.hh:407
T & Y()
Return a mutable y value.
Definition: gz/math/Vector2.hh:520
const Vector2 & operator+=(const Vector2 &_v)
Addition assignment operator.
Definition: gz/math/Vector2.hh:261
const Vector2< T > & operator+=(const T _s)
Addition assignment operator.
Definition: gz/math/Vector2.hh:291
std::ostream & operator<<(std::ostream &_out, const Graph< VV, EE, UndirectedEdge< EE >> &_g)
Partial template specification for undirected edges.
Definition: gz/math/graph/Graph.hh:745
const Vector2 operator*(T _v) const
Multiplication operators.
Definition: gz/math/Vector2.hh:418
Vector2(const Vector2< T > &_v)
Copy constructor.
Definition: gz/math/Vector2.hh:66