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_VECTOR3_HH_
18 #define _IGNITION_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 Round()
147  {
148  this->data[0] = nearbyint(this->data[0]);
149  this->data[1] = nearbyint(this->data[1]);
150  this->data[2] = nearbyint(this->data[2]);
151  return *this;
152  }
153 
156  public: Vector3 Rounded() const
157  {
158  Vector3<T> result = *this;
159  result.Round();
160  return result;
161  }
162 
167  public: inline void Set(T _x = 0, T _y = 0, T _z = 0)
168  {
169  this->data[0] = _x;
170  this->data[1] = _y;
171  this->data[2] = _z;
172  }
173 
177  public: Vector3 Cross(const Vector3<T> &_v) const
178  {
179  return Vector3(this->data[1] * _v[2] - this->data[2] * _v[1],
180  this->data[2] * _v[0] - this->data[0] * _v[2],
181  this->data[0] * _v[1] - this->data[1] * _v[0]);
182  }
183 
187  public: T Dot(const Vector3<T> &_v) const
188  {
189  return this->data[0] * _v[0] +
190  this->data[1] * _v[1] +
191  this->data[2] * _v[2];
192  }
193 
202  public: T AbsDot(const Vector3<T> &_v) const
203  {
204  return std::abs(this->data[0] * _v[0]) +
205  std::abs(this->data[1] * _v[1]) +
206  std::abs(this->data[2] * _v[2]);
207  }
208 
211  public: Vector3 Abs() const
212  {
213  return Vector3(std::abs(this->data[0]),
214  std::abs(this->data[1]),
215  std::abs(this->data[2]));
216  }
217 
220  public: Vector3 Perpendicular() const
221  {
222  static const T sqrZero = 1e-06 * 1e-06;
223 
224  Vector3<T> perp = this->Cross(Vector3(1, 0, 0));
225 
226  // Check the length of the vector
227  if (perp.SquaredLength() < sqrZero)
228  {
229  perp = this->Cross(Vector3(0, 1, 0));
230  }
231 
232  return perp;
233  }
234 
240  public: static Vector3 Normal(const Vector3<T> &_v1,
241  const Vector3<T> &_v2, const Vector3<T> &_v3)
242  {
243  Vector3<T> a = _v2 - _v1;
244  Vector3<T> b = _v3 - _v1;
245  Vector3<T> n = a.Cross(b);
246  return n.Normalize();
247  }
248 
253  public: T DistToLine(const Vector3<T> &_pt1, const Vector3 &_pt2)
254  {
255  T d = ((*this) - _pt1).Cross((*this) - _pt2).Length();
256  d = d / (_pt2 - _pt1).Length();
257  return d;
258  }
259 
263  public: void Max(const Vector3<T> &_v)
264  {
265  if (_v[0] > this->data[0])
266  this->data[0] = _v[0];
267  if (_v[1] > this->data[1])
268  this->data[1] = _v[1];
269  if (_v[2] > this->data[2])
270  this->data[2] = _v[2];
271  }
272 
276  public: void Min(const Vector3<T> &_v)
277  {
278  if (_v[0] < this->data[0])
279  this->data[0] = _v[0];
280  if (_v[1] < this->data[1])
281  this->data[1] = _v[1];
282  if (_v[2] < this->data[2])
283  this->data[2] = _v[2];
284  }
285 
288  public: T Max() const
289  {
290  return std::max(std::max(this->data[0], this->data[1]), this->data[2]);
291  }
292 
295  public: T Min() const
296  {
297  return std::min(std::min(this->data[0], this->data[1]), this->data[2]);
298  }
299 
303  public: Vector3 &operator=(const Vector3<T> &_v)
304  {
305  this->data[0] = _v[0];
306  this->data[1] = _v[1];
307  this->data[2] = _v[2];
308 
309  return *this;
310  }
311 
315  public: Vector3 &operator=(T _v)
316  {
317  this->data[0] = _v;
318  this->data[1] = _v;
319  this->data[2] = _v;
320 
321  return *this;
322  }
323 
327  public: Vector3 operator+(const Vector3<T> &_v) const
328  {
329  return Vector3(this->data[0] + _v[0],
330  this->data[1] + _v[1],
331  this->data[2] + _v[2]);
332  }
333 
337  public: const Vector3 &operator+=(const Vector3<T> &_v)
338  {
339  this->data[0] += _v[0];
340  this->data[1] += _v[1];
341  this->data[2] += _v[2];
342 
343  return *this;
344  }
345 
349  public: inline Vector3<T> operator+(const T _s) const
350  {
351  return Vector3<T>(this->data[0] + _s,
352  this->data[1] + _s,
353  this->data[2] + _s);
354  }
355 
360  public: friend inline Vector3<T> operator+(const T _s,
361  const Vector3<T> &_v)
362  {
363  return Vector3<T>(_v.X() + _s, _v.Y() + _s, _v.Z() + _s);
364  }
365 
369  public: const Vector3<T> &operator+=(const T _s)
370  {
371  this->data[0] += _s;
372  this->data[1] += _s;
373  this->data[2] += _s;
374 
375  return *this;
376  }
377 
380  public: inline Vector3 operator-() const
381  {
382  return Vector3(-this->data[0], -this->data[1], -this->data[2]);
383  }
384 
388  public: inline Vector3<T> operator-(const Vector3<T> &_pt) const
389  {
390  return Vector3(this->data[0] - _pt[0],
391  this->data[1] - _pt[1],
392  this->data[2] - _pt[2]);
393  }
394 
398  public: const Vector3<T> &operator-=(const Vector3<T> &_pt)
399  {
400  this->data[0] -= _pt[0];
401  this->data[1] -= _pt[1];
402  this->data[2] -= _pt[2];
403 
404  return *this;
405  }
406 
410  public: inline Vector3<T> operator-(const T _s) const
411  {
412  return Vector3<T>(this->data[0] - _s,
413  this->data[1] - _s,
414  this->data[2] - _s);
415  }
416 
421  public: friend inline Vector3<T> operator-(const T _s,
422  const Vector3<T> &_v)
423  {
424  return Vector3<T>(_s - _v.X(), _s - _v.Y(), _s - _v.Z());
425  }
426 
430  public: const Vector3<T> &operator-=(const T _s)
431  {
432  this->data[0] -= _s;
433  this->data[1] -= _s;
434  this->data[2] -= _s;
435 
436  return *this;
437  }
438 
443  public: const Vector3<T> operator/(const Vector3<T> &_pt) const
444  {
445  return Vector3(this->data[0] / _pt[0],
446  this->data[1] / _pt[1],
447  this->data[2] / _pt[2]);
448  }
449 
454  public: const Vector3<T> &operator/=(const Vector3<T> &_pt)
455  {
456  this->data[0] /= _pt[0];
457  this->data[1] /= _pt[1];
458  this->data[2] /= _pt[2];
459 
460  return *this;
461  }
462 
467  public: const Vector3<T> operator/(T _v) const
468  {
469  return Vector3(this->data[0] / _v,
470  this->data[1] / _v,
471  this->data[2] / _v);
472  }
473 
478  public: const Vector3<T> &operator/=(T _v)
479  {
480  this->data[0] /= _v;
481  this->data[1] /= _v;
482  this->data[2] /= _v;
483 
484  return *this;
485  }
486 
491  public: Vector3<T> operator*(const Vector3<T> &_p) const
492  {
493  return Vector3(this->data[0] * _p[0],
494  this->data[1] * _p[1],
495  this->data[2] * _p[2]);
496  }
497 
502  public: const Vector3<T> &operator*=(const Vector3<T> &_v)
503  {
504  this->data[0] *= _v[0];
505  this->data[1] *= _v[1];
506  this->data[2] *= _v[2];
507 
508  return *this;
509  }
510 
514  public: inline Vector3<T> operator*(T _s) const
515  {
516  return Vector3<T>(this->data[0] * _s,
517  this->data[1] * _s,
518  this->data[2] * _s);
519  }
520 
525  public: friend inline Vector3<T> operator*(T _s, const Vector3<T> &_v)
526  {
527  return Vector3<T>(_v.X() * _s, _v.Y() * _s, _v.Z() * _s);
528  }
529 
533  public: const Vector3<T> &operator*=(T _v)
534  {
535  this->data[0] *= _v;
536  this->data[1] *= _v;
537  this->data[2] *= _v;
538 
539  return *this;
540  }
541 
547  public: bool Equal(const Vector3 &_v, const T &_tol) const
548  {
549  return equal<T>(this->data[0], _v[0], _tol)
550  && equal<T>(this->data[1], _v[1], _tol)
551  && equal<T>(this->data[2], _v[2], _tol);
552  }
553 
558  public: bool operator==(const Vector3<T> &_v) const
559  {
560  return this->Equal(_v, static_cast<T>(1e-3));
561  }
562 
567  public: bool operator!=(const Vector3<T> &_v) const
568  {
569  return !(*this == _v);
570  }
571 
574  public: bool IsFinite() const
575  {
576  // std::isfinite works with floating point values,
577  // need to explicit cast to avoid ambiguity in vc++.
578  return std::isfinite(static_cast<double>(this->data[0])) &&
579  std::isfinite(static_cast<double>(this->data[1])) &&
580  std::isfinite(static_cast<double>(this->data[2]));
581  }
582 
584  public: inline void Correct()
585  {
586  // std::isfinite works with floating point values,
587  // need to explicit cast to avoid ambiguity in vc++.
588  if (!std::isfinite(static_cast<double>(this->data[0])))
589  this->data[0] = 0;
590  if (!std::isfinite(static_cast<double>(this->data[1])))
591  this->data[1] = 0;
592  if (!std::isfinite(static_cast<double>(this->data[2])))
593  this->data[2] = 0;
594  }
595 
601  public: T operator[](size_t _index) const
602  {
603  if (_index > 2)
604  throw IndexException();
605  return this->data[_index];
606  }
607 
610  public: void Round(int _precision)
611  {
612  this->data[0] = precision(this->data[0], _precision);
613  this->data[1] = precision(this->data[1], _precision);
614  this->data[2] = precision(this->data[2], _precision);
615  }
616 
621  public: bool Equal(const Vector3<T> &_v) const
622  {
623  return equal<T>(this->data[0], _v[0]) &&
624  equal<T>(this->data[1], _v[1]) &&
625  equal<T>(this->data[2], _v[2]);
626  }
627 
630  public: inline T X() const
631  {
632  return this->data[0];
633  }
634 
637  public: inline T Y() const
638  {
639  return this->data[1];
640  }
641 
644  public: inline T Z() const
645  {
646  return this->data[2];
647  }
648 
651  public: inline T &X()
652  {
653  return this->data[0];
654  }
655 
658  public: inline T &Y()
659  {
660  return this->data[1];
661  }
662 
665  public: inline T &Z()
666  {
667  return this->data[2];
668  }
669 
672  public: inline void X(const T &_v)
673  {
674  this->data[0] = _v;
675  }
676 
679  public: inline void Y(const T &_v)
680  {
681  this->data[1] = _v;
682  }
683 
686  public: inline void Z(const T &_v)
687  {
688  this->data[2] = _v;
689  }
690 
695  public: bool operator<(const Vector3<T> &_pt) const
696  {
697  return this->data[0] < _pt[0] || this->data[1] < _pt[1] ||
698  this->data[2] < _pt[2];
699  }
700 
705  public: friend std::ostream &operator<<(
706  std::ostream &_out, const ignition::math::Vector3<T> &_pt)
707  {
708  _out << precision(_pt[0], 6) << " " << precision(_pt[1], 6) << " "
709  << precision(_pt[2], 6);
710  return _out;
711  }
712 
717  public: friend std::istream &operator>>(
718  std::istream &_in, ignition::math::Vector3<T> &_pt)
719  {
720  // Skip white spaces
721  _in.setf(std::ios_base::skipws);
722  T x, y, z;
723  _in >> x >> y >> z;
724  _pt.Set(x, y, z);
725  return _in;
726  }
727 
729  private: T data[3];
730  };
731 
732  template<typename T> const Vector3<T> Vector3<T>::Zero(0, 0, 0);
733  template<typename T> const Vector3<T> Vector3<T>::One(1, 1, 1);
734  template<typename T> const Vector3<T> Vector3<T>::UnitX(1, 0, 0);
735  template<typename T> const Vector3<T> Vector3<T>::UnitY(0, 1, 0);
736  template<typename T> const Vector3<T> Vector3<T>::UnitZ(0, 0, 1);
737 
741  }
742 }
743 #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:658
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:202
void Set(T _x=0, T _y=0, T _z=0)
Set the contents of the vector.
Definition: Vector3.hh:167
bool IsFinite() const
See if a point is finite (e.g., not nan)
Definition: Vector3.hh:574
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:421
T precision(const T &_a, const unsigned int &_precision)
get value at a specified precision
Definition: Helpers.hh:269
Vector3 operator-() const
Negation operator.
Definition: Vector3.hh:380
T Distance(const Vector3< T > &_pt) const
Calc distance to the given point.
Definition: Vector3.hh:95
Vector3 operator+(const Vector3< T > &_v) const
Addition operator.
Definition: Vector3.hh:327
T max(const std::vector< T > &_values)
get the maximum value of vector of values
Definition: Helpers.hh:230
bool Equal(const Vector3< T > &_v) const
Equality test.
Definition: Vector3.hh:621
T & Z()
Get a mutable reference to the z value.
Definition: Vector3.hh:665
Vector3< double > Vector3d
Definition: Vector3.hh:739
Vector3()
Constructor.
Definition: Vector3.hh:55
const Vector3< T > & operator/=(T _v)
Division assignment operator.
Definition: Vector3.hh:478
T X() const
Get the x value.
Definition: Vector3.hh:630
void Correct()
Corrects any nan values.
Definition: Vector3.hh:584
Vector3 Abs() const
Get the absolute value of the vector.
Definition: Vector3.hh:211
void Z(const T &_v)
Set the z value.
Definition: Vector3.hh:686
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:187
bool operator==(const Vector3< T > &_v) const
Equal to operator.
Definition: Vector3.hh:558
Vector3 Perpendicular() const
Return a vector that is perpendicular to this one.
Definition: Vector3.hh:220
T operator[](size_t _index) const
Array subscript operator.
Definition: Vector3.hh:601
const Vector3< T > & operator/=(const Vector3< T > &_pt)
Division assignment operator.
Definition: Vector3.hh:454
Vector3 Cross(const Vector3< T > &_v) const
Return the cross product of this vector with another vector.
Definition: Vector3.hh:177
T & X()
Get a mutable reference to the x value.
Definition: Vector3.hh:651
T Y() const
Get the y value.
Definition: Vector3.hh:637
T Min() const
Get the minimum value in the vector.
Definition: Vector3.hh:295
void Round(int _precision)
Round all values to _precision decimal places.
Definition: Vector3.hh:610
Vector3 Rounded() const
Get a rounded version of this vector.
Definition: Vector3.hh:156
const Vector3< T > operator/(T _v) const
Division operator.
Definition: Vector3.hh:467
Vector3 & operator=(const Vector3< T > &_v)
Assignment operator.
Definition: Vector3.hh:303
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:276
const Vector3< T > & operator*=(const Vector3< T > &_v)
Multiplication assignment operators.
Definition: Vector3.hh:502
void Y(const T &_v)
Set the y value.
Definition: Vector3.hh:679
Vector3< T > operator+(const T _s) const
Addition operators.
Definition: Vector3.hh:349
friend Vector3< T > operator+(const T _s, const Vector3< T > &_v)
Addition operators.
Definition: Vector3.hh:360
Vector3< T > operator*(T _s) const
Multiplication operators.
Definition: Vector3.hh:514
const Vector3 & operator+=(const Vector3< T > &_v)
Addition assignment operator.
Definition: Vector3.hh:337
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:263
T Z() const
Get the z value.
Definition: Vector3.hh:644
Exception that is thrown when an out-of-bounds index is encountered.
Definition: IndexException.hh:37
Vector3< float > Vector3f
Definition: Vector3.hh:740
const Vector3< T > & operator-=(const T _s)
Subtraction assignment operator.
Definition: Vector3.hh:430
T DistToLine(const Vector3< T > &_pt1, const Vector3 &_pt2)
Get distance to a line.
Definition: Vector3.hh:253
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:705
const Vector3< T > & operator+=(const T _s)
Addition assignment operator.
Definition: Vector3.hh:369
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:240
void X(const T &_v)
Set the x value.
Definition: Vector3.hh:672
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:547
const Vector3< T > & operator-=(const Vector3< T > &_pt)
Subtraction assignment operators.
Definition: Vector3.hh:398
Vector3< T > operator*(const Vector3< T > &_p) const
Multiplication operator.
Definition: Vector3.hh:491
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:567
static const Vector3 UnitX
math::Vector3(1, 0, 0)
Definition: Vector3.hh:46
const Vector3< T > & operator*=(T _v)
Multiplication operator.
Definition: Vector3.hh:533
Vector3 Round()
Round to near whole number, return the result.
Definition: Vector3.hh:146
Definition: AffineException.hh:30
Vector3 & operator=(T _v)
Assignment operator.
Definition: Vector3.hh:315
Vector3< T > operator-(const T _s) const
Subtraction operators.
Definition: Vector3.hh:410
static const Vector3 UnitZ
math::Vector3(0, 0, 1)
Definition: Vector3.hh:52
Vector3< int > Vector3i
Definition: Vector3.hh:738
T min(const std::vector< T > &_values)
get the minimum value of vector of values
Definition: Helpers.hh:243
const Vector3< T > operator/(const Vector3< T > &_pt) const
Division operator.
Definition: Vector3.hh:443
T Max() const
Get the maximum value in the vector.
Definition: Vector3.hh:288
friend std::istream & operator>>(std::istream &_in, ignition::math::Vector3< T > &_pt)
Stream extraction operator.
Definition: Vector3.hh:717
friend Vector3< T > operator*(T _s, const Vector3< T > &_v)
Multiplication operators.
Definition: Vector3.hh:525
Vector3< T > operator-(const Vector3< T > &_pt) const
Subtraction operators.
Definition: Vector3.hh:388