Gazebo Math

API Reference

7.4.0
gz/math/Vector4.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_VECTOR4_HH_
18#define GZ_MATH_VECTOR4_HH_
19
20#include <algorithm>
21#include <cmath>
22#include <limits>
23
24#include <gz/math/Matrix4.hh>
25#include <gz/math/Helpers.hh>
26#include <gz/math/config.hh>
27
28namespace gz
29{
30 namespace math
31 {
32 // Inline bracket to help doxygen filtering.
33 inline namespace GZ_MATH_VERSION_NAMESPACE {
34 //
37 template<typename T>
38 class Vector4
39 {
41 public: static const Vector4<T> &Zero;
42
44 public: static const Vector4<T> &One;
45
47 public: static const Vector4 &NaN;
48
50 public: constexpr Vector4()
51 : data{0, 0, 0, 0}
52 {
53 }
54
60 public: constexpr Vector4(const T &_x, const T &_y, const T &_z,
61 const T &_w)
62 : data{_x, _y, _z, _w}
63 {
64 }
65
68 public: Vector4(const Vector4<T> &_v) = default;
69
71 public: ~Vector4() = default;
72
76 public: T Distance(const Vector4<T> &_pt) const
77 {
78 return static_cast<T>(sqrt(
79 (this->data[0]-_pt[0])*(this->data[0]-_pt[0]) +
80 (this->data[1]-_pt[1])*(this->data[1]-_pt[1]) +
81 (this->data[2]-_pt[2])*(this->data[2]-_pt[2]) +
82 (this->data[3]-_pt[3])*(this->data[3]-_pt[3])));
83 }
84
91 public: T Distance(T _x, T _y, T _z, T _w) const
92 {
93 return this->Distance(Vector4(_x, _y, _z, _w));
94 }
95
98 public: T Length() const
99 {
100 return static_cast<T>(sqrt(this->SquaredLength()));
101 }
102
105 public: T SquaredLength() const
106 {
107 return
108 this->data[0] * this->data[0] +
109 this->data[1] * this->data[1] +
110 this->data[2] * this->data[2] +
111 this->data[3] * this->data[3];
112 }
113
115 public: void Round()
116 {
117 this->data[0] = static_cast<T>(std::nearbyint(this->data[0]));
118 this->data[1] = static_cast<T>(std::nearbyint(this->data[1]));
119 this->data[2] = static_cast<T>(std::nearbyint(this->data[2]));
120 this->data[3] = static_cast<T>(std::nearbyint(this->data[3]));
121 }
122
125 public: Vector4 Rounded() const
126 {
127 Vector4<T> result = *this;
128 result.Round();
129 return result;
130 }
131
133 public: inline void Correct()
134 {
135 // std::isfinite works with floating point values,
136 // need to explicit cast to avoid ambiguity in vc++.
137 if (!std::isfinite(static_cast<double>(this->data[0])))
138 this->data[0] = 0;
139 if (!std::isfinite(static_cast<double>(this->data[1])))
140 this->data[1] = 0;
141 if (!std::isfinite(static_cast<double>(this->data[2])))
142 this->data[2] = 0;
143 if (!std::isfinite(static_cast<double>(this->data[3])))
144 this->data[3] = 0;
145 }
146
148 public: void Normalize()
149 {
150 T d = this->Length();
151
152 if (!equal<T>(d, static_cast<T>(0.0)))
153 {
154 this->data[0] /= d;
155 this->data[1] /= d;
156 this->data[2] /= d;
157 this->data[3] /= d;
158 }
159 }
160
163 public: Vector4 Normalized() const
164 {
165 Vector4<T> result = *this;
166 result.Normalize();
167 return result;
168 }
169
173 public: T Dot(const Vector4<T> &_v) const
174 {
175 return this->data[0] * _v[0] +
176 this->data[1] * _v[1] +
177 this->data[2] * _v[2] +
178 this->data[3] * _v[3];
179 }
180
189 public: T AbsDot(const Vector4<T> &_v) const
190 {
191 return std::abs(this->data[0] * _v[0]) +
192 std::abs(this->data[1] * _v[1]) +
193 std::abs(this->data[2] * _v[2]) +
194 std::abs(this->data[3] * _v[3]);
195 }
196
199 public: Vector4 Abs() const
200 {
201 return Vector4(std::abs(this->data[0]),
202 std::abs(this->data[1]),
203 std::abs(this->data[2]),
204 std::abs(this->data[3]));
205 }
206
212 public: void Set(T _x = 0, T _y = 0, T _z = 0, T _w = 0)
213 {
214 this->data[0] = _x;
215 this->data[1] = _y;
216 this->data[2] = _z;
217 this->data[3] = _w;
218 }
219
223 public: void Max(const Vector4<T> &_v)
224 {
225 this->data[0] = std::max(_v[0], this->data[0]);
226 this->data[1] = std::max(_v[1], this->data[1]);
227 this->data[2] = std::max(_v[2], this->data[2]);
228 this->data[3] = std::max(_v[3], this->data[3]);
229 }
230
234 public: void Min(const Vector4<T> &_v)
235 {
236 this->data[0] = std::min(_v[0], this->data[0]);
237 this->data[1] = std::min(_v[1], this->data[1]);
238 this->data[2] = std::min(_v[2], this->data[2]);
239 this->data[3] = std::min(_v[3], this->data[3]);
240 }
241
244 public: T Max() const
245 {
246 return *std::max_element(this->data, this->data+4);
247 }
248
251 public: T Min() const
252 {
253 return *std::min_element(this->data, this->data+4);
254 }
255
258 public: T Sum() const
259 {
260 return this->data[0] + this->data[1] + this->data[2] + this->data[3];
261 }
262
266 public: Vector4<T> &operator=(const Vector4<T> &_v) = default;
267
270 public: Vector4<T> &operator=(T _value)
271 {
272 this->data[0] = _value;
273 this->data[1] = _value;
274 this->data[2] = _value;
275 this->data[3] = _value;
276
277 return *this;
278 }
279
283 public: Vector4<T> operator+(const Vector4<T> &_v) const
284 {
285 return Vector4<T>(this->data[0] + _v[0],
286 this->data[1] + _v[1],
287 this->data[2] + _v[2],
288 this->data[3] + _v[3]);
289 }
290
294 public: const Vector4<T> &operator+=(const Vector4<T> &_v)
295 {
296 this->data[0] += _v[0];
297 this->data[1] += _v[1];
298 this->data[2] += _v[2];
299 this->data[3] += _v[3];
300
301 return *this;
302 }
303
307 public: inline Vector4<T> operator+(const T _s) const
308 {
309 return Vector4<T>(this->data[0] + _s,
310 this->data[1] + _s,
311 this->data[2] + _s,
312 this->data[3] + _s);
313 }
314
319 public: friend inline Vector4<T> operator+(const T _s,
320 const Vector4<T> &_v)
321 {
322 return _v + _s;
323 }
324
328 public: const Vector4<T> &operator+=(const T _s)
329 {
330 this->data[0] += _s;
331 this->data[1] += _s;
332 this->data[2] += _s;
333 this->data[3] += _s;
334
335 return *this;
336 }
337
340 public: inline Vector4 operator-() const
341 {
342 return Vector4(-this->data[0], -this->data[1],
343 -this->data[2], -this->data[3]);
344 }
345
349 public: Vector4<T> operator-(const Vector4<T> &_v) const
350 {
351 return Vector4<T>(this->data[0] - _v[0],
352 this->data[1] - _v[1],
353 this->data[2] - _v[2],
354 this->data[3] - _v[3]);
355 }
356
360 public: const Vector4<T> &operator-=(const Vector4<T> &_v)
361 {
362 this->data[0] -= _v[0];
363 this->data[1] -= _v[1];
364 this->data[2] -= _v[2];
365 this->data[3] -= _v[3];
366
367 return *this;
368 }
369
373 public: inline Vector4<T> operator-(const T _s) const
374 {
375 return Vector4<T>(this->data[0] - _s,
376 this->data[1] - _s,
377 this->data[2] - _s,
378 this->data[3] - _s);
379 }
380
385 public: friend inline Vector4<T> operator-(const T _s,
386 const Vector4<T> &_v)
387 {
388 return {_s - _v.X(), _s - _v.Y(), _s - _v.Z(), _s - _v.W()};
389 }
390
394 public: const Vector4<T> &operator-=(const T _s)
395 {
396 this->data[0] -= _s;
397 this->data[1] -= _s;
398 this->data[2] -= _s;
399 this->data[3] -= _s;
400
401 return *this;
402 }
403
409 public: const Vector4<T> operator/(const Vector4<T> &_v) const
410 {
411 return Vector4<T>(this->data[0] / _v[0],
412 this->data[1] / _v[1],
413 this->data[2] / _v[2],
414 this->data[3] / _v[3]);
415 }
416
422 public: const Vector4<T> &operator/=(const Vector4<T> &_v)
423 {
424 this->data[0] /= _v[0];
425 this->data[1] /= _v[1];
426 this->data[2] /= _v[2];
427 this->data[3] /= _v[3];
428
429 return *this;
430 }
431
437 public: const Vector4<T> operator/(T _v) const
438 {
439 return Vector4<T>(this->data[0] / _v, this->data[1] / _v,
440 this->data[2] / _v, this->data[3] / _v);
441 }
442
446 public: const Vector4<T> &operator/=(T _v)
447 {
448 this->data[0] /= _v;
449 this->data[1] /= _v;
450 this->data[2] /= _v;
451 this->data[3] /= _v;
452
453 return *this;
454 }
455
461 public: const Vector4<T> operator*(const Vector4<T> &_pt) const
462 {
463 return Vector4<T>(this->data[0] * _pt[0],
464 this->data[1] * _pt[1],
465 this->data[2] * _pt[2],
466 this->data[3] * _pt[3]);
467 }
468
472 public: const Vector4<T> operator*(const Matrix4<T> &_m) const
473 {
474 return Vector4<T>(
475 this->data[0]*_m(0, 0) + this->data[1]*_m(1, 0) +
476 this->data[2]*_m(2, 0) + this->data[3]*_m(3, 0),
477 this->data[0]*_m(0, 1) + this->data[1]*_m(1, 1) +
478 this->data[2]*_m(2, 1) + this->data[3]*_m(3, 1),
479 this->data[0]*_m(0, 2) + this->data[1]*_m(1, 2) +
480 this->data[2]*_m(2, 2) + this->data[3]*_m(3, 2),
481 this->data[0]*_m(0, 3) + this->data[1]*_m(1, 3) +
482 this->data[2]*_m(2, 3) + this->data[3]*_m(3, 3));
483 }
484
490 public: const Vector4<T> &operator*=(const Vector4<T> &_pt)
491 {
492 this->data[0] *= _pt[0];
493 this->data[1] *= _pt[1];
494 this->data[2] *= _pt[2];
495 this->data[3] *= _pt[3];
496
497 return *this;
498 }
499
503 public: const Vector4<T> operator*(T _v) const
504 {
505 return Vector4<T>(this->data[0] * _v, this->data[1] * _v,
506 this->data[2] * _v, this->data[3] * _v);
507 }
508
513 public: friend inline const Vector4 operator*(const T _s,
514 const Vector4 &_v)
515 {
516 return Vector4(_v * _s);
517 }
518
522 public: const Vector4<T> &operator*=(T _v)
523 {
524 this->data[0] *= _v;
525 this->data[1] *= _v;
526 this->data[2] *= _v;
527 this->data[3] *= _v;
528
529 return *this;
530 }
531
537 public: bool Equal(const Vector4 &_v, const T &_tol) const
538 {
539 return equal<T>(this->data[0], _v[0], _tol)
540 && equal<T>(this->data[1], _v[1], _tol)
541 && equal<T>(this->data[2], _v[2], _tol)
542 && equal<T>(this->data[3], _v[3], _tol);
543 }
544
549 public: bool operator==(const Vector4<T> &_v) const
550 {
551 return this->Equal(_v, static_cast<T>(1e-6));
552 }
553
558 public: bool operator!=(const Vector4<T> &_pt) const
559 {
560 return !(*this == _pt);
561 }
562
565 public: bool IsFinite() const
566 {
567 // std::isfinite works with floating point values,
568 // need to explicit cast to avoid ambiguity in vc++.
569 return std::isfinite(static_cast<double>(this->data[0])) &&
570 std::isfinite(static_cast<double>(this->data[1])) &&
571 std::isfinite(static_cast<double>(this->data[2])) &&
572 std::isfinite(static_cast<double>(this->data[3]));
573 }
574
579 public: T &operator[](const std::size_t _index)
580 {
581 return this->data[clamp(_index, GZ_ZERO_SIZE_T, GZ_THREE_SIZE_T)];
582 }
583
588 public: T operator[](const std::size_t _index) const
589 {
590 return this->data[clamp(_index, GZ_ZERO_SIZE_T, GZ_THREE_SIZE_T)];
591 }
592
595 public: T &X()
596 {
597 return this->data[0];
598 }
599
602 public: T &Y()
603 {
604 return this->data[1];
605 }
606
609 public: T &Z()
610 {
611 return this->data[2];
612 }
613
616 public: T &W()
617 {
618 return this->data[3];
619 }
620
623 public: T X() const
624 {
625 return this->data[0];
626 }
627
630 public: T Y() const
631 {
632 return this->data[1];
633 }
634
637 public: T Z() const
638 {
639 return this->data[2];
640 }
641
644 public: T W() const
645 {
646 return this->data[3];
647 }
648
651 public: inline void X(const T &_v)
652 {
653 this->data[0] = _v;
654 }
655
658 public: inline void Y(const T &_v)
659 {
660 this->data[1] = _v;
661 }
662
665 public: inline void Z(const T &_v)
666 {
667 this->data[2] = _v;
668 }
669
672 public: inline void W(const T &_v)
673 {
674 this->data[3] = _v;
675 }
676
681 public: bool operator<(const Vector4<T> &_pt) const
682 {
683 return this->data[0] < _pt[0] || this->data[1] < _pt[1] ||
684 this->data[2] < _pt[2] || this->data[3] < _pt[3];
685 }
686
691 public: friend std::ostream &operator<<(
692 std::ostream &_out, const gz::math::Vector4<T> &_pt)
693 {
694 for (auto i : {0, 1, 2, 3})
695 {
696 if (i > 0)
697 _out << " ";
698
699 appendToStream(_out, _pt[i]);
700 }
701 return _out;
702 }
703
708 public: friend std::istream &operator>>(
710 {
711 T x, y, z, w;
712
713 // Skip white spaces
714 _in.setf(std::ios_base::skipws);
715 _in >> x >> y >> z >> w;
716 if (!_in.fail())
717 {
718 _pt.Set(x, y, z, w);
719 }
720 return _in;
721 }
722
724 private: T data[4];
725 };
726
727 namespace detail {
728
729 template<typename T>
730 constexpr Vector4<T> gVector4Zero(0, 0, 0, 0);
731
732 template<typename T>
733 constexpr Vector4<T> gVector4One(1, 1, 1, 1);
734
735 template<typename T>
736 constexpr Vector4<T> gVector4NaN(
741
742 } // namespace detail
743
744 template<typename T>
745 const Vector4<T> &Vector4<T>::Zero = detail::gVector4Zero<T>;
746
747 template<typename T>
748 const Vector4<T> &Vector4<T>::One = detail::gVector4One<T>;
749
750 template<typename T>
751 const Vector4<T> &Vector4<T>::NaN = detail::gVector4NaN<T>;
752
756 }
757 }
758}
759#endif