Gazebo Math

API Reference

8.1.0
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::math
29{
30 // Inline bracket to help doxygen filtering.
31 inline namespace GZ_MATH_VERSION_NAMESPACE {
32 //
35 template<typename T>
36 class Vector4
37 {
39 public: static const Vector4<T> &Zero;
40
42 public: static const Vector4<T> &One;
43
45 public: static const Vector4 &NaN;
46
48 public: constexpr Vector4()
49 : data{0, 0, 0, 0}
50 {
51 }
52
58 public: constexpr Vector4(const T &_x, const T &_y, const T &_z,
59 const T &_w)
60 : data{_x, _y, _z, _w}
61 {
62 }
63
67 public: T Distance(const Vector4<T> &_pt) const
68 {
69 return static_cast<T>(sqrt(
70 (this->data[0]-_pt[0])*(this->data[0]-_pt[0]) +
71 (this->data[1]-_pt[1])*(this->data[1]-_pt[1]) +
72 (this->data[2]-_pt[2])*(this->data[2]-_pt[2]) +
73 (this->data[3]-_pt[3])*(this->data[3]-_pt[3])));
74 }
75
82 public: T Distance(T _x, T _y, T _z, T _w) const
83 {
84 return this->Distance(Vector4(_x, _y, _z, _w));
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 this->data[2] * this->data[2] +
102 this->data[3] * this->data[3];
103 }
104
106 public: void Round()
107 {
108 this->data[0] = static_cast<T>(std::nearbyint(this->data[0]));
109 this->data[1] = static_cast<T>(std::nearbyint(this->data[1]));
110 this->data[2] = static_cast<T>(std::nearbyint(this->data[2]));
111 this->data[3] = static_cast<T>(std::nearbyint(this->data[3]));
112 }
113
116 public: Vector4 Rounded() const
117 {
118 Vector4<T> result = *this;
119 result.Round();
120 return result;
121 }
122
124 public: inline void Correct()
125 {
126 // std::isfinite works with floating point values,
127 // need to explicit cast to avoid ambiguity in vc++.
128 if (!std::isfinite(static_cast<double>(this->data[0])))
129 this->data[0] = 0;
130 if (!std::isfinite(static_cast<double>(this->data[1])))
131 this->data[1] = 0;
132 if (!std::isfinite(static_cast<double>(this->data[2])))
133 this->data[2] = 0;
134 if (!std::isfinite(static_cast<double>(this->data[3])))
135 this->data[3] = 0;
136 }
137
139 public: void Normalize()
140 {
141 T d = this->Length();
142
143 if (!equal<T>(d, static_cast<T>(0.0)))
144 {
145 this->data[0] /= d;
146 this->data[1] /= d;
147 this->data[2] /= d;
148 this->data[3] /= d;
149 }
150 }
151
154 public: Vector4 Normalized() const
155 {
156 Vector4<T> result = *this;
157 result.Normalize();
158 return result;
159 }
160
164 public: T Dot(const Vector4<T> &_v) const
165 {
166 return this->data[0] * _v[0] +
167 this->data[1] * _v[1] +
168 this->data[2] * _v[2] +
169 this->data[3] * _v[3];
170 }
171
180 public: T AbsDot(const Vector4<T> &_v) const
181 {
182 return std::abs(this->data[0] * _v[0]) +
183 std::abs(this->data[1] * _v[1]) +
184 std::abs(this->data[2] * _v[2]) +
185 std::abs(this->data[3] * _v[3]);
186 }
187
190 public: Vector4 Abs() const
191 {
192 return Vector4(std::abs(this->data[0]),
193 std::abs(this->data[1]),
194 std::abs(this->data[2]),
195 std::abs(this->data[3]));
196 }
197
203 public: void Set(T _x = 0, T _y = 0, T _z = 0, T _w = 0)
204 {
205 this->data[0] = _x;
206 this->data[1] = _y;
207 this->data[2] = _z;
208 this->data[3] = _w;
209 }
210
214 public: void Max(const Vector4<T> &_v)
215 {
216 this->data[0] = std::max(_v[0], this->data[0]);
217 this->data[1] = std::max(_v[1], this->data[1]);
218 this->data[2] = std::max(_v[2], this->data[2]);
219 this->data[3] = std::max(_v[3], this->data[3]);
220 }
221
225 public: void Min(const Vector4<T> &_v)
226 {
227 this->data[0] = std::min(_v[0], this->data[0]);
228 this->data[1] = std::min(_v[1], this->data[1]);
229 this->data[2] = std::min(_v[2], this->data[2]);
230 this->data[3] = std::min(_v[3], this->data[3]);
231 }
232
235 public: T Max() const
236 {
237 return *std::max_element(this->data, this->data+4);
238 }
239
242 public: T Min() const
243 {
244 return *std::min_element(this->data, this->data+4);
245 }
246
249 public: T Sum() const
250 {
251 return this->data[0] + this->data[1] + this->data[2] + this->data[3];
252 }
253
257 {
258 this->data[0] = _value;
259 this->data[1] = _value;
260 this->data[2] = _value;
261 this->data[3] = _value;
262
263 return *this;
264 }
265
269 public: Vector4<T> operator+(const Vector4<T> &_v) const
270 {
271 return Vector4<T>(this->data[0] + _v[0],
272 this->data[1] + _v[1],
273 this->data[2] + _v[2],
274 this->data[3] + _v[3]);
275 }
276
280 public: const Vector4<T> &operator+=(const Vector4<T> &_v)
281 {
282 this->data[0] += _v[0];
283 this->data[1] += _v[1];
284 this->data[2] += _v[2];
285 this->data[3] += _v[3];
286
287 return *this;
288 }
289
293 public: inline Vector4<T> operator+(const T _s) const
294 {
295 return Vector4<T>(this->data[0] + _s,
296 this->data[1] + _s,
297 this->data[2] + _s,
298 this->data[3] + _s);
299 }
300
305 public: friend inline Vector4<T> operator+(const T _s,
306 const Vector4<T> &_v)
307 {
308 return _v + _s;
309 }
310
314 public: const Vector4<T> &operator+=(const T _s)
315 {
316 this->data[0] += _s;
317 this->data[1] += _s;
318 this->data[2] += _s;
319 this->data[3] += _s;
320
321 return *this;
322 }
323
326 public: inline Vector4 operator-() const
327 {
328 return Vector4(-this->data[0], -this->data[1],
329 -this->data[2], -this->data[3]);
330 }
331
335 public: Vector4<T> operator-(const Vector4<T> &_v) const
336 {
337 return Vector4<T>(this->data[0] - _v[0],
338 this->data[1] - _v[1],
339 this->data[2] - _v[2],
340 this->data[3] - _v[3]);
341 }
342
346 public: const Vector4<T> &operator-=(const Vector4<T> &_v)
347 {
348 this->data[0] -= _v[0];
349 this->data[1] -= _v[1];
350 this->data[2] -= _v[2];
351 this->data[3] -= _v[3];
352
353 return *this;
354 }
355
359 public: inline Vector4<T> operator-(const T _s) const
360 {
361 return Vector4<T>(this->data[0] - _s,
362 this->data[1] - _s,
363 this->data[2] - _s,
364 this->data[3] - _s);
365 }
366
371 public: friend inline Vector4<T> operator-(const T _s,
372 const Vector4<T> &_v)
373 {
374 return {_s - _v.X(), _s - _v.Y(), _s - _v.Z(), _s - _v.W()};
375 }
376
380 public: const Vector4<T> &operator-=(const T _s)
381 {
382 this->data[0] -= _s;
383 this->data[1] -= _s;
384 this->data[2] -= _s;
385 this->data[3] -= _s;
386
387 return *this;
388 }
389
395 public: const Vector4<T> operator/(const Vector4<T> &_v) const
396 {
397 return Vector4<T>(this->data[0] / _v[0],
398 this->data[1] / _v[1],
399 this->data[2] / _v[2],
400 this->data[3] / _v[3]);
401 }
402
408 public: const Vector4<T> &operator/=(const Vector4<T> &_v)
409 {
410 this->data[0] /= _v[0];
411 this->data[1] /= _v[1];
412 this->data[2] /= _v[2];
413 this->data[3] /= _v[3];
414
415 return *this;
416 }
417
423 public: const Vector4<T> operator/(T _v) const
424 {
425 return Vector4<T>(this->data[0] / _v, this->data[1] / _v,
426 this->data[2] / _v, this->data[3] / _v);
427 }
428
432 public: const Vector4<T> &operator/=(T _v)
433 {
434 this->data[0] /= _v;
435 this->data[1] /= _v;
436 this->data[2] /= _v;
437 this->data[3] /= _v;
438
439 return *this;
440 }
441
447 public: const Vector4<T> operator*(const Vector4<T> &_pt) const
448 {
449 return Vector4<T>(this->data[0] * _pt[0],
450 this->data[1] * _pt[1],
451 this->data[2] * _pt[2],
452 this->data[3] * _pt[3]);
453 }
454
458 public: const Vector4<T> operator*(const Matrix4<T> &_m) const
459 {
460 return Vector4<T>(
461 this->data[0]*_m(0, 0) + this->data[1]*_m(1, 0) +
462 this->data[2]*_m(2, 0) + this->data[3]*_m(3, 0),
463 this->data[0]*_m(0, 1) + this->data[1]*_m(1, 1) +
464 this->data[2]*_m(2, 1) + this->data[3]*_m(3, 1),
465 this->data[0]*_m(0, 2) + this->data[1]*_m(1, 2) +
466 this->data[2]*_m(2, 2) + this->data[3]*_m(3, 2),
467 this->data[0]*_m(0, 3) + this->data[1]*_m(1, 3) +
468 this->data[2]*_m(2, 3) + this->data[3]*_m(3, 3));
469 }
470
476 public: const Vector4<T> &operator*=(const Vector4<T> &_pt)
477 {
478 this->data[0] *= _pt[0];
479 this->data[1] *= _pt[1];
480 this->data[2] *= _pt[2];
481 this->data[3] *= _pt[3];
482
483 return *this;
484 }
485
489 public: const Vector4<T> operator*(T _v) const
490 {
491 return Vector4<T>(this->data[0] * _v, this->data[1] * _v,
492 this->data[2] * _v, this->data[3] * _v);
493 }
494
499 public: friend inline const Vector4 operator*(const T _s,
500 const Vector4 &_v)
501 {
502 return Vector4(_v * _s);
503 }
504
508 public: const Vector4<T> &operator*=(T _v)
509 {
510 this->data[0] *= _v;
511 this->data[1] *= _v;
512 this->data[2] *= _v;
513 this->data[3] *= _v;
514
515 return *this;
516 }
517
523 public: bool Equal(const Vector4 &_v, const T &_tol) const
524 {
525 return equal<T>(this->data[0], _v[0], _tol)
526 && equal<T>(this->data[1], _v[1], _tol)
527 && equal<T>(this->data[2], _v[2], _tol)
528 && equal<T>(this->data[3], _v[3], _tol);
529 }
530
535 public: bool operator==(const Vector4<T> &_v) const
536 {
537 return this->Equal(_v, static_cast<T>(1e-6));
538 }
539
544 public: bool operator!=(const Vector4<T> &_pt) const
545 {
546 return !(*this == _pt);
547 }
548
551 public: bool IsFinite() const
552 {
553 // std::isfinite works with floating point values,
554 // need to explicit cast to avoid ambiguity in vc++.
555 return std::isfinite(static_cast<double>(this->data[0])) &&
556 std::isfinite(static_cast<double>(this->data[1])) &&
557 std::isfinite(static_cast<double>(this->data[2])) &&
558 std::isfinite(static_cast<double>(this->data[3]));
559 }
560
565 public: T &operator[](const std::size_t _index)
566 {
567 return this->data[clamp(_index, GZ_ZERO_SIZE_T, GZ_THREE_SIZE_T)];
568 }
569
574 public: T operator[](const std::size_t _index) const
575 {
576 return this->data[clamp(_index, GZ_ZERO_SIZE_T, GZ_THREE_SIZE_T)];
577 }
578
581 public: T &X()
582 {
583 return this->data[0];
584 }
585
588 public: T &Y()
589 {
590 return this->data[1];
591 }
592
595 public: T &Z()
596 {
597 return this->data[2];
598 }
599
602 public: T &W()
603 {
604 return this->data[3];
605 }
606
609 public: T X() const
610 {
611 return this->data[0];
612 }
613
616 public: T Y() const
617 {
618 return this->data[1];
619 }
620
623 public: T Z() const
624 {
625 return this->data[2];
626 }
627
630 public: T W() const
631 {
632 return this->data[3];
633 }
634
637 public: inline void X(const T &_v)
638 {
639 this->data[0] = _v;
640 }
641
644 public: inline void Y(const T &_v)
645 {
646 this->data[1] = _v;
647 }
648
651 public: inline void Z(const T &_v)
652 {
653 this->data[2] = _v;
654 }
655
658 public: inline void W(const T &_v)
659 {
660 this->data[3] = _v;
661 }
662
667 public: bool operator<(const Vector4<T> &_pt) const
668 {
669 return this->data[0] < _pt[0] || this->data[1] < _pt[1] ||
670 this->data[2] < _pt[2] || this->data[3] < _pt[3];
671 }
672
677 public: friend std::ostream &operator<<(
679 {
680 for (auto i : {0, 1, 2, 3})
681 {
682 if (i > 0)
683 _out << " ";
684
686 }
687 return _out;
688 }
689
694 public: friend std::istream &operator>>(
696 {
697 T x, y, z, w;
698
699 // Skip white spaces
700 _in.setf(std::ios_base::skipws);
701 _in >> x >> y >> z >> w;
702 if (!_in.fail())
703 {
704 _pt.Set(x, y, z, w);
705 }
706 return _in;
707 }
708
710 private: T data[4];
711 };
712
713 namespace detail {
714
715 template<typename T>
716 constexpr Vector4<T> gVector4Zero(0, 0, 0, 0);
717
718 template<typename T>
719 constexpr Vector4<T> gVector4One(1, 1, 1, 1);
720
721 template<typename T>
722 constexpr Vector4<T> gVector4NaN(
727
728 } // namespace detail
729
730 template<typename T>
731 const Vector4<T> &Vector4<T>::Zero = detail::gVector4Zero<T>;
732
733 template<typename T>
734 const Vector4<T> &Vector4<T>::One = detail::gVector4One<T>;
735
736 template<typename T>
737 const Vector4<T> &Vector4<T>::NaN = detail::gVector4NaN<T>;
738
742 } // namespace GZ_MATH_VERSION_NAMESPACE
743} // namespace gz::math
744#endif // GZ_MATH_VERSION_NAMESPACE