Gazebo Math

API Reference

7.4.0
gz/math/Vector3.hh
Go to the documentation of this file.
1/*
2 * Copyright (C) 2014 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_VECTOR3_HH_
18#define GZ_MATH_VECTOR3_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 //
40 template<typename T>
41 class Vector3
42 {
44 public: static const Vector3 &Zero;
45
47 public: static const Vector3 &One;
48
50 public: static const Vector3 &UnitX;
51
53 public: static const Vector3 &UnitY;
54
56 public: static const Vector3 &UnitZ;
57
59 public: static const Vector3 &NaN;
60
62 public: constexpr Vector3()
63 : data{0, 0, 0}
64 {
65 }
66
71 public: constexpr Vector3(const T &_x, const T &_y, const T &_z)
72 : data{_x, _y, _z}
73 {
74 }
75
78 public: Vector3(const Vector3<T> &_v) = default;
79
81 public: ~Vector3() = default;
82
85 public: T Sum() const
86 {
87 return this->data[0] + this->data[1] + this->data[2];
88 }
89
93 public: T Distance(const Vector3<T> &_pt) const
94 {
95 return static_cast<T>(sqrt(
96 (this->data[0]-_pt[0])*(this->data[0]-_pt[0]) +
97 (this->data[1]-_pt[1])*(this->data[1]-_pt[1]) +
98 (this->data[2]-_pt[2])*(this->data[2]-_pt[2])));
99 }
100
106 public: T Distance(T _x, T _y, T _z) const
107 {
108 return this->Distance(Vector3(_x, _y, _z));
109 }
110
113 public: T Length() const
114 {
115 return static_cast<T>(sqrt(this->SquaredLength()));
116 }
117
120 public: T SquaredLength() const
121 {
122 return
123 this->data[0] * this->data[0] +
124 this->data[1] * this->data[1] +
125 this->data[2] * this->data[2];
126 }
127
131 {
132 T d = this->Length();
133
134 if (!equal<T>(d, static_cast<T>(0.0)))
135 {
136 this->data[0] /= d;
137 this->data[1] /= d;
138 this->data[2] /= d;
139 }
140
141 return *this;
142 }
143
146 public: Vector3 Normalized() const
147 {
148 Vector3<T> result = *this;
149 result.Normalize();
150 return result;
151 }
152
155 public: Vector3 Round()
156 {
157 this->data[0] = static_cast<T>(std::nearbyint(this->data[0]));
158 this->data[1] = static_cast<T>(std::nearbyint(this->data[1]));
159 this->data[2] = static_cast<T>(std::nearbyint(this->data[2]));
160 return *this;
161 }
162
165 public: Vector3 Rounded() const
166 {
167 Vector3<T> result = *this;
168 result.Round();
169 return result;
170 }
171
176 public: inline void Set(T _x = 0, T _y = 0, T _z = 0)
177 {
178 this->data[0] = _x;
179 this->data[1] = _y;
180 this->data[2] = _z;
181 }
182
186 public: Vector3 Cross(const Vector3<T> &_v) const
187 {
188 return Vector3(this->data[1] * _v[2] - this->data[2] * _v[1],
189 this->data[2] * _v[0] - this->data[0] * _v[2],
190 this->data[0] * _v[1] - this->data[1] * _v[0]);
191 }
192
196 public: T Dot(const Vector3<T> &_v) const
197 {
198 return this->data[0] * _v[0] +
199 this->data[1] * _v[1] +
200 this->data[2] * _v[2];
201 }
202
211 public: T AbsDot(const Vector3<T> &_v) const
212 {
213 return std::abs(this->data[0] * _v[0]) +
214 std::abs(this->data[1] * _v[1]) +
215 std::abs(this->data[2] * _v[2]);
216 }
217
220 public: Vector3 Abs() const
221 {
222 return Vector3(std::abs(this->data[0]),
223 std::abs(this->data[1]),
224 std::abs(this->data[2]));
225 }
226
229 public: Vector3 Perpendicular() const
230 {
231 static const T sqrZero = static_cast<T>(1e-06 * 1e-06);
232
233 Vector3<T> perp = this->Cross(Vector3(1, 0, 0));
234
235 // Check the length of the vector
236 if (perp.SquaredLength() < sqrZero)
237 {
238 perp = this->Cross(Vector3(0, 1, 0));
239 }
240
241 return perp;
242 }
243
249 public: static Vector3 Normal(const Vector3<T> &_v1,
250 const Vector3<T> &_v2, const Vector3<T> &_v3)
251 {
252 Vector3<T> a = _v2 - _v1;
253 Vector3<T> b = _v3 - _v1;
254 Vector3<T> n = a.Cross(b);
255 return n.Normalize();
256 }
257
262 public: T DistToLine(const Vector3<T> &_pt1, const Vector3 &_pt2)
263 {
264 T d = ((*this) - _pt1).Cross((*this) - _pt2).Length();
265 d = d / (_pt2 - _pt1).Length();
266 return d;
267 }
268
272 public: void Max(const Vector3<T> &_v)
273 {
274 if (_v[0] > this->data[0])
275 this->data[0] = _v[0];
276 if (_v[1] > this->data[1])
277 this->data[1] = _v[1];
278 if (_v[2] > this->data[2])
279 this->data[2] = _v[2];
280 }
281
285 public: void Min(const Vector3<T> &_v)
286 {
287 if (_v[0] < this->data[0])
288 this->data[0] = _v[0];
289 if (_v[1] < this->data[1])
290 this->data[1] = _v[1];
291 if (_v[2] < this->data[2])
292 this->data[2] = _v[2];
293 }
294
297 public: T Max() const
298 {
299 return std::max(std::max(this->data[0], this->data[1]), this->data[2]);
300 }
301
304 public: T Min() const
305 {
306 return std::min(std::min(this->data[0], this->data[1]), this->data[2]);
307 }
308
311 public: T MaxAbs() const
312 {
313 T max = std::max(std::abs(this->data[0]), std::abs(this->data[1]));
314 max = std::max(max, std::abs(this->data[2]));
315 return max;
316 }
317
320 public: T MinAbs() const
321 {
322 T min = std::min(std::abs(this->data[0]), std::abs(this->data[1]));
323 min = std::min(min, std::abs(this->data[2]));
324 return min;
325 }
326
330 public: Vector3 &operator=(const Vector3<T> &_v) = default;
331
335 public: Vector3 &operator=(T _v)
336 {
337 this->data[0] = _v;
338 this->data[1] = _v;
339 this->data[2] = _v;
340
341 return *this;
342 }
343
347 public: Vector3 operator+(const Vector3<T> &_v) const
348 {
349 return Vector3(this->data[0] + _v[0],
350 this->data[1] + _v[1],
351 this->data[2] + _v[2]);
352 }
353
357 public: const Vector3 &operator+=(const Vector3<T> &_v)
358 {
359 this->data[0] += _v[0];
360 this->data[1] += _v[1];
361 this->data[2] += _v[2];
362
363 return *this;
364 }
365
369 public: inline Vector3<T> operator+(const T _s) const
370 {
371 return Vector3<T>(this->data[0] + _s,
372 this->data[1] + _s,
373 this->data[2] + _s);
374 }
375
380 public: friend inline Vector3<T> operator+(const T _s,
381 const Vector3<T> &_v)
382 {
383 return {_v.X() + _s, _v.Y() + _s, _v.Z() + _s};
384 }
385
389 public: const Vector3<T> &operator+=(const T _s)
390 {
391 this->data[0] += _s;
392 this->data[1] += _s;
393 this->data[2] += _s;
394
395 return *this;
396 }
397
400 public: inline Vector3 operator-() const
401 {
402 return Vector3(-this->data[0], -this->data[1], -this->data[2]);
403 }
404
408 public: inline Vector3<T> operator-(const Vector3<T> &_pt) const
409 {
410 return Vector3(this->data[0] - _pt[0],
411 this->data[1] - _pt[1],
412 this->data[2] - _pt[2]);
413 }
414
418 public: const Vector3<T> &operator-=(const Vector3<T> &_pt)
419 {
420 this->data[0] -= _pt[0];
421 this->data[1] -= _pt[1];
422 this->data[2] -= _pt[2];
423
424 return *this;
425 }
426
430 public: inline Vector3<T> operator-(const T _s) const
431 {
432 return Vector3<T>(this->data[0] - _s,
433 this->data[1] - _s,
434 this->data[2] - _s);
435 }
436
441 public: friend inline Vector3<T> operator-(const T _s,
442 const Vector3<T> &_v)
443 {
444 return {_s - _v.X(), _s - _v.Y(), _s - _v.Z()};
445 }
446
450 public: const Vector3<T> &operator-=(const T _s)
451 {
452 this->data[0] -= _s;
453 this->data[1] -= _s;
454 this->data[2] -= _s;
455
456 return *this;
457 }
458
463 public: const Vector3<T> operator/(const Vector3<T> &_pt) const
464 {
465 return Vector3(this->data[0] / _pt[0],
466 this->data[1] / _pt[1],
467 this->data[2] / _pt[2]);
468 }
469
474 public: const Vector3<T> &operator/=(const Vector3<T> &_pt)
475 {
476 this->data[0] /= _pt[0];
477 this->data[1] /= _pt[1];
478 this->data[2] /= _pt[2];
479
480 return *this;
481 }
482
487 public: const Vector3<T> operator/(T _v) const
488 {
489 return Vector3(this->data[0] / _v,
490 this->data[1] / _v,
491 this->data[2] / _v);
492 }
493
498 public: const Vector3<T> &operator/=(T _v)
499 {
500 this->data[0] /= _v;
501 this->data[1] /= _v;
502 this->data[2] /= _v;
503
504 return *this;
505 }
506
511 public: Vector3<T> operator*(const Vector3<T> &_p) const
512 {
513 return Vector3(this->data[0] * _p[0],
514 this->data[1] * _p[1],
515 this->data[2] * _p[2]);
516 }
517
522 public: const Vector3<T> &operator*=(const Vector3<T> &_v)
523 {
524 this->data[0] *= _v[0];
525 this->data[1] *= _v[1];
526 this->data[2] *= _v[2];
527
528 return *this;
529 }
530
534 public: inline Vector3<T> operator*(T _s) const
535 {
536 return Vector3<T>(this->data[0] * _s,
537 this->data[1] * _s,
538 this->data[2] * _s);
539 }
540
545 public: friend inline Vector3<T> operator*(T _s, const Vector3<T> &_v)
546 {
547 return {_v.X() * _s, _v.Y() * _s, _v.Z() * _s};
548 }
549
553 public: const Vector3<T> &operator*=(T _v)
554 {
555 this->data[0] *= _v;
556 this->data[1] *= _v;
557 this->data[2] *= _v;
558
559 return *this;
560 }
561
567 public: bool Equal(const Vector3 &_v, const T &_tol) const
568 {
569 return equal<T>(this->data[0], _v[0], _tol)
570 && equal<T>(this->data[1], _v[1], _tol)
571 && equal<T>(this->data[2], _v[2], _tol);
572 }
573
578 public: bool operator==(const Vector3<T> &_v) const
579 {
580 return this->Equal(_v, static_cast<T>(1e-3));
581 }
582
587 public: bool operator!=(const Vector3<T> &_v) const
588 {
589 return !(*this == _v);
590 }
591
594 public: bool IsFinite() const
595 {
596 // std::isfinite works with floating point values,
597 // need to explicit cast to avoid ambiguity in vc++.
598 return std::isfinite(static_cast<double>(this->data[0])) &&
599 std::isfinite(static_cast<double>(this->data[1])) &&
600 std::isfinite(static_cast<double>(this->data[2]));
601 }
602
604 public: inline void Correct()
605 {
606 // std::isfinite works with floating point values,
607 // need to explicit cast to avoid ambiguity in vc++.
608 if (!std::isfinite(static_cast<double>(this->data[0])))
609 this->data[0] = 0;
610 if (!std::isfinite(static_cast<double>(this->data[1])))
611 this->data[1] = 0;
612 if (!std::isfinite(static_cast<double>(this->data[2])))
613 this->data[2] = 0;
614 }
615
620 public: T &operator[](const std::size_t _index)
621 {
622 return this->data[clamp(_index, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)];
623 }
624
629 public: T operator[](const std::size_t _index) const
630 {
631 return this->data[clamp(_index, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)];
632 }
633
636 public: void Round(int _precision)
637 {
638 this->data[0] = precision(this->data[0], _precision);
639 this->data[1] = precision(this->data[1], _precision);
640 this->data[2] = precision(this->data[2], _precision);
641 }
642
647 public: bool Equal(const Vector3<T> &_v) const
648 {
649 return equal<T>(this->data[0], _v[0]) &&
650 equal<T>(this->data[1], _v[1]) &&
651 equal<T>(this->data[2], _v[2]);
652 }
653
656 public: inline T X() const
657 {
658 return this->data[0];
659 }
660
663 public: inline T Y() const
664 {
665 return this->data[1];
666 }
667
670 public: inline T Z() const
671 {
672 return this->data[2];
673 }
674
677 public: inline T &X()
678 {
679 return this->data[0];
680 }
681
684 public: inline T &Y()
685 {
686 return this->data[1];
687 }
688
691 public: inline T &Z()
692 {
693 return this->data[2];
694 }
695
698 public: inline void X(const T &_v)
699 {
700 this->data[0] = _v;
701 }
702
705 public: inline void Y(const T &_v)
706 {
707 this->data[1] = _v;
708 }
709
712 public: inline void Z(const T &_v)
713 {
714 this->data[2] = _v;
715 }
716
721 public: bool operator<(const Vector3<T> &_pt) const
722 {
723 return this->data[0] < _pt[0] || this->data[1] < _pt[1] ||
724 this->data[2] < _pt[2];
725 }
726
731 public: friend std::ostream &operator<<(
732 std::ostream &_out, const gz::math::Vector3<T> &_pt)
733 {
734 for (auto i : {0, 1, 2})
735 {
736 if (i > 0)
737 _out << " ";
738
739 appendToStream(_out, _pt[i]);
740 }
741
742 return _out;
743 }
744
749 public: friend std::istream &operator>>(
751 {
752 // Skip white spaces
753 _in.setf(std::ios_base::skipws);
754 T x, y, z;
755 _in >> x >> y >> z;
756 if (!_in.fail())
757 {
758 _pt.Set(x, y, z);
759 }
760 return _in;
761 }
762
764 private: T data[3];
765 };
766
767 namespace detail {
768
769 template<typename T>
770 constexpr Vector3<T> gVector3Zero(0, 0, 0);
771 template<typename T>
772 constexpr Vector3<T> gVector3One(1, 1, 1);
773 template<typename T>
774 constexpr Vector3<T> gVector3UnitX(1, 0, 0);
775 template<typename T>
776 constexpr Vector3<T> gVector3UnitY(0, 1, 0);
777 template<typename T>
778 constexpr Vector3<T> gVector3UnitZ(0, 0, 1);
779 template<typename T>
780 constexpr Vector3<T> gVector3NaN(
784 } // namespace detail
785
786 template<typename T>
787 const Vector3<T> &Vector3<T>::Zero = detail::gVector3Zero<T>;
788 template<typename T>
789 const Vector3<T> &Vector3<T>::One = detail::gVector3One<T>;
790 template<typename T>
791 const Vector3<T> &Vector3<T>::UnitX = detail::gVector3UnitX<T>;
792 template<typename T>
793 const Vector3<T> &Vector3<T>::UnitY = detail::gVector3UnitY<T>;
794 template<typename T>
795 const Vector3<T> &Vector3<T>::UnitZ = detail::gVector3UnitZ<T>;
796 template<typename T>
797 const Vector3<T> &Vector3<T>::NaN = detail::gVector3NaN<T>;
798
802 }
803 }
804}
805#endif