Vector3.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 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_MATH_VECTOR3_HH_
18 #define IGNITION_MATH_VECTOR3_HH_
19 
20 #include <iostream>
21 #include <fstream>
22 #include <cmath>
23 #include <algorithm>
24 
25 #include <ignition/math/Helpers.hh>
27 
28 namespace ignition
29 {
30  namespace math
31  {
36  template<typename T>
37  class Vector3
38  {
40  public: static const Vector3 Zero;
41 
43  public: static const Vector3 One;
44 
46  public: static const Vector3 UnitX;
47 
49  public: static const Vector3 UnitY;
50 
52  public: static const Vector3 UnitZ;
53 
55  public: Vector3()
56  {
57  this->data[0] = 0;
58  this->data[1] = 0;
59  this->data[2] = 0;
60  }
61 
66  public: Vector3(const T &_x, const T &_y, const T &_z)
67  {
68  this->data[0] = _x;
69  this->data[1] = _y;
70  this->data[2] = _z;
71  }
72 
75  public: Vector3(const Vector3<T> &_v)
76  {
77  this->data[0] = _v[0];
78  this->data[1] = _v[1];
79  this->data[2] = _v[2];
80  }
81 
83  public: virtual ~Vector3() {}
84 
87  public: T Sum() const
88  {
89  return this->data[0] + this->data[1] + this->data[2];
90  }
91 
95  public: T Distance(const Vector3<T> &_pt) const
96  {
97  return sqrt((this->data[0]-_pt[0])*(this->data[0]-_pt[0]) +
98  (this->data[1]-_pt[1])*(this->data[1]-_pt[1]) +
99  (this->data[2]-_pt[2])*(this->data[2]-_pt[2]));
100  }
101 
107  public: T Distance(T _x, T _y, T _z) const
108  {
109  return this->Distance(Vector3(_x, _y, _z));
110  }
111 
114  public: T Length() const
115  {
116  return sqrt(this->SquaredLength());
117  }
118 
121  public: T SquaredLength() const
122  {
123  return std::pow(this->data[0], 2)
124  + std::pow(this->data[1], 2)
125  + std::pow(this->data[2], 2);
126  }
127 
130  public: Vector3 Normalize()
131  {
132  T d = this->Length();
133 
134  if (!equal<T>(d, static_cast<T>(0.0)))
135  {
136  this->data[0] /= d;
137  this->data[1] /= d;
138  this->data[2] /= d;
139  }
140 
141  return *this;
142  }
143 
146  public: Vector3 Normalized() const
147  {
148  Vector3<T> result = *this;
149  result.Normalize();
150  return result;
151  }
152 
155  public: Vector3 Round()
156  {
157  this->data[0] = nearbyint(this->data[0]);
158  this->data[1] = nearbyint(this->data[1]);
159  this->data[2] = nearbyint(this->data[2]);
160  return *this;
161  }
162 
165  public: Vector3 Rounded() const
166  {
167  Vector3<T> result = *this;
168  result.Round();
169  return result;
170  }
171 
176  public: inline void Set(T _x = 0, T _y = 0, T _z = 0)
177  {
178  this->data[0] = _x;
179  this->data[1] = _y;
180  this->data[2] = _z;
181  }
182 
186  public: Vector3 Cross(const Vector3<T> &_v) const
187  {
188  return Vector3(this->data[1] * _v[2] - this->data[2] * _v[1],
189  this->data[2] * _v[0] - this->data[0] * _v[2],
190  this->data[0] * _v[1] - this->data[1] * _v[0]);
191  }
192 
196  public: T Dot(const Vector3<T> &_v) const
197  {
198  return this->data[0] * _v[0] +
199  this->data[1] * _v[1] +
200  this->data[2] * _v[2];
201  }
202 
211  public: T AbsDot(const Vector3<T> &_v) const
212  {
213  return std::abs(this->data[0] * _v[0]) +
214  std::abs(this->data[1] * _v[1]) +
215  std::abs(this->data[2] * _v[2]);
216  }
217 
220  public: Vector3 Abs() const
221  {
222  return Vector3(std::abs(this->data[0]),
223  std::abs(this->data[1]),
224  std::abs(this->data[2]));
225  }
226 
229  public: Vector3 Perpendicular() const
230  {
231  static const T sqrZero = 1e-06 * 1e-06;
232 
233  Vector3<T> perp = this->Cross(Vector3(1, 0, 0));
234 
235  // Check the length of the vector
236  if (perp.SquaredLength() < sqrZero)
237  {
238  perp = this->Cross(Vector3(0, 1, 0));
239  }
240 
241  return perp;
242  }
243 
249  public: static Vector3 Normal(const Vector3<T> &_v1,
250  const Vector3<T> &_v2, const Vector3<T> &_v3)
251  {
252  Vector3<T> a = _v2 - _v1;
253  Vector3<T> b = _v3 - _v1;
254  Vector3<T> n = a.Cross(b);
255  return n.Normalize();
256  }
257 
262  public: T DistToLine(const Vector3<T> &_pt1, const Vector3 &_pt2)
263  {
264  T d = ((*this) - _pt1).Cross((*this) - _pt2).Length();
265  d = d / (_pt2 - _pt1).Length();
266  return d;
267  }
268 
272  public: void Max(const Vector3<T> &_v)
273  {
274  if (_v[0] > this->data[0])
275  this->data[0] = _v[0];
276  if (_v[1] > this->data[1])
277  this->data[1] = _v[1];
278  if (_v[2] > this->data[2])
279  this->data[2] = _v[2];
280  }
281 
285  public: void Min(const Vector3<T> &_v)
286  {
287  if (_v[0] < this->data[0])
288  this->data[0] = _v[0];
289  if (_v[1] < this->data[1])
290  this->data[1] = _v[1];
291  if (_v[2] < this->data[2])
292  this->data[2] = _v[2];
293  }
294 
297  public: T Max() const
298  {
299  return std::max(std::max(this->data[0], this->data[1]), this->data[2]);
300  }
301 
304  public: T Min() const
305  {
306  return std::min(std::min(this->data[0], this->data[1]), this->data[2]);
307  }
308 
312  public: Vector3 &operator=(const Vector3<T> &_v)
313  {
314  this->data[0] = _v[0];
315  this->data[1] = _v[1];
316  this->data[2] = _v[2];
317 
318  return *this;
319  }
320 
324  public: Vector3 &operator=(T _v)
325  {
326  this->data[0] = _v;
327  this->data[1] = _v;
328  this->data[2] = _v;
329 
330  return *this;
331  }
332 
336  public: Vector3 operator+(const Vector3<T> &_v) const
337  {
338  return Vector3(this->data[0] + _v[0],
339  this->data[1] + _v[1],
340  this->data[2] + _v[2]);
341  }
342 
346  public: const Vector3 &operator+=(const Vector3<T> &_v)
347  {
348  this->data[0] += _v[0];
349  this->data[1] += _v[1];
350  this->data[2] += _v[2];
351 
352  return *this;
353  }
354 
358  public: inline Vector3<T> operator+(const T _s) const
359  {
360  return Vector3<T>(this->data[0] + _s,
361  this->data[1] + _s,
362  this->data[2] + _s);
363  }
364 
369  public: friend inline Vector3<T> operator+(const T _s,
370  const Vector3<T> &_v)
371  {
372  return Vector3<T>(_v.X() + _s, _v.Y() + _s, _v.Z() + _s);
373  }
374 
378  public: const Vector3<T> &operator+=(const T _s)
379  {
380  this->data[0] += _s;
381  this->data[1] += _s;
382  this->data[2] += _s;
383 
384  return *this;
385  }
386 
389  public: inline Vector3 operator-() const
390  {
391  return Vector3(-this->data[0], -this->data[1], -this->data[2]);
392  }
393 
397  public: inline Vector3<T> operator-(const Vector3<T> &_pt) const
398  {
399  return Vector3(this->data[0] - _pt[0],
400  this->data[1] - _pt[1],
401  this->data[2] - _pt[2]);
402  }
403 
407  public: const Vector3<T> &operator-=(const Vector3<T> &_pt)
408  {
409  this->data[0] -= _pt[0];
410  this->data[1] -= _pt[1];
411  this->data[2] -= _pt[2];
412 
413  return *this;
414  }
415 
419  public: inline Vector3<T> operator-(const T _s) const
420  {
421  return Vector3<T>(this->data[0] - _s,
422  this->data[1] - _s,
423  this->data[2] - _s);
424  }
425 
430  public: friend inline Vector3<T> operator-(const T _s,
431  const Vector3<T> &_v)
432  {
433  return Vector3<T>(_s - _v.X(), _s - _v.Y(), _s - _v.Z());
434  }
435 
439  public: const Vector3<T> &operator-=(const T _s)
440  {
441  this->data[0] -= _s;
442  this->data[1] -= _s;
443  this->data[2] -= _s;
444 
445  return *this;
446  }
447 
452  public: const Vector3<T> operator/(const Vector3<T> &_pt) const
453  {
454  return Vector3(this->data[0] / _pt[0],
455  this->data[1] / _pt[1],
456  this->data[2] / _pt[2]);
457  }
458 
463  public: const Vector3<T> &operator/=(const Vector3<T> &_pt)
464  {
465  this->data[0] /= _pt[0];
466  this->data[1] /= _pt[1];
467  this->data[2] /= _pt[2];
468 
469  return *this;
470  }
471 
476  public: const Vector3<T> operator/(T _v) const
477  {
478  return Vector3(this->data[0] / _v,
479  this->data[1] / _v,
480  this->data[2] / _v);
481  }
482 
487  public: const Vector3<T> &operator/=(T _v)
488  {
489  this->data[0] /= _v;
490  this->data[1] /= _v;
491  this->data[2] /= _v;
492 
493  return *this;
494  }
495 
500  public: Vector3<T> operator*(const Vector3<T> &_p) const
501  {
502  return Vector3(this->data[0] * _p[0],
503  this->data[1] * _p[1],
504  this->data[2] * _p[2]);
505  }
506 
511  public: const Vector3<T> &operator*=(const Vector3<T> &_v)
512  {
513  this->data[0] *= _v[0];
514  this->data[1] *= _v[1];
515  this->data[2] *= _v[2];
516 
517  return *this;
518  }
519 
523  public: inline Vector3<T> operator*(T _s) const
524  {
525  return Vector3<T>(this->data[0] * _s,
526  this->data[1] * _s,
527  this->data[2] * _s);
528  }
529 
534  public: friend inline Vector3<T> operator*(T _s, const Vector3<T> &_v)
535  {
536  return Vector3<T>(_v.X() * _s, _v.Y() * _s, _v.Z() * _s);
537  }
538 
542  public: const Vector3<T> &operator*=(T _v)
543  {
544  this->data[0] *= _v;
545  this->data[1] *= _v;
546  this->data[2] *= _v;
547 
548  return *this;
549  }
550 
556  public: bool Equal(const Vector3 &_v, const T &_tol) const
557  {
558  return equal<T>(this->data[0], _v[0], _tol)
559  && equal<T>(this->data[1], _v[1], _tol)
560  && equal<T>(this->data[2], _v[2], _tol);
561  }
562 
567  public: bool operator==(const Vector3<T> &_v) const
568  {
569  return this->Equal(_v, static_cast<T>(1e-3));
570  }
571 
576  public: bool operator!=(const Vector3<T> &_v) const
577  {
578  return !(*this == _v);
579  }
580 
583  public: bool IsFinite() const
584  {
585  // std::isfinite works with floating point values,
586  // need to explicit cast to avoid ambiguity in vc++.
587  return std::isfinite(static_cast<double>(this->data[0])) &&
588  std::isfinite(static_cast<double>(this->data[1])) &&
589  std::isfinite(static_cast<double>(this->data[2]));
590  }
591 
593  public: inline void Correct()
594  {
595  // std::isfinite works with floating point values,
596  // need to explicit cast to avoid ambiguity in vc++.
597  if (!std::isfinite(static_cast<double>(this->data[0])))
598  this->data[0] = 0;
599  if (!std::isfinite(static_cast<double>(this->data[1])))
600  this->data[1] = 0;
601  if (!std::isfinite(static_cast<double>(this->data[2])))
602  this->data[2] = 0;
603  }
604 
610  public: T operator[](size_t _index) const
611  {
612  if (_index > 2)
613  throw IndexException();
614  return this->data[_index];
615  }
616 
619  public: void Round(int _precision)
620  {
621  this->data[0] = precision(this->data[0], _precision);
622  this->data[1] = precision(this->data[1], _precision);
623  this->data[2] = precision(this->data[2], _precision);
624  }
625 
630  public: bool Equal(const Vector3<T> &_v) const
631  {
632  return equal<T>(this->data[0], _v[0]) &&
633  equal<T>(this->data[1], _v[1]) &&
634  equal<T>(this->data[2], _v[2]);
635  }
636 
639  public: inline T X() const
640  {
641  return this->data[0];
642  }
643 
646  public: inline T Y() const
647  {
648  return this->data[1];
649  }
650 
653  public: inline T Z() const
654  {
655  return this->data[2];
656  }
657 
660  public: inline T &X()
661  {
662  return this->data[0];
663  }
664 
667  public: inline T &Y()
668  {
669  return this->data[1];
670  }
671 
674  public: inline T &Z()
675  {
676  return this->data[2];
677  }
678 
681  public: inline void X(const T &_v)
682  {
683  this->data[0] = _v;
684  }
685 
688  public: inline void Y(const T &_v)
689  {
690  this->data[1] = _v;
691  }
692 
695  public: inline void Z(const T &_v)
696  {
697  this->data[2] = _v;
698  }
699 
704  public: friend std::ostream &operator<<(
705  std::ostream &_out, const ignition::math::Vector3<T> &_pt)
706  {
707  _out << precision(_pt[0], 6) << " " << precision(_pt[1], 6) << " "
708  << precision(_pt[2], 6);
709  return _out;
710  }
711 
716  public: friend std::istream &operator>>(
717  std::istream &_in, ignition::math::Vector3<T> &_pt)
718  {
719  // Skip white spaces
720  _in.setf(std::ios_base::skipws);
721  T x, y, z;
722  _in >> x >> y >> z;
723  _pt.Set(x, y, z);
724  return _in;
725  }
726 
728  private: T data[3];
729  };
730 
731  template<typename T> const Vector3<T> Vector3<T>::Zero(0, 0, 0);
732  template<typename T> const Vector3<T> Vector3<T>::One(1, 1, 1);
733  template<typename T> const Vector3<T> Vector3<T>::UnitX(1, 0, 0);
734  template<typename T> const Vector3<T> Vector3<T>::UnitY(0, 1, 0);
735  template<typename T> const Vector3<T> Vector3<T>::UnitZ(0, 0, 1);
736 
740  }
741 }
742 #endif
static const Vector3 Zero
math::Vector3(0, 0, 0)
Definition: Vector3.hh:40
T & Y()
Get a mutable reference to the y value.
Definition: Vector3.hh:667
static const Vector3 UnitY
math::Vector3(0, 1, 0)
Definition: Vector3.hh:49
virtual ~Vector3()
Destructor.
Definition: Vector3.hh:83
T Length() const
Returns the length (magnitude) of the vector.
Definition: Vector3.hh:114
T AbsDot(const Vector3< T > &_v) const
Return the absolute dot product of this vector and another vector.
Definition: Vector3.hh:211
void Set(T _x=0, T _y=0, T _z=0)
Set the contents of the vector.
Definition: Vector3.hh:176
bool IsFinite() const
See if a point is finite (e.g., not nan)
Definition: Vector3.hh:583
T Sum() const
Return the sum of the values.
Definition: Vector3.hh:87
friend Vector3< T > operator-(const T _s, const Vector3< T > &_v)
Subtraction operators.
Definition: Vector3.hh:430
T precision(const T &_a, const unsigned int &_precision)
get value at a specified precision
Definition: Helpers.hh:363
Vector3 operator-() const
Negation operator.
Definition: Vector3.hh:389
T Distance(const Vector3< T > &_pt) const
Calc distance to the given point.
Definition: Vector3.hh:95
Vector3 Normalized() const
Return a normalized vector.
Definition: Vector3.hh:146
Vector3 operator+(const Vector3< T > &_v) const
Addition operator.
Definition: Vector3.hh:336
T max(const std::vector< T > &_values)
get the maximum value of vector of values
Definition: Helpers.hh:324
bool Equal(const Vector3< T > &_v) const
Equality test.
Definition: Vector3.hh:630
T & Z()
Get a mutable reference to the z value.
Definition: Vector3.hh:674
Vector3< double > Vector3d
Definition: Vector3.hh:738
Vector3()
Constructor.
Definition: Vector3.hh:55
const Vector3< T > & operator/=(T _v)
Division assignment operator.
Definition: Vector3.hh:487
T X() const
Get the x value.
Definition: Vector3.hh:639
void Correct()
Corrects any nan values.
Definition: Vector3.hh:593
Vector3 Abs() const
Get the absolute value of the vector.
Definition: Vector3.hh:220
void Z(const T &_v)
Set the z value.
Definition: Vector3.hh:695
Vector3 Normalize()
Normalize the vector length.
Definition: Vector3.hh:130
Vector3(const T &_x, const T &_y, const T &_z)
Constructor.
Definition: Vector3.hh:66
T Dot(const Vector3< T > &_v) const
Return the dot product of this vector and another vector.
Definition: Vector3.hh:196
bool operator==(const Vector3< T > &_v) const
Equal to operator.
Definition: Vector3.hh:567
Vector3 Perpendicular() const
Return a vector that is perpendicular to this one.
Definition: Vector3.hh:229
T operator[](size_t _index) const
Array subscript operator.
Definition: Vector3.hh:610
const Vector3< T > & operator/=(const Vector3< T > &_pt)
Division assignment operator.
Definition: Vector3.hh:463
Vector3 Cross(const Vector3< T > &_v) const
Return the cross product of this vector with another vector.
Definition: Vector3.hh:186
T & X()
Get a mutable reference to the x value.
Definition: Vector3.hh:660
T Y() const
Get the y value.
Definition: Vector3.hh:646
T Min() const
Get the minimum value in the vector.
Definition: Vector3.hh:304
void Round(int _precision)
Round all values to _precision decimal places.
Definition: Vector3.hh:619
Vector3 Rounded() const
Get a rounded version of this vector.
Definition: Vector3.hh:165
const Vector3< T > operator/(T _v) const
Division operator.
Definition: Vector3.hh:476
Vector3 & operator=(const Vector3< T > &_v)
Assignment operator.
Definition: Vector3.hh:312
static const Vector3 One
math::Vector3(1, 1, 1)
Definition: Vector3.hh:43
void Min(const Vector3< T > &_v)
Set this vector&#39;s components to the minimum of itself and the passed in vector.
Definition: Vector3.hh:285
const Vector3< T > & operator*=(const Vector3< T > &_v)
Multiplication assignment operators.
Definition: Vector3.hh:511
void Y(const T &_v)
Set the y value.
Definition: Vector3.hh:688
Vector3< T > operator+(const T _s) const
Addition operators.
Definition: Vector3.hh:358
friend Vector3< T > operator+(const T _s, const Vector3< T > &_v)
Addition operators.
Definition: Vector3.hh:369
Vector3< T > operator*(T _s) const
Multiplication operators.
Definition: Vector3.hh:523
const Vector3 & operator+=(const Vector3< T > &_v)
Addition assignment operator.
Definition: Vector3.hh:346
void Max(const Vector3< T > &_v)
Set this vector&#39;s components to the maximum of itself and the passed in vector.
Definition: Vector3.hh:272
T Z() const
Get the z value.
Definition: Vector3.hh:653
Exception that is thrown when an out-of-bounds index is encountered.
Definition: IndexException.hh:37
Vector3< float > Vector3f
Definition: Vector3.hh:739
const Vector3< T > & operator-=(const T _s)
Subtraction assignment operator.
Definition: Vector3.hh:439
T DistToLine(const Vector3< T > &_pt1, const Vector3 &_pt2)
Get distance to a line.
Definition: Vector3.hh:262
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:37
friend std::ostream & operator<<(std::ostream &_out, const ignition::math::Vector3< T > &_pt)
Stream insertion operator.
Definition: Vector3.hh:704
const Vector3< T > & operator+=(const T _s)
Addition assignment operator.
Definition: Vector3.hh:378
Vector3(const Vector3< T > &_v)
Copy constructor.
Definition: Vector3.hh:75
static Vector3 Normal(const Vector3< T > &_v1, const Vector3< T > &_v2, const Vector3< T > &_v3)
Get a normal vector to a triangle.
Definition: Vector3.hh:249
void X(const T &_v)
Set the x value.
Definition: Vector3.hh:681
T Distance(T _x, T _y, T _z) const
Calc distance to the given point.
Definition: Vector3.hh:107
bool Equal(const Vector3 &_v, const T &_tol) const
Equality test with tolerance.
Definition: Vector3.hh:556
const Vector3< T > & operator-=(const Vector3< T > &_pt)
Subtraction assignment operators.
Definition: Vector3.hh:407
Vector3< T > operator*(const Vector3< T > &_p) const
Multiplication operator.
Definition: Vector3.hh:500
T SquaredLength() const
Return the square of the length (magnitude) of the vector.
Definition: Vector3.hh:121
bool operator!=(const Vector3< T > &_v) const
Not equal to operator.
Definition: Vector3.hh:576
static const Vector3 UnitX
math::Vector3(1, 0, 0)
Definition: Vector3.hh:46
const Vector3< T > & operator*=(T _v)
Multiplication operator.
Definition: Vector3.hh:542
Vector3 Round()
Round to near whole number, return the result.
Definition: Vector3.hh:155
Definition: AffineException.hh:30
Vector3 & operator=(T _v)
Assignment operator.
Definition: Vector3.hh:324
Vector3< T > operator-(const T _s) const
Subtraction operators.
Definition: Vector3.hh:419
static const Vector3 UnitZ
math::Vector3(0, 0, 1)
Definition: Vector3.hh:52
Vector3< int > Vector3i
Definition: Vector3.hh:737
T min(const std::vector< T > &_values)
get the minimum value of vector of values
Definition: Helpers.hh:337
const Vector3< T > operator/(const Vector3< T > &_pt) const
Division operator.
Definition: Vector3.hh:452
T Max() const
Get the maximum value in the vector.
Definition: Vector3.hh:297
friend std::istream & operator>>(std::istream &_in, ignition::math::Vector3< T > &_pt)
Stream extraction operator.
Definition: Vector3.hh:716
friend Vector3< T > operator*(T _s, const Vector3< T > &_v)
Multiplication operators.
Definition: Vector3.hh:534
Vector3< T > operator-(const Vector3< T > &_pt) const
Subtraction operators.
Definition: Vector3.hh:397