Gazebo Math

API Reference

7.5.1
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 
28 namespace 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 
66  public: Vector4(const Vector4<T> &_v) = default;
67 
69  public: ~Vector4() = default;
70 
74  public: T Distance(const Vector4<T> &_pt) const
75  {
76  return static_cast<T>(sqrt(
77  (this->data[0]-_pt[0])*(this->data[0]-_pt[0]) +
78  (this->data[1]-_pt[1])*(this->data[1]-_pt[1]) +
79  (this->data[2]-_pt[2])*(this->data[2]-_pt[2]) +
80  (this->data[3]-_pt[3])*(this->data[3]-_pt[3])));
81  }
82 
89  public: T Distance(T _x, T _y, T _z, T _w) const
90  {
91  return this->Distance(Vector4(_x, _y, _z, _w));
92  }
93 
96  public: T Length() const
97  {
98  return static_cast<T>(sqrt(this->SquaredLength()));
99  }
100 
103  public: T SquaredLength() const
104  {
105  return
106  this->data[0] * this->data[0] +
107  this->data[1] * this->data[1] +
108  this->data[2] * this->data[2] +
109  this->data[3] * this->data[3];
110  }
111 
113  public: void Round()
114  {
115  this->data[0] = static_cast<T>(std::nearbyint(this->data[0]));
116  this->data[1] = static_cast<T>(std::nearbyint(this->data[1]));
117  this->data[2] = static_cast<T>(std::nearbyint(this->data[2]));
118  this->data[3] = static_cast<T>(std::nearbyint(this->data[3]));
119  }
120 
123  public: Vector4 Rounded() const
124  {
125  Vector4<T> result = *this;
126  result.Round();
127  return result;
128  }
129 
131  public: inline void Correct()
132  {
133  // std::isfinite works with floating point values,
134  // need to explicit cast to avoid ambiguity in vc++.
135  if (!std::isfinite(static_cast<double>(this->data[0])))
136  this->data[0] = 0;
137  if (!std::isfinite(static_cast<double>(this->data[1])))
138  this->data[1] = 0;
139  if (!std::isfinite(static_cast<double>(this->data[2])))
140  this->data[2] = 0;
141  if (!std::isfinite(static_cast<double>(this->data[3])))
142  this->data[3] = 0;
143  }
144 
146  public: void Normalize()
147  {
148  T d = this->Length();
149 
150  if (!equal<T>(d, static_cast<T>(0.0)))
151  {
152  this->data[0] /= d;
153  this->data[1] /= d;
154  this->data[2] /= d;
155  this->data[3] /= d;
156  }
157  }
158 
161  public: Vector4 Normalized() const
162  {
163  Vector4<T> result = *this;
164  result.Normalize();
165  return result;
166  }
167 
171  public: T Dot(const Vector4<T> &_v) const
172  {
173  return this->data[0] * _v[0] +
174  this->data[1] * _v[1] +
175  this->data[2] * _v[2] +
176  this->data[3] * _v[3];
177  }
178 
187  public: T AbsDot(const Vector4<T> &_v) const
188  {
189  return std::abs(this->data[0] * _v[0]) +
190  std::abs(this->data[1] * _v[1]) +
191  std::abs(this->data[2] * _v[2]) +
192  std::abs(this->data[3] * _v[3]);
193  }
194 
197  public: Vector4 Abs() const
198  {
199  return Vector4(std::abs(this->data[0]),
200  std::abs(this->data[1]),
201  std::abs(this->data[2]),
202  std::abs(this->data[3]));
203  }
204 
210  public: void Set(T _x = 0, T _y = 0, T _z = 0, T _w = 0)
211  {
212  this->data[0] = _x;
213  this->data[1] = _y;
214  this->data[2] = _z;
215  this->data[3] = _w;
216  }
217 
221  public: void Max(const Vector4<T> &_v)
222  {
223  this->data[0] = std::max(_v[0], this->data[0]);
224  this->data[1] = std::max(_v[1], this->data[1]);
225  this->data[2] = std::max(_v[2], this->data[2]);
226  this->data[3] = std::max(_v[3], this->data[3]);
227  }
228 
232  public: void Min(const Vector4<T> &_v)
233  {
234  this->data[0] = std::min(_v[0], this->data[0]);
235  this->data[1] = std::min(_v[1], this->data[1]);
236  this->data[2] = std::min(_v[2], this->data[2]);
237  this->data[3] = std::min(_v[3], this->data[3]);
238  }
239 
242  public: T Max() const
243  {
244  return *std::max_element(this->data, this->data+4);
245  }
246 
249  public: T Min() const
250  {
251  return *std::min_element(this->data, this->data+4);
252  }
253 
256  public: T Sum() const
257  {
258  return this->data[0] + this->data[1] + this->data[2] + this->data[3];
259  }
260 
264  public: Vector4<T> &operator=(const Vector4<T> &_v) = default;
265 
268  public: Vector4<T> &operator=(T _value)
269  {
270  this->data[0] = _value;
271  this->data[1] = _value;
272  this->data[2] = _value;
273  this->data[3] = _value;
274 
275  return *this;
276  }
277 
281  public: Vector4<T> operator+(const Vector4<T> &_v) const
282  {
283  return Vector4<T>(this->data[0] + _v[0],
284  this->data[1] + _v[1],
285  this->data[2] + _v[2],
286  this->data[3] + _v[3]);
287  }
288 
292  public: const Vector4<T> &operator+=(const Vector4<T> &_v)
293  {
294  this->data[0] += _v[0];
295  this->data[1] += _v[1];
296  this->data[2] += _v[2];
297  this->data[3] += _v[3];
298 
299  return *this;
300  }
301 
305  public: inline Vector4<T> operator+(const T _s) const
306  {
307  return Vector4<T>(this->data[0] + _s,
308  this->data[1] + _s,
309  this->data[2] + _s,
310  this->data[3] + _s);
311  }
312 
317  public: friend inline Vector4<T> operator+(const T _s,
318  const Vector4<T> &_v)
319  {
320  return _v + _s;
321  }
322 
326  public: const Vector4<T> &operator+=(const T _s)
327  {
328  this->data[0] += _s;
329  this->data[1] += _s;
330  this->data[2] += _s;
331  this->data[3] += _s;
332 
333  return *this;
334  }
335 
338  public: inline Vector4 operator-() const
339  {
340  return Vector4(-this->data[0], -this->data[1],
341  -this->data[2], -this->data[3]);
342  }
343 
347  public: Vector4<T> operator-(const Vector4<T> &_v) const
348  {
349  return Vector4<T>(this->data[0] - _v[0],
350  this->data[1] - _v[1],
351  this->data[2] - _v[2],
352  this->data[3] - _v[3]);
353  }
354 
358  public: const Vector4<T> &operator-=(const Vector4<T> &_v)
359  {
360  this->data[0] -= _v[0];
361  this->data[1] -= _v[1];
362  this->data[2] -= _v[2];
363  this->data[3] -= _v[3];
364 
365  return *this;
366  }
367 
371  public: inline Vector4<T> operator-(const T _s) const
372  {
373  return Vector4<T>(this->data[0] - _s,
374  this->data[1] - _s,
375  this->data[2] - _s,
376  this->data[3] - _s);
377  }
378 
383  public: friend inline Vector4<T> operator-(const T _s,
384  const Vector4<T> &_v)
385  {
386  return {_s - _v.X(), _s - _v.Y(), _s - _v.Z(), _s - _v.W()};
387  }
388 
392  public: const Vector4<T> &operator-=(const T _s)
393  {
394  this->data[0] -= _s;
395  this->data[1] -= _s;
396  this->data[2] -= _s;
397  this->data[3] -= _s;
398 
399  return *this;
400  }
401 
407  public: const Vector4<T> operator/(const Vector4<T> &_v) const
408  {
409  return Vector4<T>(this->data[0] / _v[0],
410  this->data[1] / _v[1],
411  this->data[2] / _v[2],
412  this->data[3] / _v[3]);
413  }
414 
420  public: const Vector4<T> &operator/=(const Vector4<T> &_v)
421  {
422  this->data[0] /= _v[0];
423  this->data[1] /= _v[1];
424  this->data[2] /= _v[2];
425  this->data[3] /= _v[3];
426 
427  return *this;
428  }
429 
435  public: const Vector4<T> operator/(T _v) const
436  {
437  return Vector4<T>(this->data[0] / _v, this->data[1] / _v,
438  this->data[2] / _v, this->data[3] / _v);
439  }
440 
444  public: const Vector4<T> &operator/=(T _v)
445  {
446  this->data[0] /= _v;
447  this->data[1] /= _v;
448  this->data[2] /= _v;
449  this->data[3] /= _v;
450 
451  return *this;
452  }
453 
459  public: const Vector4<T> operator*(const Vector4<T> &_pt) const
460  {
461  return Vector4<T>(this->data[0] * _pt[0],
462  this->data[1] * _pt[1],
463  this->data[2] * _pt[2],
464  this->data[3] * _pt[3]);
465  }
466 
470  public: const Vector4<T> operator*(const Matrix4<T> &_m) const
471  {
472  return Vector4<T>(
473  this->data[0]*_m(0, 0) + this->data[1]*_m(1, 0) +
474  this->data[2]*_m(2, 0) + this->data[3]*_m(3, 0),
475  this->data[0]*_m(0, 1) + this->data[1]*_m(1, 1) +
476  this->data[2]*_m(2, 1) + this->data[3]*_m(3, 1),
477  this->data[0]*_m(0, 2) + this->data[1]*_m(1, 2) +
478  this->data[2]*_m(2, 2) + this->data[3]*_m(3, 2),
479  this->data[0]*_m(0, 3) + this->data[1]*_m(1, 3) +
480  this->data[2]*_m(2, 3) + this->data[3]*_m(3, 3));
481  }
482 
488  public: const Vector4<T> &operator*=(const Vector4<T> &_pt)
489  {
490  this->data[0] *= _pt[0];
491  this->data[1] *= _pt[1];
492  this->data[2] *= _pt[2];
493  this->data[3] *= _pt[3];
494 
495  return *this;
496  }
497 
501  public: const Vector4<T> operator*(T _v) const
502  {
503  return Vector4<T>(this->data[0] * _v, this->data[1] * _v,
504  this->data[2] * _v, this->data[3] * _v);
505  }
506 
511  public: friend inline const Vector4 operator*(const T _s,
512  const Vector4 &_v)
513  {
514  return Vector4(_v * _s);
515  }
516 
520  public: const Vector4<T> &operator*=(T _v)
521  {
522  this->data[0] *= _v;
523  this->data[1] *= _v;
524  this->data[2] *= _v;
525  this->data[3] *= _v;
526 
527  return *this;
528  }
529 
535  public: bool Equal(const Vector4 &_v, const T &_tol) const
536  {
537  return equal<T>(this->data[0], _v[0], _tol)
538  && equal<T>(this->data[1], _v[1], _tol)
539  && equal<T>(this->data[2], _v[2], _tol)
540  && equal<T>(this->data[3], _v[3], _tol);
541  }
542 
547  public: bool operator==(const Vector4<T> &_v) const
548  {
549  return this->Equal(_v, static_cast<T>(1e-6));
550  }
551 
556  public: bool operator!=(const Vector4<T> &_pt) const
557  {
558  return !(*this == _pt);
559  }
560 
563  public: bool IsFinite() const
564  {
565  // std::isfinite works with floating point values,
566  // need to explicit cast to avoid ambiguity in vc++.
567  return std::isfinite(static_cast<double>(this->data[0])) &&
568  std::isfinite(static_cast<double>(this->data[1])) &&
569  std::isfinite(static_cast<double>(this->data[2])) &&
570  std::isfinite(static_cast<double>(this->data[3]));
571  }
572 
577  public: T &operator[](const std::size_t _index)
578  {
579  return this->data[clamp(_index, GZ_ZERO_SIZE_T, GZ_THREE_SIZE_T)];
580  }
581 
586  public: T operator[](const std::size_t _index) const
587  {
588  return this->data[clamp(_index, GZ_ZERO_SIZE_T, GZ_THREE_SIZE_T)];
589  }
590 
593  public: T &X()
594  {
595  return this->data[0];
596  }
597 
600  public: T &Y()
601  {
602  return this->data[1];
603  }
604 
607  public: T &Z()
608  {
609  return this->data[2];
610  }
611 
614  public: T &W()
615  {
616  return this->data[3];
617  }
618 
621  public: T X() const
622  {
623  return this->data[0];
624  }
625 
628  public: T Y() const
629  {
630  return this->data[1];
631  }
632 
635  public: T Z() const
636  {
637  return this->data[2];
638  }
639 
642  public: T W() const
643  {
644  return this->data[3];
645  }
646 
649  public: inline void X(const T &_v)
650  {
651  this->data[0] = _v;
652  }
653 
656  public: inline void Y(const T &_v)
657  {
658  this->data[1] = _v;
659  }
660 
663  public: inline void Z(const T &_v)
664  {
665  this->data[2] = _v;
666  }
667 
670  public: inline void W(const T &_v)
671  {
672  this->data[3] = _v;
673  }
674 
679  public: bool operator<(const Vector4<T> &_pt) const
680  {
681  return this->data[0] < _pt[0] || this->data[1] < _pt[1] ||
682  this->data[2] < _pt[2] || this->data[3] < _pt[3];
683  }
684 
689  public: friend std::ostream &operator<<(
690  std::ostream &_out, const gz::math::Vector4<T> &_pt)
691  {
692  for (auto i : {0, 1, 2, 3})
693  {
694  if (i > 0)
695  _out << " ";
696 
697  appendToStream(_out, _pt[i]);
698  }
699  return _out;
700  }
701 
706  public: friend std::istream &operator>>(
708  {
709  T x, y, z, w;
710 
711  // Skip white spaces
712  _in.setf(std::ios_base::skipws);
713  _in >> x >> y >> z >> w;
714  if (!_in.fail())
715  {
716  _pt.Set(x, y, z, w);
717  }
718  return _in;
719  }
720 
722  private: T data[4];
723  };
724 
725  namespace detail {
726 
727  template<typename T>
728  constexpr Vector4<T> gVector4Zero(0, 0, 0, 0);
729 
730  template<typename T>
731  constexpr Vector4<T> gVector4One(1, 1, 1, 1);
732 
733  template<typename T>
734  constexpr Vector4<T> gVector4NaN(
739 
740  } // namespace detail
741 
742  template<typename T>
743  const Vector4<T> &Vector4<T>::Zero = detail::gVector4Zero<T>;
744 
745  template<typename T>
746  const Vector4<T> &Vector4<T>::One = detail::gVector4One<T>;
747 
748  template<typename T>
749  const Vector4<T> &Vector4<T>::NaN = detail::gVector4NaN<T>;
750 
754  } // namespace GZ_MATH_VERSION_NAMESPACE
755 } // namespace gz::math
756 #endif // GZ_MATH_VERSION_NAMESPACE