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 <ignition/math/Matrix4.hh>
21 #include <ignition/math/Helpers.hh>
22 #include <ignition/math/config.hh>
23 
24 namespace ignition
25 {
26  namespace math
27  {
28  inline namespace IGNITION_MATH_VERSION_NAMESPACE
29  {
32  template<typename T>
33  class Vector4
34  {
36  public: static const Vector4<T> Zero;
37 
39  public: static const Vector4<T> One;
40 
42  public: Vector4()
43  {
44  this->data[0] = this->data[1] = this->data[2] = this->data[3] = 0;
45  }
46 
52  public: Vector4(const T &_x, const T &_y, const T &_z, const T &_w)
53  {
54  this->data[0] = _x;
55  this->data[1] = _y;
56  this->data[2] = _z;
57  this->data[3] = _w;
58  }
59 
62  public: Vector4(const Vector4<T> &_v)
63  {
64  this->data[0] = _v[0];
65  this->data[1] = _v[1];
66  this->data[2] = _v[2];
67  this->data[3] = _v[3];
68  }
69 
71  public: virtual ~Vector4() {}
72 
76  public: T Distance(const Vector4<T> &_pt) const
77  {
78  return sqrt((this->data[0]-_pt[0])*(this->data[0]-_pt[0]) +
79  (this->data[1]-_pt[1])*(this->data[1]-_pt[1]) +
80  (this->data[2]-_pt[2])*(this->data[2]-_pt[2]) +
81  (this->data[3]-_pt[3])*(this->data[3]-_pt[3]));
82  }
83 
86  public: T Length() const
87  {
88  return sqrt(this->SquaredLength());
89  }
90 
93  public: T SquaredLength() const
94  {
95  return std::pow(this->data[0], 2)
96  + std::pow(this->data[1], 2)
97  + std::pow(this->data[2], 2)
98  + std::pow(this->data[3], 2);
99  }
100 
102  public: void Normalize()
103  {
104  T d = this->Length();
105 
106  if (!equal<T>(d, static_cast<T>(0.0)))
107  {
108  this->data[0] /= d;
109  this->data[1] /= d;
110  this->data[2] /= d;
111  this->data[3] /= d;
112  }
113  }
114 
120  public: void Set(T _x = 0, T _y = 0, T _z = 0, T _w = 0)
121  {
122  this->data[0] = _x;
123  this->data[1] = _y;
124  this->data[2] = _z;
125  this->data[3] = _w;
126  }
127 
131  public: Vector4<T> &operator=(const Vector4<T> &_v)
132  {
133  this->data[0] = _v[0];
134  this->data[1] = _v[1];
135  this->data[2] = _v[2];
136  this->data[3] = _v[3];
137 
138  return *this;
139  }
140 
143  public: Vector4<T> &operator=(T _value)
144  {
145  this->data[0] = _value;
146  this->data[1] = _value;
147  this->data[2] = _value;
148  this->data[3] = _value;
149 
150  return *this;
151  }
152 
156  public: Vector4<T> operator+(const Vector4<T> &_v) const
157  {
158  return Vector4<T>(this->data[0] + _v[0],
159  this->data[1] + _v[1],
160  this->data[2] + _v[2],
161  this->data[3] + _v[3]);
162  }
163 
167  public: const Vector4<T> &operator+=(const Vector4<T> &_v)
168  {
169  this->data[0] += _v[0];
170  this->data[1] += _v[1];
171  this->data[2] += _v[2];
172  this->data[3] += _v[3];
173 
174  return *this;
175  }
176 
180  public: inline Vector4<T> operator+(const T _s) const
181  {
182  return Vector4<T>(this->data[0] + _s,
183  this->data[1] + _s,
184  this->data[2] + _s,
185  this->data[3] + _s);
186  }
187 
192  public: friend inline Vector4<T> operator+(const T _s,
193  const Vector4<T> &_v)
194  {
195  return _v + _s;
196  }
197 
201  public: const Vector4<T> &operator+=(const T _s)
202  {
203  this->data[0] += _s;
204  this->data[1] += _s;
205  this->data[2] += _s;
206  this->data[3] += _s;
207 
208  return *this;
209  }
210 
213  public: inline Vector4 operator-() const
214  {
215  return Vector4(-this->data[0], -this->data[1],
216  -this->data[2], -this->data[3]);
217  }
218 
222  public: Vector4<T> operator-(const Vector4<T> &_v) const
223  {
224  return Vector4<T>(this->data[0] - _v[0],
225  this->data[1] - _v[1],
226  this->data[2] - _v[2],
227  this->data[3] - _v[3]);
228  }
229 
233  public: const Vector4<T> &operator-=(const Vector4<T> &_v)
234  {
235  this->data[0] -= _v[0];
236  this->data[1] -= _v[1];
237  this->data[2] -= _v[2];
238  this->data[3] -= _v[3];
239 
240  return *this;
241  }
242 
246  public: inline Vector4<T> operator-(const T _s) const
247  {
248  return Vector4<T>(this->data[0] - _s,
249  this->data[1] - _s,
250  this->data[2] - _s,
251  this->data[3] - _s);
252  }
253 
258  public: friend inline Vector4<T> operator-(const T _s,
259  const Vector4<T> &_v)
260  {
261  return {_s - _v.X(), _s - _v.Y(), _s - _v.Z(), _s - _v.W()};
262  }
263 
267  public: const Vector4<T> &operator-=(const T _s)
268  {
269  this->data[0] -= _s;
270  this->data[1] -= _s;
271  this->data[2] -= _s;
272  this->data[3] -= _s;
273 
274  return *this;
275  }
276 
282  public: const Vector4<T> operator/(const Vector4<T> &_v) const
283  {
284  return Vector4<T>(this->data[0] / _v[0],
285  this->data[1] / _v[1],
286  this->data[2] / _v[2],
287  this->data[3] / _v[3]);
288  }
289 
295  public: const Vector4<T> &operator/=(const Vector4<T> &_v)
296  {
297  this->data[0] /= _v[0];
298  this->data[1] /= _v[1];
299  this->data[2] /= _v[2];
300  this->data[3] /= _v[3];
301 
302  return *this;
303  }
304 
310  public: const Vector4<T> operator/(T _v) const
311  {
312  return Vector4<T>(this->data[0] / _v, this->data[1] / _v,
313  this->data[2] / _v, this->data[3] / _v);
314  }
315 
319  public: const Vector4<T> &operator/=(T _v)
320  {
321  this->data[0] /= _v;
322  this->data[1] /= _v;
323  this->data[2] /= _v;
324  this->data[3] /= _v;
325 
326  return *this;
327  }
328 
334  public: const Vector4<T> operator*(const Vector4<T> &_pt) const
335  {
336  return Vector4<T>(this->data[0] * _pt[0],
337  this->data[1] * _pt[1],
338  this->data[2] * _pt[2],
339  this->data[3] * _pt[3]);
340  }
341 
345  public: const Vector4<T> operator*(const Matrix4<T> &_m) const
346  {
347  return Vector4<T>(
348  this->data[0]*_m(0, 0) + this->data[1]*_m(1, 0) +
349  this->data[2]*_m(2, 0) + this->data[3]*_m(3, 0),
350  this->data[0]*_m(0, 1) + this->data[1]*_m(1, 1) +
351  this->data[2]*_m(2, 1) + this->data[3]*_m(3, 1),
352  this->data[0]*_m(0, 2) + this->data[1]*_m(1, 2) +
353  this->data[2]*_m(2, 2) + this->data[3]*_m(3, 2),
354  this->data[0]*_m(0, 3) + this->data[1]*_m(1, 3) +
355  this->data[2]*_m(2, 3) + this->data[3]*_m(3, 3));
356  }
357 
363  public: const Vector4<T> &operator*=(const Vector4<T> &_pt)
364  {
365  this->data[0] *= _pt[0];
366  this->data[1] *= _pt[1];
367  this->data[2] *= _pt[2];
368  this->data[3] *= _pt[3];
369 
370  return *this;
371  }
372 
376  public: const Vector4<T> operator*(T _v) const
377  {
378  return Vector4<T>(this->data[0] * _v, this->data[1] * _v,
379  this->data[2] * _v, this->data[3] * _v);
380  }
381 
386  public: friend inline const Vector4 operator*(const T _s,
387  const Vector4 &_v)
388  {
389  return Vector4(_v * _s);
390  }
391 
395  public: const Vector4<T> &operator*=(T _v)
396  {
397  this->data[0] *= _v;
398  this->data[1] *= _v;
399  this->data[2] *= _v;
400  this->data[3] *= _v;
401 
402  return *this;
403  }
404 
410  public: bool Equal(const Vector4 &_v, const T &_tol) const
411  {
412  return equal<T>(this->data[0], _v[0], _tol)
413  && equal<T>(this->data[1], _v[1], _tol)
414  && equal<T>(this->data[2], _v[2], _tol)
415  && equal<T>(this->data[3], _v[3], _tol);
416  }
417 
422  public: bool operator==(const Vector4<T> &_v) const
423  {
424  return this->Equal(_v, static_cast<T>(1e-6));
425  }
426 
431  public: bool operator!=(const Vector4<T> &_pt) const
432  {
433  return !(*this == _pt);
434  }
435 
438  public: bool IsFinite() const
439  {
440  // std::isfinite works with floating point values,
441  // need to explicit cast to avoid ambiguity in vc++.
442  return std::isfinite(static_cast<double>(this->data[0])) &&
443  std::isfinite(static_cast<double>(this->data[1])) &&
444  std::isfinite(static_cast<double>(this->data[2])) &&
445  std::isfinite(static_cast<double>(this->data[3]));
446  }
447 
452  public: T &operator[](const std::size_t _index)
453  {
454  return this->data[clamp(_index, IGN_ZERO_SIZE_T, IGN_THREE_SIZE_T)];
455  }
456 
461  public: T operator[](const std::size_t _index) const
462  {
463  return this->data[clamp(_index, IGN_ZERO_SIZE_T, IGN_THREE_SIZE_T)];
464  }
465 
468  public: T &X()
469  {
470  return this->data[0];
471  }
472 
475  public: T &Y()
476  {
477  return this->data[1];
478  }
479 
482  public: T &Z()
483  {
484  return this->data[2];
485  }
486 
489  public: T &W()
490  {
491  return this->data[3];
492  }
493 
496  public: T X() const
497  {
498  return this->data[0];
499  }
500 
503  public: T Y() const
504  {
505  return this->data[1];
506  }
507 
510  public: T Z() const
511  {
512  return this->data[2];
513  }
514 
517  public: T W() const
518  {
519  return this->data[3];
520  }
521 
524  public: inline void X(const T &_v)
525  {
526  this->data[0] = _v;
527  }
528 
531  public: inline void Y(const T &_v)
532  {
533  this->data[1] = _v;
534  }
535 
538  public: inline void Z(const T &_v)
539  {
540  this->data[2] = _v;
541  }
542 
545  public: inline void W(const T &_v)
546  {
547  this->data[3] = _v;
548  }
549 
554  public: friend std::ostream &operator<<(
555  std::ostream &_out, const ignition::math::Vector4<T> &_pt)
556  {
557  _out << _pt[0] << " " << _pt[1] << " " << _pt[2] << " " << _pt[3];
558  return _out;
559  }
560 
565  public: friend std::istream &operator>>(
566  std::istream &_in, ignition::math::Vector4<T> &_pt)
567  {
568  T x, y, z, w;
569 
570  // Skip white spaces
571  _in.setf(std::ios_base::skipws);
572  _in >> x >> y >> z >> w;
573  _pt.Set(x, y, z, w);
574  return _in;
575  }
576 
578  private: T data[4];
579  };
580 
581  template<typename T>
582  const Vector4<T> Vector4<T>::Zero(0, 0, 0, 0);
583 
584  template<typename T>
585  const Vector4<T> Vector4<T>::One(1, 1, 1, 1);
586 
590  }
591  }
592 }
593 #endif
Helpers.hh
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::operator=
Vector4< T > & operator=(T _value)
Assignment operator.
Definition: Vector4.hh:143
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::Y
T & Y()
Return a mutable y value.
Definition: Vector4.hh:475
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::Vector4
Vector4()
Constructor.
Definition: Vector4.hh:42
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::Z
void Z(const T &_v)
Set the z value.
Definition: Vector4.hh:538
ignition
Definition: Angle.hh:40
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::operator-
friend Vector4< T > operator-(const T _s, const Vector4< T > &_v)
Subtraction operators.
Definition: Vector4.hh:258
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::operator+=
const Vector4< T > & operator+=(const T _s)
Addition assignment operator.
Definition: Vector4.hh:201
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::operator[]
T operator[](const std::size_t _index) const
Const-qualified array subscript operator.
Definition: Vector4.hh:461
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::operator+
Vector4< T > operator+(const Vector4< T > &_v) const
Addition operator.
Definition: Vector4.hh:156
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::operator>>
friend std::istream & operator>>(std::istream &_in, ignition::math::Vector4< T > &_pt)
Stream extraction operator.
Definition: Vector4.hh:565
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::operator-
Vector4< T > operator-(const Vector4< T > &_v) const
Subtraction operator.
Definition: Vector4.hh:222
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::operator+
Vector4< T > operator+(const T _s) const
Addition operators.
Definition: Vector4.hh:180
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::operator*
const Vector4< T > operator*(const Matrix4< T > &_m) const
Matrix multiplication operator.
Definition: Vector4.hh:345
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4f
Vector4< float > Vector4f
Definition: Vector4.hh:589
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::operator!=
bool operator!=(const Vector4< T > &_pt) const
Not equal to operator.
Definition: Vector4.hh:431
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::Equal
bool Equal(const Vector4 &_v, const T &_tol) const
Equality test with tolerance.
Definition: Vector4.hh:410
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::operator/=
const Vector4< T > & operator/=(const Vector4< T > &_v)
Division assignment operator.
Definition: Vector4.hh:295
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::operator[]
T & operator[](const std::size_t _index)
Array subscript operator.
Definition: Vector4.hh:452
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::operator-
Vector4< T > operator-(const T _s) const
Subtraction operators.
Definition: Vector4.hh:246
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::Normalize
void Normalize()
Normalize the vector length.
Definition: Vector4.hh:102
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Matrix4
A 4x4 matrix class.
Definition: Matrix4.hh:37
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::operator*
const Vector4< T > operator*(const Vector4< T > &_pt) const
Multiplication operator.
Definition: Vector4.hh:334
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::operator-
Vector4 operator-() const
Negation operator.
Definition: Vector4.hh:213
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::X
T X() const
Get the x value.
Definition: Vector4.hh:496
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::Y
T Y() const
Get the y value.
Definition: Vector4.hh:503
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::Vector4
Vector4(const Vector4< T > &_v)
Copy constructor.
Definition: Vector4.hh:62
Matrix4.hh
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4
T Generic x, y, z, w vector.
Definition: Vector4.hh:34
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::Z
T & Z()
Return a mutable z value.
Definition: Vector4.hh:482
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::X
void X(const T &_v)
Set the x value.
Definition: Vector4.hh:524
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::W
T W() const
Get the w value.
Definition: Vector4.hh:517
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::operator*=
const Vector4< T > & operator*=(const Vector4< T > &_pt)
Multiplication assignment operator.
Definition: Vector4.hh:363
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::operator/=
const Vector4< T > & operator/=(T _v)
Division operator.
Definition: Vector4.hh:319
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::IsFinite
bool IsFinite() const
See if a point is finite (e.g., not nan)
Definition: Vector4.hh:438
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::Length
T Length() const
Returns the length (magnitude) of the vector.
Definition: Vector4.hh:86
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::operator/
const Vector4< T > operator/(T _v) const
Division assignment operator.
Definition: Vector4.hh:310
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::operator+=
const Vector4< T > & operator+=(const Vector4< T > &_v)
Addition operator.
Definition: Vector4.hh:167
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::Y
void Y(const T &_v)
Set the y value.
Definition: Vector4.hh:531
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::IGN_ZERO_SIZE_T
static const size_t IGN_ZERO_SIZE_T
size_t type with a value of 0
Definition: Helpers.hh:216
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::operator*
const Vector4< T > operator*(T _v) const
Multiplication operators.
Definition: Vector4.hh:376
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::Zero
static const Vector4< T > Zero
math::Vector4(0, 0, 0, 0)
Definition: Vector4.hh:36
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::operator==
bool operator==(const Vector4< T > &_v) const
Equal to operator.
Definition: Vector4.hh:422
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4d
Vector4< double > Vector4d
Definition: Vector4.hh:588
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::IGN_THREE_SIZE_T
static const size_t IGN_THREE_SIZE_T
size_t type with a value of 3
Definition: Helpers.hh:225
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::operator-=
const Vector4< T > & operator-=(const T _s)
Subtraction assignment operator.
Definition: Vector4.hh:267
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::W
T & W()
Return a mutable w value.
Definition: Vector4.hh:489
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::operator+
friend Vector4< T > operator+(const T _s, const Vector4< T > &_v)
Addition operators.
Definition: Vector4.hh:192
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::X
T & X()
Return a mutable x value.
Definition: Vector4.hh:468
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::operator-=
const Vector4< T > & operator-=(const Vector4< T > &_v)
Subtraction assigment operators.
Definition: Vector4.hh:233
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::Vector4
Vector4(const T &_x, const T &_y, const T &_z, const T &_w)
Constructor with component values.
Definition: Vector4.hh:52
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::operator=
Vector4< T > & operator=(const Vector4< T > &_v)
Assignment operator.
Definition: Vector4.hh:131
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::Distance
T Distance(const Vector4< T > &_pt) const
Calc distance to the given point.
Definition: Vector4.hh:76
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::operator/
const Vector4< T > operator/(const Vector4< T > &_v) const
Division assignment operator.
Definition: Vector4.hh:282
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::~Vector4
virtual ~Vector4()
Destructor.
Definition: Vector4.hh:71
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::SquaredLength
T SquaredLength() const
Return the square of the length (magnitude) of the vector.
Definition: Vector4.hh:93
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::One
static const Vector4< T > One
math::Vector4(1, 1, 1, 1)
Definition: Vector4.hh:39
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::Z
T Z() const
Get the z value.
Definition: Vector4.hh:510
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::operator<<
friend std::ostream & operator<<(std::ostream &_out, const ignition::math::Vector4< T > &_pt)
Stream insertion operator.
Definition: Vector4.hh:554
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::Set
void Set(T _x=0, T _y=0, T _z=0, T _w=0)
Set the contents of the vector.
Definition: Vector4.hh:120
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::clamp
T clamp(T _v, T _min, T _max)
Simple clamping function.
Definition: Helpers.hh:395
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::W
void W(const T &_v)
Set the w value.
Definition: Vector4.hh:545
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4i
Vector4< int > Vector4i
Definition: Vector4.hh:587
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::operator*=
const Vector4< T > & operator*=(T _v)
Multiplication assignment operator.
Definition: Vector4.hh:395
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector4::operator*
friend const Vector4 operator*(const T _s, const Vector4 &_v)
Scalar left multiplication operators.
Definition: Vector4.hh:386