Gazebo Math

API Reference

7.5.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 <istream>
23 #include <limits>
24 #include <ostream>
25 
26 #include <gz/math/Helpers.hh>
27 #include <gz/math/config.hh>
28 
29 namespace gz::math
30 {
31  // Inline bracket to help doxygen filtering.
32  inline namespace GZ_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: constexpr Vector2()
50  : data{0, 0}
51  {
52  }
53 
57  public: constexpr Vector2(const T &_x, const T &_y)
58  : data{_x, _y}
59  {
60  }
61 
64  public: Vector2(const Vector2<T> &_v) = default;
65 
67  public: ~Vector2() = default;
68 
71  public: T Sum() const
72  {
73  return this->data[0] + this->data[1];
74  }
75 
79  public: double Distance(const Vector2 &_pt) const
80  {
81  return sqrt((this->data[0]-_pt[0])*(this->data[0]-_pt[0]) +
82  (this->data[1]-_pt[1])*(this->data[1]-_pt[1]));
83  }
84 
87  public: T Length() const
88  {
89  return static_cast<T>(sqrt(this->SquaredLength()));
90  }
91 
94  public: T SquaredLength() const
95  {
96  return
97  this->data[0] * this->data[0] +
98  this->data[1] * this->data[1];
99  }
100 
102  public: void Normalize()
103  {
104  T d = this->Length();
105 
106  if (!equal<T>(d, static_cast<T>(0.0)))
107  {
108  this->data[0] /= d;
109  this->data[1] /= d;
110  }
111  }
112 
115  public: Vector2 Normalized() const
116  {
117  Vector2<T> result = *this;
118  result.Normalize();
119  return result;
120  }
121 
124  public: Vector2 Round()
125  {
126  this->data[0] = static_cast<T>(std::nearbyint(this->data[0]));
127  this->data[1] = static_cast<T>(std::nearbyint(this->data[1]));
128  return *this;
129  }
130 
133  public: Vector2 Rounded() const
134  {
135  Vector2<T> result = *this;
136  result.Round();
137  return result;
138  }
139 
143  public: void Set(T _x, T _y)
144  {
145  this->data[0] = _x;
146  this->data[1] = _y;
147  }
148 
152  public: T Dot(const Vector2<T> &_v) const
153  {
154  return (this->data[0] * _v[0]) + (this->data[1] * _v[1]);
155  }
156 
159  public: Vector2 Abs() const
160  {
161  return Vector2(std::abs(this->data[0]),
162  std::abs(this->data[1]));
163  }
164 
173  public: T AbsDot(const Vector2<T> &_v) const
174  {
175  return std::abs(this->data[0] * _v[0]) +
176  std::abs(this->data[1] * _v[1]);
177  }
178 
180  public: inline void Correct()
181  {
182  // std::isfinite works with floating point values,
183  // need to explicit cast to avoid ambiguity in vc++.
184  if (!std::isfinite(static_cast<double>(this->data[0])))
185  this->data[0] = 0;
186  if (!std::isfinite(static_cast<double>(this->data[1])))
187  this->data[1] = 0;
188  }
189 
193  public: void Max(const Vector2<T> &_v)
194  {
195  this->data[0] = std::max(_v[0], this->data[0]);
196  this->data[1] = std::max(_v[1], this->data[1]);
197  }
198 
202  public: void Min(const Vector2<T> &_v)
203  {
204  this->data[0] = std::min(_v[0], this->data[0]);
205  this->data[1] = std::min(_v[1], this->data[1]);
206  }
207 
210  public: T Max() const
211  {
212  return std::max(this->data[0], this->data[1]);
213  }
214 
217  public: T Min() const
218  {
219  return std::min(this->data[0], this->data[1]);
220  }
221 
225  public: Vector2 &operator=(const Vector2 &_v) = default;
226 
230  public: const Vector2 &operator=(T _v)
231  {
232  this->data[0] = _v;
233  this->data[1] = _v;
234 
235  return *this;
236  }
237 
241  public: Vector2 operator+(const Vector2 &_v) const
242  {
243  return Vector2(this->data[0] + _v[0], this->data[1] + _v[1]);
244  }
245 
248  // \return this
249  public: const Vector2 &operator+=(const Vector2 &_v)
250  {
251  this->data[0] += _v[0];
252  this->data[1] += _v[1];
253 
254  return *this;
255  }
256 
260  public: inline Vector2<T> operator+(const T _s) const
261  {
262  return Vector2<T>(this->data[0] + _s,
263  this->data[1] + _s);
264  }
265 
270  public: friend inline Vector2<T> operator+(const T _s,
271  const Vector2<T> &_v)
272  {
273  return _v + _s;
274  }
275 
279  public: const Vector2<T> &operator+=(const T _s)
280  {
281  this->data[0] += _s;
282  this->data[1] += _s;
283 
284  return *this;
285  }
286 
289  public: inline Vector2 operator-() const
290  {
291  return Vector2(-this->data[0], -this->data[1]);
292  }
293 
297  public: Vector2 operator-(const Vector2 &_v) const
298  {
299  return Vector2(this->data[0] - _v[0], this->data[1] - _v[1]);
300  }
301 
305  public: const Vector2 &operator-=(const Vector2 &_v)
306  {
307  this->data[0] -= _v[0];
308  this->data[1] -= _v[1];
309 
310  return *this;
311  }
312 
316  public: inline Vector2<T> operator-(const T _s) const
317  {
318  return Vector2<T>(this->data[0] - _s,
319  this->data[1] - _s);
320  }
321 
326  public: friend inline Vector2<T> operator-(const T _s,
327  const Vector2<T> &_v)
328  {
329  return {_s - _v.X(), _s - _v.Y()};
330  }
331 
335  public: const Vector2<T> &operator-=(T _s)
336  {
337  this->data[0] -= _s;
338  this->data[1] -= _s;
339 
340  return *this;
341  }
342 
347  public: const Vector2 operator/(const Vector2 &_v) const
348  {
349  return Vector2(this->data[0] / _v[0], this->data[1] / _v[1]);
350  }
351 
356  public: const Vector2 &operator/=(const Vector2 &_v)
357  {
358  this->data[0] /= _v[0];
359  this->data[1] /= _v[1];
360 
361  return *this;
362  }
363 
367  public: const Vector2 operator/(T _v) const
368  {
369  return Vector2(this->data[0] / _v, this->data[1] / _v);
370  }
371 
375  public: const Vector2 &operator/=(T _v)
376  {
377  this->data[0] /= _v;
378  this->data[1] /= _v;
379 
380  return *this;
381  }
382 
386  public: const Vector2 operator*(const Vector2 &_v) const
387  {
388  return Vector2(this->data[0] * _v[0], this->data[1] * _v[1]);
389  }
390 
395  public: const Vector2 &operator*=(const Vector2 &_v)
396  {
397  this->data[0] *= _v[0];
398  this->data[1] *= _v[1];
399 
400  return *this;
401  }
402 
406  public: const Vector2 operator*(T _v) const
407  {
408  return Vector2(this->data[0] * _v, this->data[1] * _v);
409  }
410 
415  public: friend inline const Vector2 operator*(const T _s,
416  const Vector2 &_v)
417  {
418  return Vector2(_v * _s);
419  }
420 
424  public: const Vector2 &operator*=(T _v)
425  {
426  this->data[0] *= _v;
427  this->data[1] *= _v;
428 
429  return *this;
430  }
431 
437  public: bool Equal(const Vector2 &_v, const T &_tol) const
438  {
439  return equal<T>(this->data[0], _v[0], _tol)
440  && equal<T>(this->data[1], _v[1], _tol);
441  }
442 
447  public: bool operator==(const Vector2 &_v) const
448  {
449  return this->Equal(_v, static_cast<T>(1e-6));
450  }
451 
454  public: bool operator!=(const Vector2 &_v) const
455  {
456  return !(*this == _v);
457  }
458 
461  public: bool IsFinite() const
462  {
463  // std::isfinite works with floating point values,
464  // need to explicit cast to avoid ambiguity in vc++.
465  return std::isfinite(static_cast<double>(this->data[0])) &&
466  std::isfinite(static_cast<double>(this->data[1]));
467  }
468 
472  public: T &operator[](const std::size_t _index)
473  {
474  return this->data[clamp(_index, GZ_ZERO_SIZE_T, GZ_ONE_SIZE_T)];
475  }
476 
480  public: T operator[](const std::size_t _index) const
481  {
482  return this->data[clamp(_index, GZ_ZERO_SIZE_T, GZ_ONE_SIZE_T)];
483  }
484 
487  public: inline T X() const
488  {
489  return this->data[0];
490  }
491 
494  public: inline T Y() const
495  {
496  return this->data[1];
497  }
498 
501  public: inline T &X()
502  {
503  return this->data[0];
504  }
505 
508  public: inline T &Y()
509  {
510  return this->data[1];
511  }
512 
515  public: inline void X(const T &_v)
516  {
517  this->data[0] = _v;
518  }
519 
522  public: inline void Y(const T &_v)
523  {
524  this->data[1] = _v;
525  }
526 
531  public: friend std::ostream
532  &operator<<(std::ostream &_out, const Vector2<T> &_pt)
533  {
534  for (auto i : {0, 1})
535  {
536  if (i > 0)
537  _out << " ";
538 
539  appendToStream(_out, _pt[i]);
540  }
541  return _out;
542  }
543 
548  public: bool operator<(const Vector2<T> &_pt) const
549  {
550  return this->data[0] < _pt[0] || this->data[1] < _pt[1];
551  }
552 
557  public: friend std::istream
559  {
560  T x, y;
561  // Skip white spaces
562  _in.setf(std::ios_base::skipws);
563  _in >> x >> y;
564  if (!_in.fail())
565  {
566  _pt.Set(x, y);
567  }
568  return _in;
569  }
570 
572  private: T data[2];
573  };
574 
575  namespace detail {
576 
577  template<typename T>
578  constexpr Vector2<T> gVector2Zero(0, 0);
579 
580  template<typename T>
581  constexpr Vector2<T> gVector2One(1, 1);
582 
583  template<typename T>
584  constexpr Vector2<T> gVector2NaN(
587 
588  } // namespace detail
589 
590  template<typename T>
591  const Vector2<T> &Vector2<T>::Zero = detail::gVector2Zero<T>;
592 
593  template<typename T>
594  const Vector2<T> &Vector2<T>::One = detail::gVector2One<T>;
595 
596  template<typename T>
597  const Vector2<T> &Vector2<T>::NaN = detail::gVector2NaN<T>;
598 
602  } // namespace GZ_MATH_VERSION_NAMESPACE
603 } // namespace gz::math
604 #endif // GZ_MATH_VECTOR2_HH_