Gazebo Math

API Reference

7.4.0
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
29namespace gz
30{
31 namespace math
32 {
33 // Inline bracket to help doxygen filtering.
34 inline namespace GZ_MATH_VERSION_NAMESPACE {
35 //
38 template<typename T>
39 class Vector2
40 {
42 public: static const Vector2<T> &Zero;
43
45 public: static const Vector2<T> &One;
46
48 public: static const Vector2 &NaN;
49
51 public: constexpr Vector2()
52 : data{0, 0}
53 {
54 }
55
59 public: constexpr Vector2(const T &_x, const T &_y)
60 : data{_x, _y}
61 {
62 }
63
66 public: Vector2(const Vector2<T> &_v) = default;
67
69 public: ~Vector2() = default;
70
73 public: T Sum() const
74 {
75 return this->data[0] + this->data[1];
76 }
77
81 public: double Distance(const Vector2 &_pt) const
82 {
83 return sqrt((this->data[0]-_pt[0])*(this->data[0]-_pt[0]) +
84 (this->data[1]-_pt[1])*(this->data[1]-_pt[1]));
85 }
86
89 public: T Length() const
90 {
91 return static_cast<T>(sqrt(this->SquaredLength()));
92 }
93
96 public: T SquaredLength() const
97 {
98 return
99 this->data[0] * this->data[0] +
100 this->data[1] * this->data[1];
101 }
102
104 public: void Normalize()
105 {
106 T d = this->Length();
107
108 if (!equal<T>(d, static_cast<T>(0.0)))
109 {
110 this->data[0] /= d;
111 this->data[1] /= d;
112 }
113 }
114
117 public: Vector2 Normalized() const
118 {
119 Vector2<T> result = *this;
120 result.Normalize();
121 return result;
122 }
123
126 public: Vector2 Round()
127 {
128 this->data[0] = static_cast<T>(std::nearbyint(this->data[0]));
129 this->data[1] = static_cast<T>(std::nearbyint(this->data[1]));
130 return *this;
131 }
132
135 public: Vector2 Rounded() const
136 {
137 Vector2<T> result = *this;
138 result.Round();
139 return result;
140 }
141
145 public: void Set(T _x, T _y)
146 {
147 this->data[0] = _x;
148 this->data[1] = _y;
149 }
150
154 public: T Dot(const Vector2<T> &_v) const
155 {
156 return (this->data[0] * _v[0]) + (this->data[1] * _v[1]);
157 }
158
161 public: Vector2 Abs() const
162 {
163 return Vector2(std::abs(this->data[0]),
164 std::abs(this->data[1]));
165 }
166
175 public: T AbsDot(const Vector2<T> &_v) const
176 {
177 return std::abs(this->data[0] * _v[0]) +
178 std::abs(this->data[1] * _v[1]);
179 }
180
182 public: inline void Correct()
183 {
184 // std::isfinite works with floating point values,
185 // need to explicit cast to avoid ambiguity in vc++.
186 if (!std::isfinite(static_cast<double>(this->data[0])))
187 this->data[0] = 0;
188 if (!std::isfinite(static_cast<double>(this->data[1])))
189 this->data[1] = 0;
190 }
191
195 public: void Max(const Vector2<T> &_v)
196 {
197 this->data[0] = std::max(_v[0], this->data[0]);
198 this->data[1] = std::max(_v[1], this->data[1]);
199 }
200
204 public: void Min(const Vector2<T> &_v)
205 {
206 this->data[0] = std::min(_v[0], this->data[0]);
207 this->data[1] = std::min(_v[1], this->data[1]);
208 }
209
212 public: T Max() const
213 {
214 return std::max(this->data[0], this->data[1]);
215 }
216
219 public: T Min() const
220 {
221 return std::min(this->data[0], this->data[1]);
222 }
223
227 public: Vector2 &operator=(const Vector2 &_v) = default;
228
232 public: const Vector2 &operator=(T _v)
233 {
234 this->data[0] = _v;
235 this->data[1] = _v;
236
237 return *this;
238 }
239
243 public: Vector2 operator+(const Vector2 &_v) const
244 {
245 return Vector2(this->data[0] + _v[0], this->data[1] + _v[1]);
246 }
247
250 // \return this
251 public: const Vector2 &operator+=(const Vector2 &_v)
252 {
253 this->data[0] += _v[0];
254 this->data[1] += _v[1];
255
256 return *this;
257 }
258
262 public: inline Vector2<T> operator+(const T _s) const
263 {
264 return Vector2<T>(this->data[0] + _s,
265 this->data[1] + _s);
266 }
267
272 public: friend inline Vector2<T> operator+(const T _s,
273 const Vector2<T> &_v)
274 {
275 return _v + _s;
276 }
277
281 public: const Vector2<T> &operator+=(const T _s)
282 {
283 this->data[0] += _s;
284 this->data[1] += _s;
285
286 return *this;
287 }
288
291 public: inline Vector2 operator-() const
292 {
293 return Vector2(-this->data[0], -this->data[1]);
294 }
295
299 public: Vector2 operator-(const Vector2 &_v) const
300 {
301 return Vector2(this->data[0] - _v[0], this->data[1] - _v[1]);
302 }
303
307 public: const Vector2 &operator-=(const Vector2 &_v)
308 {
309 this->data[0] -= _v[0];
310 this->data[1] -= _v[1];
311
312 return *this;
313 }
314
318 public: inline Vector2<T> operator-(const T _s) const
319 {
320 return Vector2<T>(this->data[0] - _s,
321 this->data[1] - _s);
322 }
323
328 public: friend inline Vector2<T> operator-(const T _s,
329 const Vector2<T> &_v)
330 {
331 return {_s - _v.X(), _s - _v.Y()};
332 }
333
337 public: const Vector2<T> &operator-=(T _s)
338 {
339 this->data[0] -= _s;
340 this->data[1] -= _s;
341
342 return *this;
343 }
344
349 public: const Vector2 operator/(const Vector2 &_v) const
350 {
351 return Vector2(this->data[0] / _v[0], this->data[1] / _v[1]);
352 }
353
358 public: const Vector2 &operator/=(const Vector2 &_v)
359 {
360 this->data[0] /= _v[0];
361 this->data[1] /= _v[1];
362
363 return *this;
364 }
365
369 public: const Vector2 operator/(T _v) const
370 {
371 return Vector2(this->data[0] / _v, this->data[1] / _v);
372 }
373
377 public: const Vector2 &operator/=(T _v)
378 {
379 this->data[0] /= _v;
380 this->data[1] /= _v;
381
382 return *this;
383 }
384
388 public: const Vector2 operator*(const Vector2 &_v) const
389 {
390 return Vector2(this->data[0] * _v[0], this->data[1] * _v[1]);
391 }
392
397 public: const Vector2 &operator*=(const Vector2 &_v)
398 {
399 this->data[0] *= _v[0];
400 this->data[1] *= _v[1];
401
402 return *this;
403 }
404
408 public: const Vector2 operator*(T _v) const
409 {
410 return Vector2(this->data[0] * _v, this->data[1] * _v);
411 }
412
417 public: friend inline const Vector2 operator*(const T _s,
418 const Vector2 &_v)
419 {
420 return Vector2(_v * _s);
421 }
422
426 public: const Vector2 &operator*=(T _v)
427 {
428 this->data[0] *= _v;
429 this->data[1] *= _v;
430
431 return *this;
432 }
433
439 public: bool Equal(const Vector2 &_v, const T &_tol) const
440 {
441 return equal<T>(this->data[0], _v[0], _tol)
442 && equal<T>(this->data[1], _v[1], _tol);
443 }
444
449 public: bool operator==(const Vector2 &_v) const
450 {
451 return this->Equal(_v, static_cast<T>(1e-6));
452 }
453
456 public: bool operator!=(const Vector2 &_v) const
457 {
458 return !(*this == _v);
459 }
460
463 public: bool IsFinite() const
464 {
465 // std::isfinite works with floating point values,
466 // need to explicit cast to avoid ambiguity in vc++.
467 return std::isfinite(static_cast<double>(this->data[0])) &&
468 std::isfinite(static_cast<double>(this->data[1]));
469 }
470
474 public: T &operator[](const std::size_t _index)
475 {
476 return this->data[clamp(_index, GZ_ZERO_SIZE_T, GZ_ONE_SIZE_T)];
477 }
478
482 public: T operator[](const std::size_t _index) const
483 {
484 return this->data[clamp(_index, GZ_ZERO_SIZE_T, GZ_ONE_SIZE_T)];
485 }
486
489 public: inline T X() const
490 {
491 return this->data[0];
492 }
493
496 public: inline T Y() const
497 {
498 return this->data[1];
499 }
500
503 public: inline T &X()
504 {
505 return this->data[0];
506 }
507
510 public: inline T &Y()
511 {
512 return this->data[1];
513 }
514
517 public: inline void X(const T &_v)
518 {
519 this->data[0] = _v;
520 }
521
524 public: inline void Y(const T &_v)
525 {
526 this->data[1] = _v;
527 }
528
533 public: friend std::ostream
534 &operator<<(std::ostream &_out, const Vector2<T> &_pt)
535 {
536 for (auto i : {0, 1})
537 {
538 if (i > 0)
539 _out << " ";
540
541 appendToStream(_out, _pt[i]);
542 }
543 return _out;
544 }
545
550 public: bool operator<(const Vector2<T> &_pt) const
551 {
552 return this->data[0] < _pt[0] || this->data[1] < _pt[1];
553 }
554
559 public: friend std::istream
561 {
562 T x, y;
563 // Skip white spaces
564 _in.setf(std::ios_base::skipws);
565 _in >> x >> y;
566 if (!_in.fail())
567 {
568 _pt.Set(x, y);
569 }
570 return _in;
571 }
572
574 private: T data[2];
575 };
576
577 namespace detail {
578
579 template<typename T>
580 constexpr Vector2<T> gVector2Zero(0, 0);
581
582 template<typename T>
583 constexpr Vector2<T> gVector2One(1, 1);
584
585 template<typename T>
586 constexpr Vector2<T> gVector2NaN(
589
590 } // namespace detail
591
592 template<typename T>
593 const Vector2<T> &Vector2<T>::Zero = detail::gVector2Zero<T>;
594
595 template<typename T>
596 const Vector2<T> &Vector2<T>::One = detail::gVector2One<T>;
597
598 template<typename T>
599 const Vector2<T> &Vector2<T>::NaN = detail::gVector2NaN<T>;
600
604 }
605 }
606}
607#endif