Ignition Math

API Reference

6.8.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 IGNITION_MATH_VECTOR4_HH_
18 #define IGNITION_MATH_VECTOR4_HH_
19 
20 #include <algorithm>
21 #include <limits>
22 
23 #include <ignition/math/Matrix4.hh>
24 #include <ignition/math/Helpers.hh>
25 #include <ignition/math/config.hh>
26 
27 namespace ignition
28 {
29  namespace math
30  {
31  // Inline bracket to help doxygen filtering.
32  inline namespace IGNITION_MATH_VERSION_NAMESPACE {
33  //
36  template<typename T>
37  class Vector4
38  {
40  public: static const Vector4<T> Zero;
41 
43  public: static const Vector4<T> One;
44 
46  public: static const Vector4 NaN;
47 
49  public: Vector4()
50  {
51  this->data[0] = this->data[1] = this->data[2] = this->data[3] = 0;
52  }
53 
59  public: Vector4(const T &_x, const T &_y, const T &_z, const T &_w)
60  {
61  this->data[0] = _x;
62  this->data[1] = _y;
63  this->data[2] = _z;
64  this->data[3] = _w;
65  }
66 
69  public: Vector4(const Vector4<T> &_v)
70  {
71  this->data[0] = _v[0];
72  this->data[1] = _v[1];
73  this->data[2] = _v[2];
74  this->data[3] = _v[3];
75  }
76 
78  public: virtual ~Vector4() {}
79 
83  public: T Distance(const Vector4<T> &_pt) const
84  {
85  return static_cast<T>(sqrt(
86  (this->data[0]-_pt[0])*(this->data[0]-_pt[0]) +
87  (this->data[1]-_pt[1])*(this->data[1]-_pt[1]) +
88  (this->data[2]-_pt[2])*(this->data[2]-_pt[2]) +
89  (this->data[3]-_pt[3])*(this->data[3]-_pt[3])));
90  }
91 
98  public: T Distance(T _x, T _y, T _z, T _w) const
99  {
100  return this->Distance(Vector4(_x, _y, _z, _w));
101  }
102 
105  public: T Length() const
106  {
107  return static_cast<T>(sqrt(this->SquaredLength()));
108  }
109 
112  public: T SquaredLength() const
113  {
114  return
115  this->data[0] * this->data[0] +
116  this->data[1] * this->data[1] +
117  this->data[2] * this->data[2] +
118  this->data[3] * this->data[3];
119  }
120 
122  public: void Round()
123  {
124  this->data[0] = nearbyint(this->data[0]);
125  this->data[1] = nearbyint(this->data[1]);
126  this->data[2] = nearbyint(this->data[2]);
127  this->data[3] = nearbyint(this->data[3]);
128  }
129 
132  public: Vector4 Rounded() const
133  {
134  Vector4<T> result = *this;
135  result.Round();
136  return result;
137  }
138 
140  public: inline void Correct()
141  {
142  // std::isfinite works with floating point values,
143  // need to explicit cast to avoid ambiguity in vc++.
144  if (!std::isfinite(static_cast<double>(this->data[0])))
145  this->data[0] = 0;
146  if (!std::isfinite(static_cast<double>(this->data[1])))
147  this->data[1] = 0;
148  if (!std::isfinite(static_cast<double>(this->data[2])))
149  this->data[2] = 0;
150  if (!std::isfinite(static_cast<double>(this->data[3])))
151  this->data[3] = 0;
152  }
153 
155  public: void Normalize()
156  {
157  T d = this->Length();
158 
159  if (!equal<T>(d, static_cast<T>(0.0)))
160  {
161  this->data[0] /= d;
162  this->data[1] /= d;
163  this->data[2] /= d;
164  this->data[3] /= d;
165  }
166  }
167 
170  public: Vector4 Normalized() const
171  {
172  Vector4<T> result = *this;
173  result.Normalize();
174  return result;
175  }
176 
180  public: T Dot(const Vector4<T> &_v) const
181  {
182  return this->data[0] * _v[0] +
183  this->data[1] * _v[1] +
184  this->data[2] * _v[2] +
185  this->data[3] * _v[3];
186  }
187 
196  public: T AbsDot(const Vector4<T> &_v) const
197  {
198  return std::abs(this->data[0] * _v[0]) +
199  std::abs(this->data[1] * _v[1]) +
200  std::abs(this->data[2] * _v[2]) +
201  std::abs(this->data[3] * _v[3]);
202  }
203 
206  public: Vector4 Abs() const
207  {
208  return Vector4(std::abs(this->data[0]),
209  std::abs(this->data[1]),
210  std::abs(this->data[2]),
211  std::abs(this->data[3]));
212  }
213 
219  public: void Set(T _x = 0, T _y = 0, T _z = 0, T _w = 0)
220  {
221  this->data[0] = _x;
222  this->data[1] = _y;
223  this->data[2] = _z;
224  this->data[3] = _w;
225  }
226 
230  public: void Max(const Vector4<T> &_v)
231  {
232  this->data[0] = std::max(_v[0], this->data[0]);
233  this->data[1] = std::max(_v[1], this->data[1]);
234  this->data[2] = std::max(_v[2], this->data[2]);
235  this->data[3] = std::max(_v[3], this->data[3]);
236  }
237 
241  public: void Min(const Vector4<T> &_v)
242  {
243  this->data[0] = std::min(_v[0], this->data[0]);
244  this->data[1] = std::min(_v[1], this->data[1]);
245  this->data[2] = std::min(_v[2], this->data[2]);
246  this->data[3] = std::min(_v[3], this->data[3]);
247  }
248 
251  public: T Max() const
252  {
253  return *std::max_element(this->data, this->data+4);
254  }
255 
258  public: T Min() const
259  {
260  return *std::min_element(this->data, this->data+4);
261  }
262 
265  public: T Sum() const
266  {
267  return this->data[0] + this->data[1] + this->data[2] + this->data[3];
268  }
269 
273  public: Vector4<T> &operator=(const Vector4<T> &_v)
274  {
275  this->data[0] = _v[0];
276  this->data[1] = _v[1];
277  this->data[2] = _v[2];
278  this->data[3] = _v[3];
279 
280  return *this;
281  }
282 
285  public: Vector4<T> &operator=(T _value)
286  {
287  this->data[0] = _value;
288  this->data[1] = _value;
289  this->data[2] = _value;
290  this->data[3] = _value;
291 
292  return *this;
293  }
294 
298  public: Vector4<T> operator+(const Vector4<T> &_v) const
299  {
300  return Vector4<T>(this->data[0] + _v[0],
301  this->data[1] + _v[1],
302  this->data[2] + _v[2],
303  this->data[3] + _v[3]);
304  }
305 
309  public: const Vector4<T> &operator+=(const Vector4<T> &_v)
310  {
311  this->data[0] += _v[0];
312  this->data[1] += _v[1];
313  this->data[2] += _v[2];
314  this->data[3] += _v[3];
315 
316  return *this;
317  }
318 
322  public: inline Vector4<T> operator+(const T _s) const
323  {
324  return Vector4<T>(this->data[0] + _s,
325  this->data[1] + _s,
326  this->data[2] + _s,
327  this->data[3] + _s);
328  }
329 
334  public: friend inline Vector4<T> operator+(const T _s,
335  const Vector4<T> &_v)
336  {
337  return _v + _s;
338  }
339 
343  public: const Vector4<T> &operator+=(const T _s)
344  {
345  this->data[0] += _s;
346  this->data[1] += _s;
347  this->data[2] += _s;
348  this->data[3] += _s;
349 
350  return *this;
351  }
352 
355  public: inline Vector4 operator-() const
356  {
357  return Vector4(-this->data[0], -this->data[1],
358  -this->data[2], -this->data[3]);
359  }
360 
364  public: Vector4<T> operator-(const Vector4<T> &_v) const
365  {
366  return Vector4<T>(this->data[0] - _v[0],
367  this->data[1] - _v[1],
368  this->data[2] - _v[2],
369  this->data[3] - _v[3]);
370  }
371 
375  public: const Vector4<T> &operator-=(const Vector4<T> &_v)
376  {
377  this->data[0] -= _v[0];
378  this->data[1] -= _v[1];
379  this->data[2] -= _v[2];
380  this->data[3] -= _v[3];
381 
382  return *this;
383  }
384 
388  public: inline Vector4<T> operator-(const T _s) const
389  {
390  return Vector4<T>(this->data[0] - _s,
391  this->data[1] - _s,
392  this->data[2] - _s,
393  this->data[3] - _s);
394  }
395 
400  public: friend inline Vector4<T> operator-(const T _s,
401  const Vector4<T> &_v)
402  {
403  return {_s - _v.X(), _s - _v.Y(), _s - _v.Z(), _s - _v.W()};
404  }
405 
409  public: const Vector4<T> &operator-=(const T _s)
410  {
411  this->data[0] -= _s;
412  this->data[1] -= _s;
413  this->data[2] -= _s;
414  this->data[3] -= _s;
415 
416  return *this;
417  }
418 
424  public: const Vector4<T> operator/(const Vector4<T> &_v) const
425  {
426  return Vector4<T>(this->data[0] / _v[0],
427  this->data[1] / _v[1],
428  this->data[2] / _v[2],
429  this->data[3] / _v[3]);
430  }
431 
437  public: const Vector4<T> &operator/=(const Vector4<T> &_v)
438  {
439  this->data[0] /= _v[0];
440  this->data[1] /= _v[1];
441  this->data[2] /= _v[2];
442  this->data[3] /= _v[3];
443 
444  return *this;
445  }
446 
452  public: const Vector4<T> operator/(T _v) const
453  {
454  return Vector4<T>(this->data[0] / _v, this->data[1] / _v,
455  this->data[2] / _v, this->data[3] / _v);
456  }
457 
461  public: const Vector4<T> &operator/=(T _v)
462  {
463  this->data[0] /= _v;
464  this->data[1] /= _v;
465  this->data[2] /= _v;
466  this->data[3] /= _v;
467 
468  return *this;
469  }
470 
476  public: const Vector4<T> operator*(const Vector4<T> &_pt) const
477  {
478  return Vector4<T>(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 
487  public: const Vector4<T> operator*(const Matrix4<T> &_m) const
488  {
489  return Vector4<T>(
490  this->data[0]*_m(0, 0) + this->data[1]*_m(1, 0) +
491  this->data[2]*_m(2, 0) + this->data[3]*_m(3, 0),
492  this->data[0]*_m(0, 1) + this->data[1]*_m(1, 1) +
493  this->data[2]*_m(2, 1) + this->data[3]*_m(3, 1),
494  this->data[0]*_m(0, 2) + this->data[1]*_m(1, 2) +
495  this->data[2]*_m(2, 2) + this->data[3]*_m(3, 2),
496  this->data[0]*_m(0, 3) + this->data[1]*_m(1, 3) +
497  this->data[2]*_m(2, 3) + this->data[3]*_m(3, 3));
498  }
499 
505  public: const Vector4<T> &operator*=(const Vector4<T> &_pt)
506  {
507  this->data[0] *= _pt[0];
508  this->data[1] *= _pt[1];
509  this->data[2] *= _pt[2];
510  this->data[3] *= _pt[3];
511 
512  return *this;
513  }
514 
518  public: const Vector4<T> operator*(T _v) const
519  {
520  return Vector4<T>(this->data[0] * _v, this->data[1] * _v,
521  this->data[2] * _v, this->data[3] * _v);
522  }
523 
528  public: friend inline const Vector4 operator*(const T _s,
529  const Vector4 &_v)
530  {
531  return Vector4(_v * _s);
532  }
533 
537  public: const Vector4<T> &operator*=(T _v)
538  {
539  this->data[0] *= _v;
540  this->data[1] *= _v;
541  this->data[2] *= _v;
542  this->data[3] *= _v;
543 
544  return *this;
545  }
546 
552  public: bool Equal(const Vector4 &_v, const T &_tol) const
553  {
554  return equal<T>(this->data[0], _v[0], _tol)
555  && equal<T>(this->data[1], _v[1], _tol)
556  && equal<T>(this->data[2], _v[2], _tol)
557  && equal<T>(this->data[3], _v[3], _tol);
558  }
559 
564  public: bool operator==(const Vector4<T> &_v) const
565  {
566  return this->Equal(_v, static_cast<T>(1e-6));
567  }
568 
573  public: bool operator!=(const Vector4<T> &_pt) const
574  {
575  return !(*this == _pt);
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  std::isfinite(static_cast<double>(this->data[3]));
588  }
589 
594  public: T &operator[](const std::size_t _index)
595  {
596  return this->data[clamp(_index, IGN_ZERO_SIZE_T, IGN_THREE_SIZE_T)];
597  }
598 
603  public: T operator[](const std::size_t _index) const
604  {
605  return this->data[clamp(_index, IGN_ZERO_SIZE_T, IGN_THREE_SIZE_T)];
606  }
607 
610  public: T &X()
611  {
612  return this->data[0];
613  }
614 
617  public: T &Y()
618  {
619  return this->data[1];
620  }
621 
624  public: T &Z()
625  {
626  return this->data[2];
627  }
628 
631  public: T &W()
632  {
633  return this->data[3];
634  }
635 
638  public: T X() const
639  {
640  return this->data[0];
641  }
642 
645  public: T Y() const
646  {
647  return this->data[1];
648  }
649 
652  public: T Z() const
653  {
654  return this->data[2];
655  }
656 
659  public: T W() const
660  {
661  return this->data[3];
662  }
663 
666  public: inline void X(const T &_v)
667  {
668  this->data[0] = _v;
669  }
670 
673  public: inline void Y(const T &_v)
674  {
675  this->data[1] = _v;
676  }
677 
680  public: inline void Z(const T &_v)
681  {
682  this->data[2] = _v;
683  }
684 
687  public: inline void W(const T &_v)
688  {
689  this->data[3] = _v;
690  }
691 
696  public: bool operator<(const Vector4<T> &_pt) const
697  {
698  return this->data[0] < _pt[0] || this->data[1] < _pt[1] ||
699  this->data[2] < _pt[2] || this->data[3] < _pt[3];
700  }
701 
706  public: friend std::ostream &operator<<(
707  std::ostream &_out, const ignition::math::Vector4<T> &_pt)
708  {
709  _out << _pt[0] << " " << _pt[1] << " " << _pt[2] << " " << _pt[3];
710  return _out;
711  }
712 
717  public: friend std::istream &operator>>(
719  {
720  T x, y, z, w;
721 
722  // Skip white spaces
723  _in.setf(std::ios_base::skipws);
724  _in >> x >> y >> z >> w;
725  if (!_in.fail())
726  {
727  _pt.Set(x, y, z, w);
728  }
729  return _in;
730  }
731 
733  private: T data[4];
734  };
735 
736  template<typename T>
737  const Vector4<T> Vector4<T>::Zero(0, 0, 0, 0);
738 
739  template<typename T>
740  const Vector4<T> Vector4<T>::One(1, 1, 1, 1);
741 
742  template<typename T> const Vector4<T> Vector4<T>::NaN(
747 
751  }
752  }
753 }
754 #endif
friend Vector4< T > operator+(const T _s, const Vector4< T > &_v)
Addition operators.
Definition: Vector4.hh:334
const Vector4< T > operator*(T _v) const
Multiplication operators.
Definition: Vector4.hh:518
T setf(T... args)
static const Vector4< T > Zero
math::Vector4(0, 0, 0, 0)
Definition: Vector4.hh:40
const Vector4< T > operator/(T _v) const
Division assignment operator.
Definition: Vector4.hh:452
T Dot(const Vector4< T > &_v) const
Return the dot product of this vector and another vector.
Definition: Vector4.hh:180
T & Z()
Return a mutable z value.
Definition: Vector4.hh:624
Vector4< T > operator-(const T _s) const
Subtraction operators.
Definition: Vector4.hh:388
T & W()
Return a mutable w value.
Definition: Vector4.hh:631
void Round()
Round to near whole number.
Definition: Vector4.hh:122
bool operator!=(const Vector4< T > &_pt) const
Not equal to operator.
Definition: Vector4.hh:573
Vector4< T > operator+(const T _s) const
Addition operators.
Definition: Vector4.hh:322
T fail(T... args)
bool operator==(const Vector4< T > &_v) const
Equal to operator.
Definition: Vector4.hh:564
friend std::istream & operator>>(std::istream &_in, Vector4< T > &_pt)
Stream extraction operator.
Definition: Vector4.hh:717
T max_element(T... args)
const Vector4< T > & operator/=(const Vector4< T > &_v)
Division assignment operator.
Definition: Vector4.hh:437
friend const Vector4 operator*(const T _s, const Vector4 &_v)
Scalar left multiplication operators.
Definition: Vector4.hh:528
void Y(const T &_v)
Set the y value.
Definition: Vector4.hh:673
friend std::ostream & operator<<(std::ostream &_out, const Vector4< T > &_pt)
Stream insertion operator.
Definition: Vector4.hh:706
A 4x4 matrix class.
Definition: Matrix4.hh:38
Vector4< T > & operator=(T _value)
Assignment operator.
Definition: Vector4.hh:285
T Distance(T _x, T _y, T _z, T _w) const
Calc distance to the given point.
Definition: Vector4.hh:98
T Z() const
Get the z value.
Definition: Vector4.hh:652
STL class.
void X(const T &_v)
Set the x value.
Definition: Vector4.hh:666
void W(const T &_v)
Set the w value.
Definition: Vector4.hh:687
const Vector4< T > & operator-=(const T _s)
Subtraction assignment operator.
Definition: Vector4.hh:409
T Y() const
Get the y value.
Definition: Vector4.hh:645
T min(T... args)
void Min(const Vector4< T > &_v)
Set this vector&#39;s components to the minimum of itself and the passed in vector.
Definition: Vector4.hh:241
Vector4< float > Vector4f
Definition: Vector4.hh:750
static const Vector4< T > One
math::Vector4(1, 1, 1, 1)
Definition: Vector4.hh:43
T operator[](const std::size_t _index) const
Const-qualified array subscript operator.
Definition: Vector4.hh:603
Vector4< T > & operator=(const Vector4< T > &_v)
Assignment operator.
Definition: Vector4.hh:273
Vector4()
Constructor.
Definition: Vector4.hh:49
const Vector4< T > & operator/=(T _v)
Division operator.
Definition: Vector4.hh:461
T W() const
Get the w value.
Definition: Vector4.hh:659
static const size_t IGN_ZERO_SIZE_T
size_t type with a value of 0
Definition: Helpers.hh:227
T isfinite(T... args)
const Vector4< T > operator*(const Matrix4< T > &_m) const
Matrix multiplication operator.
Definition: Vector4.hh:487
Vector4 Rounded() const
Get a rounded version of this vector.
Definition: Vector4.hh:132
T max(T... args)
virtual ~Vector4()
Destructor.
Definition: Vector4.hh:78
T Length() const
Returns the length (magnitude) of the vector.
Definition: Vector4.hh:105
Vector4(const Vector4< T > &_v)
Copy constructor.
Definition: Vector4.hh:69
void Normalize()
Normalize the vector length.
Definition: Vector4.hh:155
Vector4 Abs() const
Get the absolute value of the vector.
Definition: Vector4.hh:206
void Correct()
Corrects any nan values.
Definition: Vector4.hh:140
T & X()
Return a mutable x value.
Definition: Vector4.hh:610
const Vector4< T > & operator*=(const Vector4< T > &_pt)
Multiplication assignment operator.
Definition: Vector4.hh:505
const Vector4< T > operator/(const Vector4< T > &_v) const
Division assignment operator.
Definition: Vector4.hh:424
T & Y()
Return a mutable y value.
Definition: Vector4.hh:617
const Vector4< T > & operator-=(const Vector4< T > &_v)
Subtraction assigment operators.
Definition: Vector4.hh:375
Vector4< int > Vector4i
Definition: Vector4.hh:748
const Vector4< T > & operator*=(T _v)
Multiplication assignment operator.
Definition: Vector4.hh:537
Vector4 Normalized() const
Return a normalized vector.
Definition: Vector4.hh:170
T & operator[](const std::size_t _index)
Array subscript operator.
Definition: Vector4.hh:594
T SquaredLength() const
Return the square of the length (magnitude) of the vector.
Definition: Vector4.hh:112
Vector4< T > operator-(const Vector4< T > &_v) const
Subtraction operator.
Definition: Vector4.hh:364
bool Equal(const Vector4 &_v, const T &_tol) const
Equality test with tolerance.
Definition: Vector4.hh:552
T Sum() const
Return the sum of the values.
Definition: Vector4.hh:265
const Vector4< T > & operator+=(const T _s)
Addition assignment operator.
Definition: Vector4.hh:343
T Max() const
Get the maximum value in the vector.
Definition: Vector4.hh:251
static const size_t IGN_THREE_SIZE_T
size_t type with a value of 3
Definition: Helpers.hh:236
bool IsFinite() const
See if a point is finite (e.g., not nan)
Definition: Vector4.hh:580
static const Vector4 NaN
math::Vector4(NaN, NaN, NaN, NaN)
Definition: Vector4.hh:46
Definition: Angle.hh:42
T min_element(T... args)
T Min() const
Get the minimum value in the vector.
Definition: Vector4.hh:258
friend Vector4< T > operator-(const T _s, const Vector4< T > &_v)
Subtraction operators.
Definition: Vector4.hh:400
Vector4< T > operator+(const Vector4< T > &_v) const
Addition operator.
Definition: Vector4.hh:298
STL class.
const Vector4< T > & operator+=(const Vector4< T > &_v)
Addition operator.
Definition: Vector4.hh:309
T Generic x, y, z, w vector.
Definition: Vector4.hh:37
Vector4< double > Vector4d
Definition: Vector4.hh:749
void Max(const Vector4< T > &_v)
Set this vector&#39;s components to the maximum of itself and the passed in vector.
Definition: Vector4.hh:230
const Vector4< T > operator*(const Vector4< T > &_pt) const
Multiplication operator.
Definition: Vector4.hh:476
void Set(T _x=0, T _y=0, T _z=0, T _w=0)
Set the contents of the vector.
Definition: Vector4.hh:219
T X() const
Get the x value.
Definition: Vector4.hh:638
T clamp(T _v, T _min, T _max)
Simple clamping function.
Definition: Helpers.hh:406
T Distance(const Vector4< T > &_pt) const
Calc distance to the given point.
Definition: Vector4.hh:83
Vector4(const T &_x, const T &_y, const T &_z, const T &_w)
Constructor with component values.
Definition: Vector4.hh:59
T AbsDot(const Vector4< T > &_v) const
Return the absolute dot product of this vector and another vector. This is similar to the Dot functio...
Definition: Vector4.hh:196
Vector4 operator-() const
Negation operator.
Definition: Vector4.hh:355
void Z(const T &_v)
Set the z value.
Definition: Vector4.hh:680