Vector4.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2016 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 <ignition/math/Matrix4.hh>
21 #include <ignition/math/Helpers.hh>
22 
23 namespace ignition
24 {
25  namespace math
26  {
29  template<typename T>
30  class Vector4
31  {
33  public: static const Vector4<T> Zero;
34 
36  public: static const Vector4<T> One;
37 
39  public: Vector4()
40  {
41  this->data[0] = this->data[1] = this->data[2] = this->data[3] = 0;
42  }
43 
49  public: Vector4(const T &_x, const T &_y, const T &_z, const T &_w)
50  {
51  this->data[0] = _x;
52  this->data[1] = _y;
53  this->data[2] = _z;
54  this->data[3] = _w;
55  }
56 
59  public: Vector4(const Vector4<T> &_v)
60  {
61  this->data[0] = _v[0];
62  this->data[1] = _v[1];
63  this->data[2] = _v[2];
64  this->data[3] = _v[3];
65  }
66 
68  public: virtual ~Vector4() {}
69 
73  public: T Distance(const Vector4<T> &_pt) const
74  {
75  return sqrt((this->data[0]-_pt[0])*(this->data[0]-_pt[0]) +
76  (this->data[1]-_pt[1])*(this->data[1]-_pt[1]) +
77  (this->data[2]-_pt[2])*(this->data[2]-_pt[2]) +
78  (this->data[3]-_pt[3])*(this->data[3]-_pt[3]));
79  }
80 
83  public: T Length() const
84  {
85  return sqrt(this->SquaredLength());
86  }
87 
90  public: T SquaredLength() const
91  {
92  return std::pow(this->data[0], 2)
93  + std::pow(this->data[1], 2)
94  + std::pow(this->data[2], 2)
95  + std::pow(this->data[3], 2);
96  }
97 
99  public: void Normalize()
100  {
101  T d = this->Length();
102 
103  if (!equal<T>(d, static_cast<T>(0.0)))
104  {
105  this->data[0] /= d;
106  this->data[1] /= d;
107  this->data[2] /= d;
108  this->data[3] /= d;
109  }
110  }
111 
117  public: void Set(T _x = 0, T _y = 0, T _z = 0, T _w = 0)
118  {
119  this->data[0] = _x;
120  this->data[1] = _y;
121  this->data[2] = _z;
122  this->data[3] = _w;
123  }
124 
128  public: Vector4<T> &operator=(const Vector4<T> &_v)
129  {
130  this->data[0] = _v[0];
131  this->data[1] = _v[1];
132  this->data[2] = _v[2];
133  this->data[3] = _v[3];
134 
135  return *this;
136  }
137 
140  public: Vector4<T> &operator=(T _value)
141  {
142  this->data[0] = _value;
143  this->data[1] = _value;
144  this->data[2] = _value;
145  this->data[3] = _value;
146 
147  return *this;
148  }
149 
153  public: Vector4<T> operator+(const Vector4<T> &_v) const
154  {
155  return Vector4<T>(this->data[0] + _v[0],
156  this->data[1] + _v[1],
157  this->data[2] + _v[2],
158  this->data[3] + _v[3]);
159  }
160 
164  public: const Vector4<T> &operator+=(const Vector4<T> &_v)
165  {
166  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  return *this;
172  }
173 
177  public: inline Vector4<T> operator+(const T _s) const
178  {
179  return Vector4<T>(this->data[0] + _s,
180  this->data[1] + _s,
181  this->data[2] + _s,
182  this->data[3] + _s);
183  }
184 
189  public: friend inline Vector4<T> operator+(const T _s,
190  const Vector4<T> &_v)
191  {
192  return _v + _s;
193  }
194 
198  public: const Vector4<T> &operator+=(const T _s)
199  {
200  this->data[0] += _s;
201  this->data[1] += _s;
202  this->data[2] += _s;
203  this->data[3] += _s;
204 
205  return *this;
206  }
207 
210  public: inline Vector4 operator-() const
211  {
212  return Vector4(-this->data[0], -this->data[1],
213  -this->data[2], -this->data[3]);
214  }
215 
219  public: Vector4<T> operator-(const Vector4<T> &_v) const
220  {
221  return Vector4<T>(this->data[0] - _v[0],
222  this->data[1] - _v[1],
223  this->data[2] - _v[2],
224  this->data[3] - _v[3]);
225  }
226 
230  public: const Vector4<T> &operator-=(const Vector4<T> &_v)
231  {
232  this->data[0] -= _v[0];
233  this->data[1] -= _v[1];
234  this->data[2] -= _v[2];
235  this->data[3] -= _v[3];
236 
237  return *this;
238  }
239 
243  public: inline Vector4<T> operator-(const T _s) const
244  {
245  return Vector4<T>(this->data[0] - _s,
246  this->data[1] - _s,
247  this->data[2] - _s,
248  this->data[3] - _s);
249  }
250 
255  public: friend inline Vector4<T> operator-(const T _s,
256  const Vector4<T> &_v)
257  {
258  return Vector4<T>(_s - _v.X(), _s - _v.Y(), _s - _v.Z(), _s - _v.W());
259  }
260 
264  public: const Vector4<T> &operator-=(const T _s)
265  {
266  this->data[0] -= _s;
267  this->data[1] -= _s;
268  this->data[2] -= _s;
269  this->data[3] -= _s;
270 
271  return *this;
272  }
273 
279  public: const Vector4<T> operator/(const Vector4<T> &_v) const
280  {
281  return Vector4<T>(this->data[0] / _v[0],
282  this->data[1] / _v[1],
283  this->data[2] / _v[2],
284  this->data[3] / _v[3]);
285  }
286 
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 
307  public: const Vector4<T> operator/(T _v) const
308  {
309  return Vector4<T>(this->data[0] / _v, this->data[1] / _v,
310  this->data[2] / _v, this->data[3] / _v);
311  }
312 
316  public: const Vector4<T> &operator/=(T _v)
317  {
318  this->data[0] /= _v;
319  this->data[1] /= _v;
320  this->data[2] /= _v;
321  this->data[3] /= _v;
322 
323  return *this;
324  }
325 
331  public: const Vector4<T> operator*(const Vector4<T> &_pt) const
332  {
333  return Vector4<T>(this->data[0] * _pt[0],
334  this->data[1] * _pt[1],
335  this->data[2] * _pt[2],
336  this->data[3] * _pt[3]);
337  }
338 
342  public: const Vector4<T> operator*(const Matrix4<T> &_m) const
343  {
344  return Vector4<T>(
345  this->data[0]*_m(0, 0) + this->data[1]*_m(1, 0) +
346  this->data[2]*_m(2, 0) + this->data[3]*_m(3, 0),
347  this->data[0]*_m(0, 1) + this->data[1]*_m(1, 1) +
348  this->data[2]*_m(2, 1) + this->data[3]*_m(3, 1),
349  this->data[0]*_m(0, 2) + this->data[1]*_m(1, 2) +
350  this->data[2]*_m(2, 2) + this->data[3]*_m(3, 2),
351  this->data[0]*_m(0, 3) + this->data[1]*_m(1, 3) +
352  this->data[2]*_m(2, 3) + this->data[3]*_m(3, 3));
353  }
354 
360  public: const Vector4<T> &operator*=(const Vector4<T> &_pt)
361  {
362  this->data[0] *= _pt[0];
363  this->data[1] *= _pt[1];
364  this->data[2] *= _pt[2];
365  this->data[3] *= _pt[3];
366 
367  return *this;
368  }
369 
373  public: const Vector4<T> operator*(T _v) const
374  {
375  return Vector4<T>(this->data[0] * _v, this->data[1] * _v,
376  this->data[2] * _v, this->data[3] * _v);
377  }
378 
383  public: friend inline const Vector4 operator*(const T _s,
384  const Vector4 &_v)
385  {
386  return Vector4(_v * _s);
387  }
388 
392  public: const Vector4<T> &operator*=(T _v)
393  {
394  this->data[0] *= _v;
395  this->data[1] *= _v;
396  this->data[2] *= _v;
397  this->data[3] *= _v;
398 
399  return *this;
400  }
401 
407  public: bool Equal(const Vector4 &_v, const T &_tol) const
408  {
409  return equal<T>(this->data[0], _v[0], _tol)
410  && equal<T>(this->data[1], _v[1], _tol)
411  && equal<T>(this->data[2], _v[2], _tol)
412  && equal<T>(this->data[3], _v[3], _tol);
413  }
414 
419  public: bool operator==(const Vector4<T> &_v) const
420  {
421  return this->Equal(_v, static_cast<T>(1e-6));
422  }
423 
428  public: bool operator!=(const Vector4<T> &_pt) const
429  {
430  return !(*this == _pt);
431  }
432 
435  public: bool IsFinite() const
436  {
437  // std::isfinite works with floating point values,
438  // need to explicit cast to avoid ambiguity in vc++.
439  return std::isfinite(static_cast<double>(this->data[0])) &&
440  std::isfinite(static_cast<double>(this->data[1])) &&
441  std::isfinite(static_cast<double>(this->data[2])) &&
442  std::isfinite(static_cast<double>(this->data[3]));
443  }
448  public: inline T operator[](const size_t _index) const
449  {
450  return this->data[clamp(_index, IGN_ZERO_SIZE_T, IGN_THREE_SIZE_T)];
451  }
452 
455  public: inline T X() const
456  {
457  return this->data[0];
458  }
459 
462  public: inline T Y() const
463  {
464  return this->data[1];
465  }
466 
469  public: inline T Z() const
470  {
471  return this->data[2];
472  }
473 
476  public: inline T W() const
477  {
478  return this->data[3];
479  }
480 
483  public: inline void X(const T &_v)
484  {
485  this->data[0] = _v;
486  }
487 
490  public: inline void Y(const T &_v)
491  {
492  this->data[1] = _v;
493  }
494 
497  public: inline void Z(const T &_v)
498  {
499  this->data[2] = _v;
500  }
501 
504  public: inline void W(const T &_v)
505  {
506  this->data[3] = _v;
507  }
508 
513  public: friend std::ostream &operator<<(
514  std::ostream &_out, const ignition::math::Vector4<T> &_pt)
515  {
516  _out << _pt[0] << " " << _pt[1] << " " << _pt[2] << " " << _pt[3];
517  return _out;
518  }
519 
524  public: friend std::istream &operator>>(
525  std::istream &_in, ignition::math::Vector4<T> &_pt)
526  {
527  T x, y, z, w;
528 
529  // Skip white spaces
530  _in.setf(std::ios_base::skipws);
531  _in >> x >> y >> z >> w;
532  _pt.Set(x, y, z, w);
533  return _in;
534  }
535 
537  private: T data[4];
538  };
539 
540  template<typename T>
541  const Vector4<T> Vector4<T>::Zero(0, 0, 0, 0);
542 
543  template<typename T>
544  const Vector4<T> Vector4<T>::One(1, 1, 1, 1);
545 
549  }
550 }
551 #endif
const Vector4< T > operator*(const Matrix4< T > &_m) const
Matrix multiplication operator.
Definition: Vector4.hh:342
friend Vector4< T > operator+(const T _s, const Vector4< T > &_v)
Addition operators.
Definition: Vector4.hh:189
Vector4 operator-() const
Negation operator.
Definition: Vector4.hh:210
T operator[](const size_t _index) const
Array subscript operator.
Definition: Vector4.hh:448
bool Equal(const Vector4 &_v, const T &_tol) const
Equality test with tolerance.
Definition: Vector4.hh:407
Vector4< int > Vector4i
Definition: Vector4.hh:546
Vector4< T > operator-(const Vector4< T > &_v) const
Subtraction operator.
Definition: Vector4.hh:219
const Vector4< T > & operator-=(const Vector4< T > &_v)
Subtraction assigment operators.
Definition: Vector4.hh:230
T X() const
Get the x value.
Definition: Vector4.hh:455
Vector4< double > Vector4d
Definition: Vector4.hh:547
T Y() const
Get the y value.
Definition: Vector4.hh:462
static const size_t IGN_THREE_SIZE_T
size_t type with a value of 3
Definition: Helpers.hh:222
static const size_t IGN_ZERO_SIZE_T
size_t type with a value of 0
Definition: Helpers.hh:213
friend std::ostream & operator<<(std::ostream &_out, const ignition::math::Vector4< T > &_pt)
Stream insertion operator.
Definition: Vector4.hh:513
const Vector4< T > operator*(const Vector4< T > &_pt) const
Multiplication operator.
Definition: Vector4.hh:331
Vector4< T > & operator=(const Vector4< T > &_v)
Assignment operator.
Definition: Vector4.hh:128
Vector4()
Constructor.
Definition: Vector4.hh:39
friend const Vector4 operator*(const T _s, const Vector4 &_v)
Scalar left multiplication operators.
Definition: Vector4.hh:383
Vector4< T > & operator=(T _value)
Assignment operator.
Definition: Vector4.hh:140
void Z(const T &_v)
Set the z value.
Definition: Vector4.hh:497
const Vector4< T > & operator-=(const T _s)
Subtraction assignment operator.
Definition: Vector4.hh:264
const Vector4< T > operator/(const Vector4< T > &_v) const
Division assignment operator.
Definition: Vector4.hh:279
T Z() const
Get the z value.
Definition: Vector4.hh:469
Vector4(const Vector4< T > &_v)
Copy constructor.
Definition: Vector4.hh:59
Vector4< T > operator+(const Vector4< T > &_v) const
Addition operator.
Definition: Vector4.hh:153
A 4x4 matrix class.
Definition: Matrix4.hh:33
Vector4< T > operator+(const T _s) const
Addition operators.
Definition: Vector4.hh:177
T SquaredLength() const
Return the square of the length (magnitude) of the vector.
Definition: Vector4.hh:90
T Distance(const Vector4< T > &_pt) const
Calc distance to the given point.
Definition: Vector4.hh:73
void Normalize()
Normalize the vector length.
Definition: Vector4.hh:99
virtual ~Vector4()
Destructor.
Definition: Vector4.hh:68
const Vector4< T > operator/(T _v) const
Division assignment operator.
Definition: Vector4.hh:307
const Vector4< T > & operator*=(T _v)
Multiplication assignment operator.
Definition: Vector4.hh:392
bool operator!=(const Vector4< T > &_pt) const
Not equal to operator.
Definition: Vector4.hh:428
bool operator==(const Vector4< T > &_v) const
Equal to operator.
Definition: Vector4.hh:419
void Set(T _x=0, T _y=0, T _z=0, T _w=0)
Set the contents of the vector.
Definition: Vector4.hh:117
bool IsFinite() const
See if a point is finite (e.g., not nan)
Definition: Vector4.hh:435
const Vector4< T > operator*(T _v) const
Multiplication operators.
Definition: Vector4.hh:373
T Length() const
Returns the length (magnitude) of the vector.
Definition: Vector4.hh:83
Vector4< float > Vector4f
Definition: Vector4.hh:548
void W(const T &_v)
Set the w value.
Definition: Vector4.hh:504
const Vector4< T > & operator+=(const T _s)
Addition assignment operator.
Definition: Vector4.hh:198
const Vector4< T > & operator*=(const Vector4< T > &_pt)
Multiplication assignment operator.
Definition: Vector4.hh:360
const Vector4< T > & operator/=(const Vector4< T > &_v)
Division assignment operator.
Definition: Vector4.hh:292
void Y(const T &_v)
Set the y value.
Definition: Vector4.hh:490
const Vector4< T > & operator/=(T _v)
Division operator.
Definition: Vector4.hh:316
T W() const
Get the w value.
Definition: Vector4.hh:476
void X(const T &_v)
Set the x value.
Definition: Vector4.hh:483
static const Vector4< T > One
math::Vector4(1, 1, 1, 1)
Definition: Vector4.hh:36
Vector4(const T &_x, const T &_y, const T &_z, const T &_w)
Constructor with component values.
Definition: Vector4.hh:49
Vector4< T > operator-(const T _s) const
Subtraction operators.
Definition: Vector4.hh:243
Definition: Angle.hh:38
friend std::istream & operator>>(std::istream &_in, ignition::math::Vector4< T > &_pt)
Stream extraction operator.
Definition: Vector4.hh:524
friend Vector4< T > operator-(const T _s, const Vector4< T > &_v)
Subtraction operators.
Definition: Vector4.hh:255
T Generic x, y, z, w vector.
Definition: Vector4.hh:30
static const Vector4< T > Zero
math::Vector4(0, 0, 0, 0)
Definition: Vector4.hh:33
T clamp(T _v, T _min, T _max)
Simple clamping function.
Definition: Helpers.hh:392
const Vector4< T > & operator+=(const Vector4< T > &_v)
Addition operator.
Definition: Vector4.hh:164