All Classes Namespaces Files Functions Variables Typedefs Friends Macros Pages
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 
81  public: T Length() const
82  {
83  return sqrt(
84  this->data[0] * this->data[0] +
85  this->data[1] * this->data[1] +
86  this->data[2] * this->data[2] +
87  this->data[3] * this->data[3]);
88  }
89 
92  public: T SquaredLength() const
93  {
94  return this->data[0] * this->data[0] + this->data[1] * this->data[1] +
95  this->data[2] * this->data[2] + this->data[3] * this->data[3];
96  }
97 
99  public: void Normalize()
100  {
101  T d = this->Length();
102 
103  this->data[0] /= d;
104  this->data[1] /= d;
105  this->data[2] /= d;
106  this->data[3] /= d;
107  }
108 
114  public: void Set(T _x = 0, T _y = 0, T _z = 0, T _w = 0)
115  {
116  this->data[0] = _x;
117  this->data[1] = _y;
118  this->data[2] = _z;
119  this->data[3] = _w;
120  }
121 
125  public: Vector4<T> &operator=(const Vector4<T> &_v)
126  {
127  this->data[0] = _v[0];
128  this->data[1] = _v[1];
129  this->data[2] = _v[2];
130  this->data[3] = _v[3];
131 
132  return *this;
133  }
134 
137  public: Vector4<T> &operator=(T _value)
138  {
139  this->data[0] = _value;
140  this->data[1] = _value;
141  this->data[2] = _value;
142  this->data[3] = _value;
143 
144  return *this;
145  }
146 
150  public: Vector4<T> operator+(const Vector4<T> &_v) const
151  {
152  return Vector4<T>(this->data[0] + _v[0],
153  this->data[1] + _v[1],
154  this->data[2] + _v[2],
155  this->data[3] + _v[3]);
156  }
157 
161  public: const Vector4<T> &operator+=(const Vector4<T> &_v)
162  {
163  this->data[0] += _v[0];
164  this->data[1] += _v[1];
165  this->data[2] += _v[2];
166  this->data[3] += _v[3];
167 
168  return *this;
169  }
170 
174  public: Vector4<T> operator-(const Vector4<T> &_v) const
175  {
176  return Vector4<T>(this->data[0] - _v[0],
177  this->data[1] - _v[1],
178  this->data[2] - _v[2],
179  this->data[3] - _v[3]);
180  }
181 
185  public: const Vector4<T> &operator-=(const Vector4<T> &_v)
186  {
187  this->data[0] -= _v[0];
188  this->data[1] -= _v[1];
189  this->data[2] -= _v[2];
190  this->data[3] -= _v[3];
191 
192  return *this;
193  }
194 
200  public: const Vector4<T> operator/(const Vector4<T> &_v) const
201  {
202  return Vector4<T>(this->data[0] / _v[0],
203  this->data[1] / _v[1],
204  this->data[2] / _v[2],
205  this->data[3] / _v[3]);
206  }
207 
213  public: const Vector4<T> &operator/=(const Vector4<T> &_v)
214  {
215  this->data[0] /= _v[0];
216  this->data[1] /= _v[1];
217  this->data[2] /= _v[2];
218  this->data[3] /= _v[3];
219 
220  return *this;
221  }
222 
228  public: const Vector4<T> operator/(T _v) const
229  {
230  return Vector4<T>(this->data[0] / _v, this->data[1] / _v,
231  this->data[2] / _v, this->data[3] / _v);
232  }
233 
237  public: const Vector4<T> &operator/=(T _v)
238  {
239  this->data[0] /= _v;
240  this->data[1] /= _v;
241  this->data[2] /= _v;
242  this->data[3] /= _v;
243 
244  return *this;
245  }
246 
252  public: const Vector4<T> operator*(const Vector4<T> &_pt) const
253  {
254  return Vector4<T>(this->data[0] * _pt[0],
255  this->data[1] * _pt[1],
256  this->data[2] * _pt[2],
257  this->data[3] * _pt[3]);
258  }
259 
263  public: const Vector4<T> operator*(const Matrix4<T> &_m) const
264  {
265  return Vector4<T>(
266  this->data[0]*_m(0, 0) + this->data[1]*_m(1, 0) +
267  this->data[2]*_m(2, 0) + this->data[3]*_m(3, 0),
268  this->data[0]*_m(0, 1) + this->data[1]*_m(1, 1) +
269  this->data[2]*_m(2, 1) + this->data[3]*_m(3, 1),
270  this->data[0]*_m(0, 2) + this->data[1]*_m(1, 2) +
271  this->data[2]*_m(2, 2) + this->data[3]*_m(3, 2),
272  this->data[0]*_m(0, 3) + this->data[1]*_m(1, 3) +
273  this->data[2]*_m(2, 3) + this->data[3]*_m(3, 3));
274  }
275 
281  public: const Vector4<T> &operator*=(const Vector4<T> &_pt)
282  {
283  this->data[0] *= _pt[0];
284  this->data[1] *= _pt[1];
285  this->data[2] *= _pt[2];
286  this->data[3] *= _pt[3];
287 
288  return *this;
289  }
290 
294  public: const Vector4<T> operator*(T _v) const
295  {
296  return Vector4<T>(this->data[0] * _v, this->data[1] * _v,
297  this->data[2] * _v, this->data[3] * _v);
298  }
299 
303  public: const Vector4<T> &operator*=(T _v)
304  {
305  this->data[0] *= _v;
306  this->data[1] *= _v;
307  this->data[2] *= _v;
308  this->data[3] *= _v;
309 
310  return *this;
311  }
312 
317  public: bool operator==(const Vector4<T> &_v) const
318  {
319  return equal(this->data[0], _v[0]) && equal(this->data[1], _v[1]) &&
320  equal(this->data[2], _v[2]) && equal(this->data[3], _v[3]);
321  }
322 
327  public: bool operator!=(const Vector4<T> &_pt) const
328  {
329  return !(*this == _pt);
330  }
331 
334  public: bool IsFinite() const
335  {
336  // std::isfinite works with floating point values,
337  // need to explicit cast to avoid ambiguity in vc++.
338  return std::isfinite(static_cast<double>(this->data[0])) &&
339  std::isfinite(static_cast<double>(this->data[1])) &&
340  std::isfinite(static_cast<double>(this->data[2])) &&
341  std::isfinite(static_cast<double>(this->data[3]));
342  }
348  public: inline T operator[](size_t _index) const
349  {
350  if (_index > 3)
351  throw IndexException();
352  return this->data[_index];
353  }
354 
357  public: inline T X() const
358  {
359  return this->data[0];
360  }
361 
364  public: inline T Y() const
365  {
366  return this->data[1];
367  }
368 
371  public: inline T Z() const
372  {
373  return this->data[2];
374  }
375 
378  public: inline T W() const
379  {
380  return this->data[3];
381  }
382 
385  public: inline void X(const T &_v)
386  {
387  this->data[0] = _v;
388  }
389 
392  public: inline void Y(const T &_v)
393  {
394  this->data[1] = _v;
395  }
396 
399  public: inline void Z(const T &_v)
400  {
401  this->data[2] = _v;
402  }
403 
406  public: inline void W(const T &_v)
407  {
408  this->data[3] = _v;
409  }
410 
415  public: friend std::ostream &operator<<(
416  std::ostream &_out, const ignition::math::Vector4<T> &_pt)
417  {
418  _out << _pt[0] << " " << _pt[1] << " " << _pt[2] << " " << _pt[3];
419  return _out;
420  }
421 
426  public: friend std::istream &operator>>(
427  std::istream &_in, ignition::math::Vector4<T> &_pt)
428  {
429  T x, y, z, w;
430 
431  // Skip white spaces
432  _in.setf(std::ios_base::skipws);
433  _in >> x >> y >> z >> w;
434  _pt.Set(x, y, z, w);
435  return _in;
436  }
437 
439  private: T data[4];
440  };
441 
442  template<typename T>
443  const Vector4<T> Vector4<T>::Zero(0, 0, 0, 0);
444 
445  template<typename T>
446  const Vector4<T> Vector4<T>::One(1, 1, 1, 1);
447 
451  }
452 }
453 #endif
Vector4< int > Vector4i
Definition: Vector4.hh:448
const Vector4< T > & operator-=(const Vector4< T > &_v)
Subtraction assigment operators.
Definition: Vector4.hh:185
const Vector4< T > operator*(const Vector4< T > &_pt) const
Multiplication operator.
Definition: Vector4.hh:252
Vector4< double > Vector4d
Definition: Vector4.hh:449
const Vector4< T > operator/(const Vector4< T > &_v) const
Division assignment operator.
Definition: Vector4.hh:200
friend std::ostream & operator<<(std::ostream &_out, const ignition::math::Vector4< T > &_pt)
Stream insertion operator.
Definition: Vector4.hh:415
Vector4< T > & operator=(const Vector4< T > &_v)
Assignment operator.
Definition: Vector4.hh:125
Vector4()
Constructor.
Definition: Vector4.hh:38
Vector4< T > & operator=(T _value)
Assignment operator.
Definition: Vector4.hh:137
void Z(const T &_v)
Set the z value.
Definition: Vector4.hh:399
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:263
A 4x4 matrix class.
Definition: Matrix4.hh:33
bool IsFinite() const
See if a point is finite (e.g., not nan)
Definition: Vector4.hh:334
T Y() const
Get the y value.
Definition: Vector4.hh:364
void Normalize()
Normalize the vector length.
Definition: Vector4.hh:99
virtual ~Vector4()
Destructor.
Definition: Vector4.hh:67
const Vector4< T > & operator*=(T _v)
Multiplication assignment operator.
Definition: Vector4.hh:303
void Set(T _x=0, T _y=0, T _z=0, T _w=0)
Set the contents of the vector.
Definition: Vector4.hh:114
bool operator!=(const Vector4< T > &_pt) const
Not equal to operator.
Definition: Vector4.hh:327
Vector4< T > operator-(const Vector4< T > &_v) const
Subtraction operator.
Definition: Vector4.hh:174
T W() const
Get the w value.
Definition: Vector4.hh:378
Vector4< float > Vector4f
Definition: Vector4.hh:450
void W(const T &_v)
Set the w value.
Definition: Vector4.hh:406
const Vector4< T > & operator*=(const Vector4< T > &_pt)
Multiplication assignment operator.
Definition: Vector4.hh:281
T Length() const
Returns the length (magnitude) of the vector.
Definition: Vector4.hh:81
T operator[](size_t _index) const
Array subscript operator.
Definition: Vector4.hh:348
const Vector4< T > & operator/=(const Vector4< T > &_v)
Division assignment operator.
Definition: Vector4.hh:213
Exception that is thrown when an out-of-bounds index is encountered.
Definition: IndexException.hh:30
void Y(const T &_v)
Set the y value.
Definition: Vector4.hh:392
const Vector4< T > & operator/=(T _v)
Division operator.
Definition: Vector4.hh:237
void X(const T &_v)
Set the x value.
Definition: Vector4.hh:385
static const Vector4< T > One
math::Vector3(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:294
T Z() const
Get the z value.
Definition: Vector4.hh:371
Vector4< T > operator+(const Vector4< T > &_v) const
Addition operator.
Definition: Vector4.hh:150
T X() const
Get the x value.
Definition: Vector4.hh:357
friend std::istream & operator>>(std::istream &_in, ignition::math::Vector4< T > &_pt)
Stream extraction operator.
Definition: Vector4.hh:426
bool equal(const T &_a, const T &_b, const T &_epsilon=1e-6)
check if two values are equal, within a tolerance
Definition: Helpers.hh:177
const Vector4< T > operator/(T _v) const
Division assignment operator.
Definition: Vector4.hh:228
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::Vector3(0, 0, 0)
Definition: Vector4.hh:32
T SquaredLength() const
Return the square of the length (magnitude) of the vector.
Definition: Vector4.hh:92
bool operator==(const Vector4< T > &_v) const
Equal to operator.
Definition: Vector4.hh:317
const Vector4< T > & operator+=(const Vector4< T > &_v)
Addition operator.
Definition: Vector4.hh:161