Gazebo Math

API Reference

8.1.0
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::math
30{
31 // Inline bracket to help doxygen filtering.
32 inline namespace GZ_MATH_VERSION_NAMESPACE {
33 //
38 template<typename T>
39 class Vector3
40 {
42 public: static const Vector3 &Zero;
43
45 public: static const Vector3 &One;
46
48 public: static const Vector3 &UnitX;
49
51 public: static const Vector3 &UnitY;
52
54 public: static const Vector3 &UnitZ;
55
57 public: static const Vector3 &NaN;
58
60 public: constexpr Vector3()
61 : data{0, 0, 0}
62 {
63 }
64
69 public: constexpr Vector3(const T &_x, const T &_y, const T &_z)
70 : data{_x, _y, _z}
71 {
72 }
73
76 public: T Sum() const
77 {
78 return this->data[0] + this->data[1] + this->data[2];
79 }
80
84 public: T Distance(const Vector3<T> &_pt) const
85 {
86 return static_cast<T>(sqrt(
87 (this->data[0]-_pt[0])*(this->data[0]-_pt[0]) +
88 (this->data[1]-_pt[1])*(this->data[1]-_pt[1]) +
89 (this->data[2]-_pt[2])*(this->data[2]-_pt[2])));
90 }
91
97 public: T Distance(T _x, T _y, T _z) const
98 {
99 return this->Distance(Vector3(_x, _y, _z));
100 }
101
104 public: T Length() const
105 {
106 return static_cast<T>(sqrt(this->SquaredLength()));
107 }
108
111 public: T SquaredLength() const
112 {
113 return
114 this->data[0] * this->data[0] +
115 this->data[1] * this->data[1] +
116 this->data[2] * this->data[2];
117 }
118
122 {
123 T d = this->Length();
124
125 if (!equal<T>(d, static_cast<T>(0.0)))
126 {
127 this->data[0] /= d;
128 this->data[1] /= d;
129 this->data[2] /= d;
130 }
131
132 return *this;
133 }
134
137 public: Vector3 Normalized() const
138 {
139 Vector3<T> result = *this;
140 result.Normalize();
141 return result;
142 }
143
146 public: Vector3 Round()
147 {
148 this->data[0] = static_cast<T>(std::nearbyint(this->data[0]));
149 this->data[1] = static_cast<T>(std::nearbyint(this->data[1]));
150 this->data[2] = static_cast<T>(std::nearbyint(this->data[2]));
151 return *this;
152 }
153
156 public: Vector3 Rounded() const
157 {
158 Vector3<T> result = *this;
159 result.Round();
160 return result;
161 }
162
167 public: inline void Set(T _x = 0, T _y = 0, T _z = 0)
168 {
169 this->data[0] = _x;
170 this->data[1] = _y;
171 this->data[2] = _z;
172 }
173
177 public: Vector3 Cross(const Vector3<T> &_v) const
178 {
179 return Vector3(this->data[1] * _v[2] - this->data[2] * _v[1],
180 this->data[2] * _v[0] - this->data[0] * _v[2],
181 this->data[0] * _v[1] - this->data[1] * _v[0]);
182 }
183
187 public: T Dot(const Vector3<T> &_v) const
188 {
189 return this->data[0] * _v[0] +
190 this->data[1] * _v[1] +
191 this->data[2] * _v[2];
192 }
193
202 public: T AbsDot(const Vector3<T> &_v) const
203 {
204 return std::abs(this->data[0] * _v[0]) +
205 std::abs(this->data[1] * _v[1]) +
206 std::abs(this->data[2] * _v[2]);
207 }
208
211 public: Vector3 Abs() const
212 {
213 return Vector3(std::abs(this->data[0]),
214 std::abs(this->data[1]),
215 std::abs(this->data[2]));
216 }
217
220 public: Vector3 Perpendicular() const
221 {
222 static const T sqrZero = static_cast<T>(1e-06 * 1e-06);
223
224 Vector3<T> perp = this->Cross(Vector3(1, 0, 0));
225
226 // Check the length of the vector
227 if (perp.SquaredLength() < sqrZero)
228 {
229 perp = this->Cross(Vector3(0, 1, 0));
230 }
231
232 return perp;
233 }
234
240 public: static Vector3 Normal(const Vector3<T> &_v1,
241 const Vector3<T> &_v2, const Vector3<T> &_v3)
242 {
243 Vector3<T> a = _v2 - _v1;
244 Vector3<T> b = _v3 - _v1;
245 Vector3<T> n = a.Cross(b);
246 return n.Normalize();
247 }
248
253 public: T DistToLine(const Vector3<T> &_pt1, const Vector3 &_pt2)
254 {
255 T d = ((*this) - _pt1).Cross((*this) - _pt2).Length();
256 d = d / (_pt2 - _pt1).Length();
257 return d;
258 }
259
263 public: void Max(const Vector3<T> &_v)
264 {
265 if (_v[0] > this->data[0])
266 this->data[0] = _v[0];
267 if (_v[1] > this->data[1])
268 this->data[1] = _v[1];
269 if (_v[2] > this->data[2])
270 this->data[2] = _v[2];
271 }
272
276 public: void Min(const Vector3<T> &_v)
277 {
278 if (_v[0] < this->data[0])
279 this->data[0] = _v[0];
280 if (_v[1] < this->data[1])
281 this->data[1] = _v[1];
282 if (_v[2] < this->data[2])
283 this->data[2] = _v[2];
284 }
285
288 public: T Max() const
289 {
290 return std::max(std::max(this->data[0], this->data[1]), this->data[2]);
291 }
292
295 public: T Min() const
296 {
297 return std::min(std::min(this->data[0], this->data[1]), this->data[2]);
298 }
299
302 public: T MaxAbs() const
303 {
304 T max = std::max(std::abs(this->data[0]), std::abs(this->data[1]));
305 max = std::max(max, std::abs(this->data[2]));
306 return max;
307 }
308
311 public: T MinAbs() const
312 {
313 T min = std::min(std::abs(this->data[0]), std::abs(this->data[1]));
314 min = std::min(min, std::abs(this->data[2]));
315 return min;
316 }
317
321 public: Vector3 &operator=(T _v)
322 {
323 this->data[0] = _v;
324 this->data[1] = _v;
325 this->data[2] = _v;
326
327 return *this;
328 }
329
333 public: Vector3 operator+(const Vector3<T> &_v) const
334 {
335 return Vector3(this->data[0] + _v[0],
336 this->data[1] + _v[1],
337 this->data[2] + _v[2]);
338 }
339
343 public: const Vector3 &operator+=(const Vector3<T> &_v)
344 {
345 this->data[0] += _v[0];
346 this->data[1] += _v[1];
347 this->data[2] += _v[2];
348
349 return *this;
350 }
351
355 public: inline Vector3<T> operator+(const T _s) const
356 {
357 return Vector3<T>(this->data[0] + _s,
358 this->data[1] + _s,
359 this->data[2] + _s);
360 }
361
366 public: friend inline Vector3<T> operator+(const T _s,
367 const Vector3<T> &_v)
368 {
369 return {_v.X() + _s, _v.Y() + _s, _v.Z() + _s};
370 }
371
375 public: const Vector3<T> &operator+=(const T _s)
376 {
377 this->data[0] += _s;
378 this->data[1] += _s;
379 this->data[2] += _s;
380
381 return *this;
382 }
383
386 public: inline Vector3 operator-() const
387 {
388 return Vector3(-this->data[0], -this->data[1], -this->data[2]);
389 }
390
394 public: inline Vector3<T> operator-(const Vector3<T> &_pt) const
395 {
396 return Vector3(this->data[0] - _pt[0],
397 this->data[1] - _pt[1],
398 this->data[2] - _pt[2]);
399 }
400
404 public: const Vector3<T> &operator-=(const Vector3<T> &_pt)
405 {
406 this->data[0] -= _pt[0];
407 this->data[1] -= _pt[1];
408 this->data[2] -= _pt[2];
409
410 return *this;
411 }
412
416 public: inline Vector3<T> operator-(const T _s) const
417 {
418 return Vector3<T>(this->data[0] - _s,
419 this->data[1] - _s,
420 this->data[2] - _s);
421 }
422
427 public: friend inline Vector3<T> operator-(const T _s,
428 const Vector3<T> &_v)
429 {
430 return {_s - _v.X(), _s - _v.Y(), _s - _v.Z()};
431 }
432
436 public: const Vector3<T> &operator-=(const T _s)
437 {
438 this->data[0] -= _s;
439 this->data[1] -= _s;
440 this->data[2] -= _s;
441
442 return *this;
443 }
444
449 public: const Vector3<T> operator/(const Vector3<T> &_pt) const
450 {
451 return Vector3(this->data[0] / _pt[0],
452 this->data[1] / _pt[1],
453 this->data[2] / _pt[2]);
454 }
455
460 public: const Vector3<T> &operator/=(const Vector3<T> &_pt)
461 {
462 this->data[0] /= _pt[0];
463 this->data[1] /= _pt[1];
464 this->data[2] /= _pt[2];
465
466 return *this;
467 }
468
473 public: const Vector3<T> operator/(T _v) const
474 {
475 return Vector3(this->data[0] / _v,
476 this->data[1] / _v,
477 this->data[2] / _v);
478 }
479
484 public: const Vector3<T> &operator/=(T _v)
485 {
486 this->data[0] /= _v;
487 this->data[1] /= _v;
488 this->data[2] /= _v;
489
490 return *this;
491 }
492
497 public: Vector3<T> operator*(const Vector3<T> &_p) const
498 {
499 return Vector3(this->data[0] * _p[0],
500 this->data[1] * _p[1],
501 this->data[2] * _p[2]);
502 }
503
508 public: const Vector3<T> &operator*=(const Vector3<T> &_v)
509 {
510 this->data[0] *= _v[0];
511 this->data[1] *= _v[1];
512 this->data[2] *= _v[2];
513
514 return *this;
515 }
516
520 public: inline Vector3<T> operator*(T _s) const
521 {
522 return Vector3<T>(this->data[0] * _s,
523 this->data[1] * _s,
524 this->data[2] * _s);
525 }
526
531 public: friend inline Vector3<T> operator*(T _s, const Vector3<T> &_v)
532 {
533 return {_v.X() * _s, _v.Y() * _s, _v.Z() * _s};
534 }
535
539 public: const Vector3<T> &operator*=(T _v)
540 {
541 this->data[0] *= _v;
542 this->data[1] *= _v;
543 this->data[2] *= _v;
544
545 return *this;
546 }
547
553 public: bool Equal(const Vector3 &_v, const T &_tol) const
554 {
555 return equal<T>(this->data[0], _v[0], _tol)
556 && equal<T>(this->data[1], _v[1], _tol)
557 && equal<T>(this->data[2], _v[2], _tol);
558 }
559
564 public: bool operator==(const Vector3<T> &_v) const
565 {
566 return this->Equal(_v, static_cast<T>(1e-3));
567 }
568
573 public: bool operator!=(const Vector3<T> &_v) const
574 {
575 return !(*this == _v);
576 }
577
580 public: bool IsFinite() const
581 {
582 // std::isfinite works with floating point values,
583 // need to explicit cast to avoid ambiguity in vc++.
584 return std::isfinite(static_cast<double>(this->data[0])) &&
585 std::isfinite(static_cast<double>(this->data[1])) &&
586 std::isfinite(static_cast<double>(this->data[2]));
587 }
588
590 public: inline void Correct()
591 {
592 // std::isfinite works with floating point values,
593 // need to explicit cast to avoid ambiguity in vc++.
594 if (!std::isfinite(static_cast<double>(this->data[0])))
595 this->data[0] = 0;
596 if (!std::isfinite(static_cast<double>(this->data[1])))
597 this->data[1] = 0;
598 if (!std::isfinite(static_cast<double>(this->data[2])))
599 this->data[2] = 0;
600 }
601
606 public: T &operator[](const std::size_t _index)
607 {
608 return this->data[clamp(_index, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)];
609 }
610
615 public: T operator[](const std::size_t _index) const
616 {
617 return this->data[clamp(_index, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)];
618 }
619
622 public: void Round(int _precision)
623 {
624 this->data[0] = precision(this->data[0], _precision);
625 this->data[1] = precision(this->data[1], _precision);
626 this->data[2] = precision(this->data[2], _precision);
627 }
628
633 public: bool Equal(const Vector3<T> &_v) const
634 {
635 return equal<T>(this->data[0], _v[0]) &&
636 equal<T>(this->data[1], _v[1]) &&
637 equal<T>(this->data[2], _v[2]);
638 }
639
642 public: inline T X() const
643 {
644 return this->data[0];
645 }
646
649 public: inline T Y() const
650 {
651 return this->data[1];
652 }
653
656 public: inline T Z() const
657 {
658 return this->data[2];
659 }
660
663 public: inline T &X()
664 {
665 return this->data[0];
666 }
667
670 public: inline T &Y()
671 {
672 return this->data[1];
673 }
674
677 public: inline T &Z()
678 {
679 return this->data[2];
680 }
681
684 public: inline void X(const T &_v)
685 {
686 this->data[0] = _v;
687 }
688
691 public: inline void Y(const T &_v)
692 {
693 this->data[1] = _v;
694 }
695
698 public: inline void Z(const T &_v)
699 {
700 this->data[2] = _v;
701 }
702
707 public: bool operator<(const Vector3<T> &_pt) const
708 {
709 return this->data[0] < _pt[0] || this->data[1] < _pt[1] ||
710 this->data[2] < _pt[2];
711 }
712
717 public: friend std::ostream &operator<<(
719 {
720 for (auto i : {0, 1, 2})
721 {
722 if (i > 0)
723 _out << " ";
724
726 }
727
728 return _out;
729 }
730
735 public: friend std::istream &operator>>(
737 {
738 // Skip white spaces
739 _in.setf(std::ios_base::skipws);
740 T x, y, z;
741 _in >> x >> y >> z;
742 if (!_in.fail())
743 {
744 _pt.Set(x, y, z);
745 }
746 return _in;
747 }
748
750 private: T data[3];
751 };
752
753 namespace detail {
754
755 template<typename T>
756 constexpr Vector3<T> gVector3Zero(0, 0, 0);
757 template<typename T>
758 constexpr Vector3<T> gVector3One(1, 1, 1);
759 template<typename T>
760 constexpr Vector3<T> gVector3UnitX(1, 0, 0);
761 template<typename T>
762 constexpr Vector3<T> gVector3UnitY(0, 1, 0);
763 template<typename T>
764 constexpr Vector3<T> gVector3UnitZ(0, 0, 1);
765 template<typename T>
766 constexpr Vector3<T> gVector3NaN(
770 } // namespace detail
771
772 template<typename T>
773 const Vector3<T> &Vector3<T>::Zero = detail::gVector3Zero<T>;
774 template<typename T>
775 const Vector3<T> &Vector3<T>::One = detail::gVector3One<T>;
776 template<typename T>
777 const Vector3<T> &Vector3<T>::UnitX = detail::gVector3UnitX<T>;
778 template<typename T>
779 const Vector3<T> &Vector3<T>::UnitY = detail::gVector3UnitY<T>;
780 template<typename T>
781 const Vector3<T> &Vector3<T>::UnitZ = detail::gVector3UnitZ<T>;
782 template<typename T>
783 const Vector3<T> &Vector3<T>::NaN = detail::gVector3NaN<T>;
784
788 } // namespace GZ_MATH_VERSION_NAMESPACE
789} // namespace gz::math
790#endif // GZ_MATH_VECTOR3_HH_