Vector4.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2014 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_VECTOR4_HH_
18 #define _IGNITION_VECTOR4_HH_
19 
20 #include <ignition/math/Matrix4.hh>
21 
22 namespace ignition
23 {
24  namespace math
25  {
28  template<typename T>
29  class Vector4
30  {
32  public: static const Vector4<T> Zero;
33 
35  public: static const Vector4<T> One;
36 
38  public: Vector4()
39  {
40  this->data[0] = this->data[1] = this->data[2] = this->data[3] = 0;
41  }
42 
48  public: Vector4(const T &_x, const T &_y, const T &_z, const T &_w)
49  {
50  this->data[0] = _x;
51  this->data[1] = _y;
52  this->data[2] = _z;
53  this->data[3] = _w;
54  }
55 
58  public: Vector4(const Vector4<T> &_v)
59  {
60  this->data[0] = _v[0];
61  this->data[1] = _v[1];
62  this->data[2] = _v[2];
63  this->data[3] = _v[3];
64  }
65 
67  public: virtual ~Vector4() {}
68 
72  public: T Distance(const Vector4<T> &_pt) const
73  {
74  return sqrt((this->data[0]-_pt[0])*(this->data[0]-_pt[0]) +
75  (this->data[1]-_pt[1])*(this->data[1]-_pt[1]) +
76  (this->data[2]-_pt[2])*(this->data[2]-_pt[2]) +
77  (this->data[3]-_pt[3])*(this->data[3]-_pt[3]));
78  }
79 
82  public: T Length() const
83  {
84  return sqrt(this->SquaredLength());
85  }
86 
89  public: T SquaredLength() const
90  {
91  return std::pow(this->data[0], 2)
92  + std::pow(this->data[1], 2)
93  + std::pow(this->data[2], 2)
94  + std::pow(this->data[3], 2);
95  }
96 
98  public: void Normalize()
99  {
100  T d = this->Length();
101 
102  if (!equal<T>(d, static_cast<T>(0.0)))
103  {
104  this->data[0] /= d;
105  this->data[1] /= d;
106  this->data[2] /= d;
107  this->data[3] /= d;
108  }
109  }
110 
116  public: void Set(T _x = 0, T _y = 0, T _z = 0, T _w = 0)
117  {
118  this->data[0] = _x;
119  this->data[1] = _y;
120  this->data[2] = _z;
121  this->data[3] = _w;
122  }
123 
127  public: Vector4<T> &operator=(const Vector4<T> &_v)
128  {
129  this->data[0] = _v[0];
130  this->data[1] = _v[1];
131  this->data[2] = _v[2];
132  this->data[3] = _v[3];
133 
134  return *this;
135  }
136 
139  public: Vector4<T> &operator=(T _value)
140  {
141  this->data[0] = _value;
142  this->data[1] = _value;
143  this->data[2] = _value;
144  this->data[3] = _value;
145 
146  return *this;
147  }
148 
152  public: Vector4<T> operator+(const Vector4<T> &_v) const
153  {
154  return Vector4<T>(this->data[0] + _v[0],
155  this->data[1] + _v[1],
156  this->data[2] + _v[2],
157  this->data[3] + _v[3]);
158  }
159 
163  public: const Vector4<T> &operator+=(const Vector4<T> &_v)
164  {
165  this->data[0] += _v[0];
166  this->data[1] += _v[1];
167  this->data[2] += _v[2];
168  this->data[3] += _v[3];
169 
170  return *this;
171  }
172 
176  public: inline Vector4<T> operator+(const T _s) const
177  {
178  return Vector4<T>(this->data[0] + _s,
179  this->data[1] + _s,
180  this->data[2] + _s,
181  this->data[3] + _s);
182  }
183 
188  public: friend inline Vector4<T> operator+(const T _s,
189  const Vector4<T> &_v)
190  {
191  return _v + _s;
192  }
193 
197  public: const Vector4<T> &operator+=(const T _s)
198  {
199  this->data[0] += _s;
200  this->data[1] += _s;
201  this->data[2] += _s;
202  this->data[3] += _s;
203 
204  return *this;
205  }
206 
209  public: inline Vector4 operator-() const
210  {
211  return Vector4(-this->data[0], -this->data[1],
212  -this->data[2], -this->data[3]);
213  }
214 
218  public: Vector4<T> operator-(const Vector4<T> &_v) const
219  {
220  return Vector4<T>(this->data[0] - _v[0],
221  this->data[1] - _v[1],
222  this->data[2] - _v[2],
223  this->data[3] - _v[3]);
224  }
225 
229  public: const Vector4<T> &operator-=(const Vector4<T> &_v)
230  {
231  this->data[0] -= _v[0];
232  this->data[1] -= _v[1];
233  this->data[2] -= _v[2];
234  this->data[3] -= _v[3];
235 
236  return *this;
237  }
238 
242  public: inline Vector4<T> operator-(const T _s) const
243  {
244  return Vector4<T>(this->data[0] - _s,
245  this->data[1] - _s,
246  this->data[2] - _s,
247  this->data[3] - _s);
248  }
249 
254  public: friend inline Vector4<T> operator-(const T _s,
255  const Vector4<T> &_v)
256  {
257  return Vector4<T>(_s - _v.X(), _s - _v.Y(), _s - _v.Z(), _s - _v.W());
258  }
259 
263  public: const Vector4<T> &operator-=(const T _s)
264  {
265  this->data[0] -= _s;
266  this->data[1] -= _s;
267  this->data[2] -= _s;
268  this->data[3] -= _s;
269 
270  return *this;
271  }
272 
278  public: const Vector4<T> operator/(const Vector4<T> &_v) const
279  {
280  return Vector4<T>(this->data[0] / _v[0],
281  this->data[1] / _v[1],
282  this->data[2] / _v[2],
283  this->data[3] / _v[3]);
284  }
285 
291  public: const Vector4<T> &operator/=(const Vector4<T> &_v)
292  {
293  this->data[0] /= _v[0];
294  this->data[1] /= _v[1];
295  this->data[2] /= _v[2];
296  this->data[3] /= _v[3];
297 
298  return *this;
299  }
300 
306  public: const Vector4<T> operator/(T _v) const
307  {
308  return Vector4<T>(this->data[0] / _v, this->data[1] / _v,
309  this->data[2] / _v, this->data[3] / _v);
310  }
311 
315  public: const Vector4<T> &operator/=(T _v)
316  {
317  this->data[0] /= _v;
318  this->data[1] /= _v;
319  this->data[2] /= _v;
320  this->data[3] /= _v;
321 
322  return *this;
323  }
324 
330  public: const Vector4<T> operator*(const Vector4<T> &_pt) const
331  {
332  return Vector4<T>(this->data[0] * _pt[0],
333  this->data[1] * _pt[1],
334  this->data[2] * _pt[2],
335  this->data[3] * _pt[3]);
336  }
337 
341  public: const Vector4<T> operator*(const Matrix4<T> &_m) const
342  {
343  return Vector4<T>(
344  this->data[0]*_m(0, 0) + this->data[1]*_m(1, 0) +
345  this->data[2]*_m(2, 0) + this->data[3]*_m(3, 0),
346  this->data[0]*_m(0, 1) + this->data[1]*_m(1, 1) +
347  this->data[2]*_m(2, 1) + this->data[3]*_m(3, 1),
348  this->data[0]*_m(0, 2) + this->data[1]*_m(1, 2) +
349  this->data[2]*_m(2, 2) + this->data[3]*_m(3, 2),
350  this->data[0]*_m(0, 3) + this->data[1]*_m(1, 3) +
351  this->data[2]*_m(2, 3) + this->data[3]*_m(3, 3));
352  }
353 
359  public: const Vector4<T> &operator*=(const Vector4<T> &_pt)
360  {
361  this->data[0] *= _pt[0];
362  this->data[1] *= _pt[1];
363  this->data[2] *= _pt[2];
364  this->data[3] *= _pt[3];
365 
366  return *this;
367  }
368 
372  public: const Vector4<T> operator*(T _v) const
373  {
374  return Vector4<T>(this->data[0] * _v, this->data[1] * _v,
375  this->data[2] * _v, this->data[3] * _v);
376  }
377 
382  public: friend inline const Vector4 operator*(const T _s,
383  const Vector4 &_v)
384  {
385  return Vector4(_v * _s);
386  }
387 
391  public: const Vector4<T> &operator*=(T _v)
392  {
393  this->data[0] *= _v;
394  this->data[1] *= _v;
395  this->data[2] *= _v;
396  this->data[3] *= _v;
397 
398  return *this;
399  }
400 
406  public: bool Equal(const Vector4 &_v, const T &_tol) const
407  {
408  return equal<T>(this->data[0], _v[0], _tol)
409  && equal<T>(this->data[1], _v[1], _tol)
410  && equal<T>(this->data[2], _v[2], _tol)
411  && equal<T>(this->data[3], _v[3], _tol);
412  }
413 
418  public: bool operator==(const Vector4<T> &_v) const
419  {
420  return this->Equal(_v, static_cast<T>(1e-6));
421  }
422 
427  public: bool operator!=(const Vector4<T> &_pt) const
428  {
429  return !(*this == _pt);
430  }
431 
434  public: bool IsFinite() const
435  {
436  // std::isfinite works with floating point values,
437  // need to explicit cast to avoid ambiguity in vc++.
438  return std::isfinite(static_cast<double>(this->data[0])) &&
439  std::isfinite(static_cast<double>(this->data[1])) &&
440  std::isfinite(static_cast<double>(this->data[2])) &&
441  std::isfinite(static_cast<double>(this->data[3]));
442  }
448  public: inline T operator[](size_t _index) const
449  {
450  if (_index > 3)
451  throw IndexException();
452  return this->data[_index];
453  }
454 
457  public: inline T X() const
458  {
459  return this->data[0];
460  }
461 
464  public: inline T Y() const
465  {
466  return this->data[1];
467  }
468 
471  public: inline T Z() const
472  {
473  return this->data[2];
474  }
475 
478  public: inline T W() const
479  {
480  return this->data[3];
481  }
482 
485  public: inline void X(const T &_v)
486  {
487  this->data[0] = _v;
488  }
489 
492  public: inline void Y(const T &_v)
493  {
494  this->data[1] = _v;
495  }
496 
499  public: inline void Z(const T &_v)
500  {
501  this->data[2] = _v;
502  }
503 
506  public: inline void W(const T &_v)
507  {
508  this->data[3] = _v;
509  }
510 
515  public: friend std::ostream &operator<<(
516  std::ostream &_out, const ignition::math::Vector4<T> &_pt)
517  {
518  _out << _pt[0] << " " << _pt[1] << " " << _pt[2] << " " << _pt[3];
519  return _out;
520  }
521 
526  public: friend std::istream &operator>>(
527  std::istream &_in, ignition::math::Vector4<T> &_pt)
528  {
529  T x, y, z, w;
530 
531  // Skip white spaces
532  _in.setf(std::ios_base::skipws);
533  _in >> x >> y >> z >> w;
534  _pt.Set(x, y, z, w);
535  return _in;
536  }
537 
539  private: T data[4];
540  };
541 
542  template<typename T>
543  const Vector4<T> Vector4<T>::Zero(0, 0, 0, 0);
544 
545  template<typename T>
546  const Vector4<T> Vector4<T>::One(1, 1, 1, 1);
547 
551  }
552 }
553 #endif
friend Vector4< T > operator+(const T _s, const Vector4< T > &_v)
Addition operators.
Definition: Vector4.hh:188
Vector4< T > operator+(const T _s) const
Addition operators.
Definition: Vector4.hh:176
Vector4 operator-() const
Negation operator.
Definition: Vector4.hh:209
Vector4< int > Vector4i
Definition: Vector4.hh:548
const Vector4< T > & operator-=(const Vector4< T > &_v)
Subtraction assigment operators.
Definition: Vector4.hh:229
const Vector4< T > operator*(const Vector4< T > &_pt) const
Multiplication operator.
Definition: Vector4.hh:330
Vector4< double > Vector4d
Definition: Vector4.hh:549
bool Equal(const Vector4 &_v, const T &_tol) const
Equality test with tolerance.
Definition: Vector4.hh:406
const Vector4< T > operator/(const Vector4< T > &_v) const
Division assignment operator.
Definition: Vector4.hh:278
friend std::ostream & operator<<(std::ostream &_out, const ignition::math::Vector4< T > &_pt)
Stream insertion operator.
Definition: Vector4.hh:515
Vector4< T > & operator=(const Vector4< T > &_v)
Assignment operator.
Definition: Vector4.hh:127
Vector4()
Constructor.
Definition: Vector4.hh:38
friend const Vector4 operator*(const T _s, const Vector4 &_v)
Scalar left multiplication operators.
Definition: Vector4.hh:382
Vector4< T > & operator=(T _value)
Assignment operator.
Definition: Vector4.hh:139
void Z(const T &_v)
Set the z value.
Definition: Vector4.hh:499
const Vector4< T > & operator-=(const T _s)
Subtraction assignment operator.
Definition: Vector4.hh:263
Vector4(const Vector4< T > &_v)
Copy constructor.
Definition: Vector4.hh:58
const Vector4< T > operator*(const Matrix4< T > &_m) const
Matrix multiplication operator.
Definition: Vector4.hh:341
A 4x4 matrix class.
Definition: Matrix4.hh:34
bool IsFinite() const
See if a point is finite (e.g., not nan)
Definition: Vector4.hh:434
T Y() const
Get the y value.
Definition: Vector4.hh:464
void Normalize()
Normalize the vector length.
Definition: Vector4.hh:98
virtual ~Vector4()
Destructor.
Definition: Vector4.hh:67
const Vector4< T > & operator*=(T _v)
Multiplication assignment operator.
Definition: Vector4.hh:391
void Set(T _x=0, T _y=0, T _z=0, T _w=0)
Set the contents of the vector.
Definition: Vector4.hh:116
bool operator!=(const Vector4< T > &_pt) const
Not equal to operator.
Definition: Vector4.hh:427
Vector4< T > operator-(const Vector4< T > &_v) const
Subtraction operator.
Definition: Vector4.hh:218
T W() const
Get the w value.
Definition: Vector4.hh:478
Vector4< float > Vector4f
Definition: Vector4.hh:550
void W(const T &_v)
Set the w value.
Definition: Vector4.hh:506
const Vector4< T > & operator+=(const T _s)
Addition assignment operator.
Definition: Vector4.hh:197
const Vector4< T > & operator*=(const Vector4< T > &_pt)
Multiplication assignment operator.
Definition: Vector4.hh:359
T Length() const
Returns the length (magnitude) of the vector.
Definition: Vector4.hh:82
T operator[](size_t _index) const
Array subscript operator.
Definition: Vector4.hh:448
const Vector4< T > & operator/=(const Vector4< T > &_v)
Division assignment operator.
Definition: Vector4.hh:291
Exception that is thrown when an out-of-bounds index is encountered.
Definition: IndexException.hh:37
void Y(const T &_v)
Set the y value.
Definition: Vector4.hh:492
const Vector4< T > & operator/=(T _v)
Division operator.
Definition: Vector4.hh:315
void X(const T &_v)
Set the x value.
Definition: Vector4.hh:485
static const Vector4< T > One
math::Vector4(1, 1, 1, 1)
Definition: Vector4.hh:35
Vector4(const T &_x, const T &_y, const T &_z, const T &_w)
Constructor with component values.
Definition: Vector4.hh:48
const Vector4< T > operator*(T _v) const
Multiplication operators.
Definition: Vector4.hh:372
T Z() const
Get the z value.
Definition: Vector4.hh:471
Vector4< T > operator+(const Vector4< T > &_v) const
Addition operator.
Definition: Vector4.hh:152
T X() const
Get the x value.
Definition: Vector4.hh:457
Vector4< T > operator-(const T _s) const
Subtraction operators.
Definition: Vector4.hh:242
Definition: AffineException.hh:30
friend std::istream & operator>>(std::istream &_in, ignition::math::Vector4< T > &_pt)
Stream extraction operator.
Definition: Vector4.hh:526
friend Vector4< T > operator-(const T _s, const Vector4< T > &_v)
Subtraction operators.
Definition: Vector4.hh:254
const Vector4< T > operator/(T _v) const
Division assignment operator.
Definition: Vector4.hh:306
T Distance(const Vector4< T > &_pt) const
Calc distance to the given point.
Definition: Vector4.hh:72
T Generic x, y, z, w vector.
Definition: Vector4.hh:29
static const Vector4< T > Zero
math::Vector4(0, 0, 0, 0)
Definition: Vector4.hh:32
T SquaredLength() const
Return the square of the length (magnitude) of the vector.
Definition: Vector4.hh:89
bool operator==(const Vector4< T > &_v) const
Equal to operator.
Definition: Vector4.hh:418
const Vector4< T > & operator+=(const Vector4< T > &_v)
Addition operator.
Definition: Vector4.hh:163