Gazebo Math

API Reference

8.1.0
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::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: T Sum() const
65 {
66 return this->data[0] + this->data[1];
67 }
68
72 public: double Distance(const Vector2 &_pt) const
73 {
74 return sqrt((this->data[0]-_pt[0])*(this->data[0]-_pt[0]) +
75 (this->data[1]-_pt[1])*(this->data[1]-_pt[1]));
76 }
77
80 public: T Length() const
81 {
82 return static_cast<T>(sqrt(this->SquaredLength()));
83 }
84
87 public: T SquaredLength() const
88 {
89 return
90 this->data[0] * this->data[0] +
91 this->data[1] * this->data[1];
92 }
93
95 public: void Normalize()
96 {
97 T d = this->Length();
98
99 if (!equal<T>(d, static_cast<T>(0.0)))
100 {
101 this->data[0] /= d;
102 this->data[1] /= d;
103 }
104 }
105
108 public: Vector2 Normalized() const
109 {
110 Vector2<T> result = *this;
111 result.Normalize();
112 return result;
113 }
114
117 public: Vector2 Round()
118 {
119 this->data[0] = static_cast<T>(std::nearbyint(this->data[0]));
120 this->data[1] = static_cast<T>(std::nearbyint(this->data[1]));
121 return *this;
122 }
123
126 public: Vector2 Rounded() const
127 {
128 Vector2<T> result = *this;
129 result.Round();
130 return result;
131 }
132
136 public: void Set(T _x, T _y)
137 {
138 this->data[0] = _x;
139 this->data[1] = _y;
140 }
141
145 public: T Dot(const Vector2<T> &_v) const
146 {
147 return (this->data[0] * _v[0]) + (this->data[1] * _v[1]);
148 }
149
152 public: Vector2 Abs() const
153 {
154 return Vector2(std::abs(this->data[0]),
155 std::abs(this->data[1]));
156 }
157
166 public: T AbsDot(const Vector2<T> &_v) const
167 {
168 return std::abs(this->data[0] * _v[0]) +
169 std::abs(this->data[1] * _v[1]);
170 }
171
173 public: inline void Correct()
174 {
175 // std::isfinite works with floating point values,
176 // need to explicit cast to avoid ambiguity in vc++.
177 if (!std::isfinite(static_cast<double>(this->data[0])))
178 this->data[0] = 0;
179 if (!std::isfinite(static_cast<double>(this->data[1])))
180 this->data[1] = 0;
181 }
182
186 public: void Max(const Vector2<T> &_v)
187 {
188 this->data[0] = std::max(_v[0], this->data[0]);
189 this->data[1] = std::max(_v[1], this->data[1]);
190 }
191
195 public: void Min(const Vector2<T> &_v)
196 {
197 this->data[0] = std::min(_v[0], this->data[0]);
198 this->data[1] = std::min(_v[1], this->data[1]);
199 }
200
203 public: T Max() const
204 {
205 return std::max(this->data[0], this->data[1]);
206 }
207
210 public: T Min() const
211 {
212 return std::min(this->data[0], this->data[1]);
213 }
214
218 public: const Vector2 &operator=(T _v)
219 {
220 this->data[0] = _v;
221 this->data[1] = _v;
222
223 return *this;
224 }
225
229 public: Vector2 operator+(const Vector2 &_v) const
230 {
231 return Vector2(this->data[0] + _v[0], this->data[1] + _v[1]);
232 }
233
236 // \return this
237 public: const Vector2 &operator+=(const Vector2 &_v)
238 {
239 this->data[0] += _v[0];
240 this->data[1] += _v[1];
241
242 return *this;
243 }
244
248 public: inline Vector2<T> operator+(const T _s) const
249 {
250 return Vector2<T>(this->data[0] + _s,
251 this->data[1] + _s);
252 }
253
258 public: friend inline Vector2<T> operator+(const T _s,
259 const Vector2<T> &_v)
260 {
261 return _v + _s;
262 }
263
267 public: const Vector2<T> &operator+=(const T _s)
268 {
269 this->data[0] += _s;
270 this->data[1] += _s;
271
272 return *this;
273 }
274
277 public: inline Vector2 operator-() const
278 {
279 return Vector2(-this->data[0], -this->data[1]);
280 }
281
285 public: Vector2 operator-(const Vector2 &_v) const
286 {
287 return Vector2(this->data[0] - _v[0], this->data[1] - _v[1]);
288 }
289
293 public: const Vector2 &operator-=(const Vector2 &_v)
294 {
295 this->data[0] -= _v[0];
296 this->data[1] -= _v[1];
297
298 return *this;
299 }
300
304 public: inline Vector2<T> operator-(const T _s) const
305 {
306 return Vector2<T>(this->data[0] - _s,
307 this->data[1] - _s);
308 }
309
314 public: friend inline Vector2<T> operator-(const T _s,
315 const Vector2<T> &_v)
316 {
317 return {_s - _v.X(), _s - _v.Y()};
318 }
319
323 public: const Vector2<T> &operator-=(T _s)
324 {
325 this->data[0] -= _s;
326 this->data[1] -= _s;
327
328 return *this;
329 }
330
335 public: const Vector2 operator/(const Vector2 &_v) const
336 {
337 return Vector2(this->data[0] / _v[0], this->data[1] / _v[1]);
338 }
339
344 public: const Vector2 &operator/=(const Vector2 &_v)
345 {
346 this->data[0] /= _v[0];
347 this->data[1] /= _v[1];
348
349 return *this;
350 }
351
355 public: const Vector2 operator/(T _v) const
356 {
357 return Vector2(this->data[0] / _v, this->data[1] / _v);
358 }
359
363 public: const Vector2 &operator/=(T _v)
364 {
365 this->data[0] /= _v;
366 this->data[1] /= _v;
367
368 return *this;
369 }
370
374 public: const Vector2 operator*(const Vector2 &_v) const
375 {
376 return Vector2(this->data[0] * _v[0], this->data[1] * _v[1]);
377 }
378
383 public: const Vector2 &operator*=(const Vector2 &_v)
384 {
385 this->data[0] *= _v[0];
386 this->data[1] *= _v[1];
387
388 return *this;
389 }
390
394 public: const Vector2 operator*(T _v) const
395 {
396 return Vector2(this->data[0] * _v, this->data[1] * _v);
397 }
398
403 public: friend inline const Vector2 operator*(const T _s,
404 const Vector2 &_v)
405 {
406 return Vector2(_v * _s);
407 }
408
412 public: const Vector2 &operator*=(T _v)
413 {
414 this->data[0] *= _v;
415 this->data[1] *= _v;
416
417 return *this;
418 }
419
425 public: bool Equal(const Vector2 &_v, const T &_tol) const
426 {
427 return equal<T>(this->data[0], _v[0], _tol)
428 && equal<T>(this->data[1], _v[1], _tol);
429 }
430
435 public: bool operator==(const Vector2 &_v) const
436 {
437 return this->Equal(_v, static_cast<T>(1e-6));
438 }
439
442 public: bool operator!=(const Vector2 &_v) const
443 {
444 return !(*this == _v);
445 }
446
449 public: bool IsFinite() const
450 {
451 // std::isfinite works with floating point values,
452 // need to explicit cast to avoid ambiguity in vc++.
453 return std::isfinite(static_cast<double>(this->data[0])) &&
454 std::isfinite(static_cast<double>(this->data[1]));
455 }
456
460 public: T &operator[](const std::size_t _index)
461 {
462 return this->data[clamp(_index, GZ_ZERO_SIZE_T, GZ_ONE_SIZE_T)];
463 }
464
468 public: T operator[](const std::size_t _index) const
469 {
470 return this->data[clamp(_index, GZ_ZERO_SIZE_T, GZ_ONE_SIZE_T)];
471 }
472
475 public: inline T X() const
476 {
477 return this->data[0];
478 }
479
482 public: inline T Y() const
483 {
484 return this->data[1];
485 }
486
489 public: inline T &X()
490 {
491 return this->data[0];
492 }
493
496 public: inline T &Y()
497 {
498 return this->data[1];
499 }
500
503 public: inline void X(const T &_v)
504 {
505 this->data[0] = _v;
506 }
507
510 public: inline void Y(const T &_v)
511 {
512 this->data[1] = _v;
513 }
514
519 public: friend std::ostream
521 {
522 for (auto i : {0, 1})
523 {
524 if (i > 0)
525 _out << " ";
526
528 }
529 return _out;
530 }
531
536 public: bool operator<(const Vector2<T> &_pt) const
537 {
538 return this->data[0] < _pt[0] || this->data[1] < _pt[1];
539 }
540
545 public: friend std::istream
547 {
548 T x, y;
549 // Skip white spaces
550 _in.setf(std::ios_base::skipws);
551 _in >> x >> y;
552 if (!_in.fail())
553 {
554 _pt.Set(x, y);
555 }
556 return _in;
557 }
558
560 private: T data[2];
561 };
562
563 namespace detail {
564
565 template<typename T>
566 constexpr Vector2<T> gVector2Zero(0, 0);
567
568 template<typename T>
569 constexpr Vector2<T> gVector2One(1, 1);
570
571 template<typename T>
572 constexpr Vector2<T> gVector2NaN(
575
576 } // namespace detail
577
578 template<typename T>
579 const Vector2<T> &Vector2<T>::Zero = detail::gVector2Zero<T>;
580
581 template<typename T>
582 const Vector2<T> &Vector2<T>::One = detail::gVector2One<T>;
583
584 template<typename T>
585 const Vector2<T> &Vector2<T>::NaN = detail::gVector2NaN<T>;
586
590 } // namespace GZ_MATH_VERSION_NAMESPACE
591} // namespace gz::math
592#endif // GZ_MATH_VECTOR2_HH_