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>
26 
27 namespace ignition
28 {
29  namespace math
30  {
35  template<typename T>
36  class Vector3
37  {
39  public: static const Vector3 Zero;
40 
42  public: static const Vector3 One;
43 
45  public: static const Vector3 UnitX;
46 
48  public: static const Vector3 UnitY;
49 
51  public: static const Vector3 UnitZ;
52 
54  public: Vector3()
55  {
56  this->data[0] = 0;
57  this->data[1] = 0;
58  this->data[2] = 0;
59  }
60 
65  public: Vector3(const T &_x, const T &_y, const T &_z)
66  {
67  this->data[0] = _x;
68  this->data[1] = _y;
69  this->data[2] = _z;
70  }
71 
74  public: Vector3(const Vector3<T> &_v)
75  {
76  this->data[0] = _v[0];
77  this->data[1] = _v[1];
78  this->data[2] = _v[2];
79  }
80 
82  public: virtual ~Vector3() {}
83 
86  public: T Sum() const
87  {
88  return this->data[0] + this->data[1] + this->data[2];
89  }
90 
94  public: T Distance(const Vector3<T> &_pt) const
95  {
96  return sqrt((this->data[0]-_pt[0])*(this->data[0]-_pt[0]) +
97  (this->data[1]-_pt[1])*(this->data[1]-_pt[1]) +
98  (this->data[2]-_pt[2])*(this->data[2]-_pt[2]));
99  }
100 
106  public: T Distance(T _x, T _y, T _z) const
107  {
108  return this->Distance(Vector3(_x, _y, _z));
109  }
110 
113  public: T Length() const
114  {
115  return sqrt(this->SquaredLength());
116  }
117 
120  public: T SquaredLength() const
121  {
122  return std::pow(this->data[0], 2)
123  + std::pow(this->data[1], 2)
124  + std::pow(this->data[2], 2);
125  }
126 
129  public: Vector3 Normalize()
130  {
131  T d = this->Length();
132 
133  if (!equal<T>(d, static_cast<T>(0.0)))
134  {
135  this->data[0] /= d;
136  this->data[1] /= d;
137  this->data[2] /= d;
138  }
139 
140  return *this;
141  }
142 
145  public: Vector3 Normalized() const
146  {
147  Vector3<T> result = *this;
148  result.Normalize();
149  return result;
150  }
151 
154  public: Vector3 Round()
155  {
156  this->data[0] = nearbyint(this->data[0]);
157  this->data[1] = nearbyint(this->data[1]);
158  this->data[2] = nearbyint(this->data[2]);
159  return *this;
160  }
161 
164  public: Vector3 Rounded() const
165  {
166  Vector3<T> result = *this;
167  result.Round();
168  return result;
169  }
170 
175  public: inline void Set(T _x = 0, T _y = 0, T _z = 0)
176  {
177  this->data[0] = _x;
178  this->data[1] = _y;
179  this->data[2] = _z;
180  }
181 
185  public: Vector3 Cross(const Vector3<T> &_v) const
186  {
187  return Vector3(this->data[1] * _v[2] - this->data[2] * _v[1],
188  this->data[2] * _v[0] - this->data[0] * _v[2],
189  this->data[0] * _v[1] - this->data[1] * _v[0]);
190  }
191 
195  public: T Dot(const Vector3<T> &_v) const
196  {
197  return this->data[0] * _v[0] +
198  this->data[1] * _v[1] +
199  this->data[2] * _v[2];
200  }
201 
210  public: T AbsDot(const Vector3<T> &_v) const
211  {
212  return std::abs(this->data[0] * _v[0]) +
213  std::abs(this->data[1] * _v[1]) +
214  std::abs(this->data[2] * _v[2]);
215  }
216 
219  public: Vector3 Abs() const
220  {
221  return Vector3(std::abs(this->data[0]),
222  std::abs(this->data[1]),
223  std::abs(this->data[2]));
224  }
225 
228  public: Vector3 Perpendicular() const
229  {
230  static const T sqrZero = 1e-06 * 1e-06;
231 
232  Vector3<T> perp = this->Cross(Vector3(1, 0, 0));
233 
234  // Check the length of the vector
235  if (perp.SquaredLength() < sqrZero)
236  {
237  perp = this->Cross(Vector3(0, 1, 0));
238  }
239 
240  return perp;
241  }
242 
248  public: static Vector3 Normal(const Vector3<T> &_v1,
249  const Vector3<T> &_v2, const Vector3<T> &_v3)
250  {
251  Vector3<T> a = _v2 - _v1;
252  Vector3<T> b = _v3 - _v1;
253  Vector3<T> n = a.Cross(b);
254  return n.Normalize();
255  }
256 
261  public: T DistToLine(const Vector3<T> &_pt1, const Vector3 &_pt2)
262  {
263  T d = ((*this) - _pt1).Cross((*this) - _pt2).Length();
264  d = d / (_pt2 - _pt1).Length();
265  return d;
266  }
267 
271  public: void Max(const Vector3<T> &_v)
272  {
273  if (_v[0] > this->data[0])
274  this->data[0] = _v[0];
275  if (_v[1] > this->data[1])
276  this->data[1] = _v[1];
277  if (_v[2] > this->data[2])
278  this->data[2] = _v[2];
279  }
280 
284  public: void Min(const Vector3<T> &_v)
285  {
286  if (_v[0] < this->data[0])
287  this->data[0] = _v[0];
288  if (_v[1] < this->data[1])
289  this->data[1] = _v[1];
290  if (_v[2] < this->data[2])
291  this->data[2] = _v[2];
292  }
293 
296  public: T Max() const
297  {
298  return std::max(std::max(this->data[0], this->data[1]), this->data[2]);
299  }
300 
303  public: T Min() const
304  {
305  return std::min(std::min(this->data[0], this->data[1]), this->data[2]);
306  }
307 
311  public: Vector3 &operator=(const Vector3<T> &_v)
312  {
313  this->data[0] = _v[0];
314  this->data[1] = _v[1];
315  this->data[2] = _v[2];
316 
317  return *this;
318  }
319 
323  public: Vector3 &operator=(T _v)
324  {
325  this->data[0] = _v;
326  this->data[1] = _v;
327  this->data[2] = _v;
328 
329  return *this;
330  }
331 
335  public: Vector3 operator+(const Vector3<T> &_v) const
336  {
337  return Vector3(this->data[0] + _v[0],
338  this->data[1] + _v[1],
339  this->data[2] + _v[2]);
340  }
341 
345  public: const Vector3 &operator+=(const Vector3<T> &_v)
346  {
347  this->data[0] += _v[0];
348  this->data[1] += _v[1];
349  this->data[2] += _v[2];
350 
351  return *this;
352  }
353 
357  public: inline Vector3<T> operator+(const T _s) const
358  {
359  return Vector3<T>(this->data[0] + _s,
360  this->data[1] + _s,
361  this->data[2] + _s);
362  }
363 
368  public: friend inline Vector3<T> operator+(const T _s,
369  const Vector3<T> &_v)
370  {
371  return {_v.X() + _s, _v.Y() + _s, _v.Z() + _s};
372  }
373 
377  public: const Vector3<T> &operator+=(const T _s)
378  {
379  this->data[0] += _s;
380  this->data[1] += _s;
381  this->data[2] += _s;
382 
383  return *this;
384  }
385 
388  public: inline Vector3 operator-() const
389  {
390  return Vector3(-this->data[0], -this->data[1], -this->data[2]);
391  }
392 
396  public: inline Vector3<T> operator-(const Vector3<T> &_pt) const
397  {
398  return Vector3(this->data[0] - _pt[0],
399  this->data[1] - _pt[1],
400  this->data[2] - _pt[2]);
401  }
402 
406  public: const Vector3<T> &operator-=(const Vector3<T> &_pt)
407  {
408  this->data[0] -= _pt[0];
409  this->data[1] -= _pt[1];
410  this->data[2] -= _pt[2];
411 
412  return *this;
413  }
414 
418  public: inline Vector3<T> operator-(const T _s) const
419  {
420  return Vector3<T>(this->data[0] - _s,
421  this->data[1] - _s,
422  this->data[2] - _s);
423  }
424 
429  public: friend inline Vector3<T> operator-(const T _s,
430  const Vector3<T> &_v)
431  {
432  return {_s - _v.X(), _s - _v.Y(), _s - _v.Z()};
433  }
434 
438  public: const Vector3<T> &operator-=(const T _s)
439  {
440  this->data[0] -= _s;
441  this->data[1] -= _s;
442  this->data[2] -= _s;
443 
444  return *this;
445  }
446 
451  public: const Vector3<T> operator/(const Vector3<T> &_pt) const
452  {
453  return Vector3(this->data[0] / _pt[0],
454  this->data[1] / _pt[1],
455  this->data[2] / _pt[2]);
456  }
457 
462  public: const Vector3<T> &operator/=(const Vector3<T> &_pt)
463  {
464  this->data[0] /= _pt[0];
465  this->data[1] /= _pt[1];
466  this->data[2] /= _pt[2];
467 
468  return *this;
469  }
470 
475  public: const Vector3<T> operator/(T _v) const
476  {
477  return Vector3(this->data[0] / _v,
478  this->data[1] / _v,
479  this->data[2] / _v);
480  }
481 
486  public: const Vector3<T> &operator/=(T _v)
487  {
488  this->data[0] /= _v;
489  this->data[1] /= _v;
490  this->data[2] /= _v;
491 
492  return *this;
493  }
494 
499  public: Vector3<T> operator*(const Vector3<T> &_p) const
500  {
501  return Vector3(this->data[0] * _p[0],
502  this->data[1] * _p[1],
503  this->data[2] * _p[2]);
504  }
505 
510  public: const Vector3<T> &operator*=(const Vector3<T> &_v)
511  {
512  this->data[0] *= _v[0];
513  this->data[1] *= _v[1];
514  this->data[2] *= _v[2];
515 
516  return *this;
517  }
518 
522  public: inline Vector3<T> operator*(T _s) const
523  {
524  return Vector3<T>(this->data[0] * _s,
525  this->data[1] * _s,
526  this->data[2] * _s);
527  }
528 
533  public: friend inline Vector3<T> operator*(T _s, const Vector3<T> &_v)
534  {
535  return {_v.X() * _s, _v.Y() * _s, _v.Z() * _s};
536  }
537 
541  public: const Vector3<T> &operator*=(T _v)
542  {
543  this->data[0] *= _v;
544  this->data[1] *= _v;
545  this->data[2] *= _v;
546 
547  return *this;
548  }
549 
555  public: bool Equal(const Vector3 &_v, const T &_tol) const
556  {
557  return equal<T>(this->data[0], _v[0], _tol)
558  && equal<T>(this->data[1], _v[1], _tol)
559  && equal<T>(this->data[2], _v[2], _tol);
560  }
561 
566  public: bool operator==(const Vector3<T> &_v) const
567  {
568  return this->Equal(_v, static_cast<T>(1e-3));
569  }
570 
575  public: bool operator!=(const Vector3<T> &_v) const
576  {
577  return !(*this == _v);
578  }
579 
582  public: bool IsFinite() const
583  {
584  // std::isfinite works with floating point values,
585  // need to explicit cast to avoid ambiguity in vc++.
586  return std::isfinite(static_cast<double>(this->data[0])) &&
587  std::isfinite(static_cast<double>(this->data[1])) &&
588  std::isfinite(static_cast<double>(this->data[2]));
589  }
590 
592  public: inline void Correct()
593  {
594  // std::isfinite works with floating point values,
595  // need to explicit cast to avoid ambiguity in vc++.
596  if (!std::isfinite(static_cast<double>(this->data[0])))
597  this->data[0] = 0;
598  if (!std::isfinite(static_cast<double>(this->data[1])))
599  this->data[1] = 0;
600  if (!std::isfinite(static_cast<double>(this->data[2])))
601  this->data[2] = 0;
602  }
603 
608  public: T operator[](const size_t _index) const
609  {
610  return this->data[clamp(_index, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)];
611  }
612 
615  public: void Round(int _precision)
616  {
617  this->data[0] = precision(this->data[0], _precision);
618  this->data[1] = precision(this->data[1], _precision);
619  this->data[2] = precision(this->data[2], _precision);
620  }
621 
626  public: bool Equal(const Vector3<T> &_v) const
627  {
628  return equal<T>(this->data[0], _v[0]) &&
629  equal<T>(this->data[1], _v[1]) &&
630  equal<T>(this->data[2], _v[2]);
631  }
632 
635  public: inline T X() const
636  {
637  return this->data[0];
638  }
639 
642  public: inline T Y() const
643  {
644  return this->data[1];
645  }
646 
649  public: inline T Z() const
650  {
651  return this->data[2];
652  }
653 
656  public: inline T &X()
657  {
658  return this->data[0];
659  }
660 
663  public: inline T &Y()
664  {
665  return this->data[1];
666  }
667 
670  public: inline T &Z()
671  {
672  return this->data[2];
673  }
674 
677  public: inline void X(const T &_v)
678  {
679  this->data[0] = _v;
680  }
681 
684  public: inline void Y(const T &_v)
685  {
686  this->data[1] = _v;
687  }
688 
691  public: inline void Z(const T &_v)
692  {
693  this->data[2] = _v;
694  }
695 
700  public: bool operator<(const Vector3<T> &_pt) const
701  {
702  return this->data[0] < _pt[0] || this->data[1] < _pt[1] ||
703  this->data[2] < _pt[2];
704  }
705 
710  public: friend std::ostream &operator<<(
711  std::ostream &_out, const ignition::math::Vector3<T> &_pt)
712  {
713  _out << precision(_pt[0], 6) << " " << precision(_pt[1], 6) << " "
714  << precision(_pt[2], 6);
715  return _out;
716  }
717 
722  public: friend std::istream &operator>>(
723  std::istream &_in, ignition::math::Vector3<T> &_pt)
724  {
725  // Skip white spaces
726  _in.setf(std::ios_base::skipws);
727  T x, y, z;
728  _in >> x >> y >> z;
729  _pt.Set(x, y, z);
730  return _in;
731  }
732 
734  private: T data[3];
735  };
736 
737  template<typename T> const Vector3<T> Vector3<T>::Zero(0, 0, 0);
738  template<typename T> const Vector3<T> Vector3<T>::One(1, 1, 1);
739  template<typename T> const Vector3<T> Vector3<T>::UnitX(1, 0, 0);
740  template<typename T> const Vector3<T> Vector3<T>::UnitY(0, 1, 0);
741  template<typename T> const Vector3<T> Vector3<T>::UnitZ(0, 0, 1);
742 
746  }
747 }
748 #endif
Vector3 Rounded() const
Get a rounded version of this vector.
Definition: Vector3.hh:164
static const Vector3 Zero
math::Vector3(0, 0, 0)
Definition: Vector3.hh:39
T X() const
Get the x value.
Definition: Vector3.hh:635
T & Y()
Get a mutable reference to the y value.
Definition: Vector3.hh:663
static const Vector3 UnitY
math::Vector3(0, 1, 0)
Definition: Vector3.hh:48
virtual ~Vector3()
Destructor.
Definition: Vector3.hh:82
bool Equal(const Vector3 &_v, const T &_tol) const
Equality test with tolerance.
Definition: Vector3.hh:555
void Set(T _x=0, T _y=0, T _z=0)
Set the contents of the vector.
Definition: Vector3.hh:175
T Length() const
Returns the length (magnitude) of the vector.
Definition: Vector3.hh:113
Vector3< T > operator-(const T _s) const
Subtraction operators.
Definition: Vector3.hh:418
friend Vector3< T > operator-(const T _s, const Vector3< T > &_v)
Subtraction operators.
Definition: Vector3.hh:429
T precision(const T &_a, const unsigned int &_precision)
get value at a specified precision
Definition: Helpers.hh:576
bool operator==(const Vector3< T > &_v) const
Equal to operator.
Definition: Vector3.hh:566
static const size_t IGN_ZERO_SIZE_T
size_t type with a value of 0
Definition: Helpers.hh:213
T Max() const
Get the maximum value in the vector.
Definition: Vector3.hh:296
T max(const std::vector< T > &_values)
get the maximum value of vector of values
Definition: Helpers.hh:515
Vector3 operator-() const
Negation operator.
Definition: Vector3.hh:388
T & Z()
Get a mutable reference to the z value.
Definition: Vector3.hh:670
Vector3< double > Vector3d
Definition: Vector3.hh:744
Vector3 Abs() const
Get the absolute value of the vector.
Definition: Vector3.hh:219
Vector3()
Constructor.
Definition: Vector3.hh:54
const Vector3< T > operator/(const Vector3< T > &_pt) const
Division operator.
Definition: Vector3.hh:451
Vector3 Normalized() const
Return a normalized vector.
Definition: Vector3.hh:145
const Vector3< T > & operator/=(T _v)
Division assignment operator.
Definition: Vector3.hh:486
void Correct()
Corrects any nan values.
Definition: Vector3.hh:592
bool IsFinite() const
See if a point is finite (e.g., not nan)
Definition: Vector3.hh:582
void Z(const T &_v)
Set the z value.
Definition: Vector3.hh:691
Vector3 Normalize()
Normalize the vector length.
Definition: Vector3.hh:129
Vector3(const T &_x, const T &_y, const T &_z)
Constructor.
Definition: Vector3.hh:65
const Vector3< T > & operator/=(const Vector3< T > &_pt)
Division assignment operator.
Definition: Vector3.hh:462
T Dot(const Vector3< T > &_v) const
Return the dot product of this vector and another vector.
Definition: Vector3.hh:195
T & X()
Get a mutable reference to the x value.
Definition: Vector3.hh:656
T SquaredLength() const
Return the square of the length (magnitude) of the vector.
Definition: Vector3.hh:120
void Round(int _precision)
Round all values to _precision decimal places.
Definition: Vector3.hh:615
bool Equal(const Vector3< T > &_v) const
Equality test.
Definition: Vector3.hh:626
Vector3 & operator=(const Vector3< T > &_v)
Assignment operator.
Definition: Vector3.hh:311
static const Vector3 One
math::Vector3(1, 1, 1)
Definition: Vector3.hh:42
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:284
const Vector3< T > & operator*=(const Vector3< T > &_v)
Multiplication assignment operators.
Definition: Vector3.hh:510
void Y(const T &_v)
Set the y value.
Definition: Vector3.hh:684
friend Vector3< T > operator+(const T _s, const Vector3< T > &_v)
Addition operators.
Definition: Vector3.hh:368
T Y() const
Get the y value.
Definition: Vector3.hh:642
T Min() const
Get the minimum value in the vector.
Definition: Vector3.hh:303
const Vector3 & operator+=(const Vector3< T > &_v)
Addition assignment operator.
Definition: Vector3.hh:345
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:271
T operator[](const size_t _index) const
Array subscript operator.
Definition: Vector3.hh:608
Vector3< float > Vector3f
Definition: Vector3.hh:745
T Distance(T _x, T _y, T _z) const
Calc distance to the given point.
Definition: Vector3.hh:106
const Vector3< T > & operator-=(const T _s)
Subtraction assignment operator.
Definition: Vector3.hh:438
static const size_t IGN_TWO_SIZE_T
size_t type with a value of 2
Definition: Helpers.hh:219
Vector3< T > operator-(const Vector3< T > &_pt) const
Subtraction operators.
Definition: Vector3.hh:396
T DistToLine(const Vector3< T > &_pt1, const Vector3 &_pt2)
Get distance to a line.
Definition: Vector3.hh:261
bool operator!=(const Vector3< T > &_v) const
Not equal to operator.
Definition: Vector3.hh:575
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:36
friend std::ostream & operator<<(std::ostream &_out, const ignition::math::Vector3< T > &_pt)
Stream insertion operator.
Definition: Vector3.hh:710
const Vector3< T > & operator+=(const T _s)
Addition assignment operator.
Definition: Vector3.hh:377
Vector3(const Vector3< T > &_v)
Copy constructor.
Definition: Vector3.hh:74
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:248
void X(const T &_v)
Set the x value.
Definition: Vector3.hh:677
T AbsDot(const Vector3< T > &_v) const
Return the absolute dot product of this vector and another vector.
Definition: Vector3.hh:210
T Z() const
Get the z value.
Definition: Vector3.hh:649
Vector3 Cross(const Vector3< T > &_v) const
Return the cross product of this vector with another vector.
Definition: Vector3.hh:185
const Vector3< T > & operator-=(const Vector3< T > &_pt)
Subtraction assignment operators.
Definition: Vector3.hh:406
Vector3< T > operator*(const Vector3< T > &_p) const
Multiplication operator.
Definition: Vector3.hh:499
Vector3< T > operator*(T _s) const
Multiplication operators.
Definition: Vector3.hh:522
static const Vector3 UnitX
math::Vector3(1, 0, 0)
Definition: Vector3.hh:45
Vector3 operator+(const Vector3< T > &_v) const
Addition operator.
Definition: Vector3.hh:335
const Vector3< T > & operator*=(T _v)
Multiplication operator.
Definition: Vector3.hh:541
T Distance(const Vector3< T > &_pt) const
Calc distance to the given point.
Definition: Vector3.hh:94
Vector3 Round()
Round to near whole number, return the result.
Definition: Vector3.hh:154
Definition: Angle.hh:38
Vector3 & operator=(T _v)
Assignment operator.
Definition: Vector3.hh:323
const Vector3< T > operator/(T _v) const
Division operator.
Definition: Vector3.hh:475
static const Vector3 UnitZ
math::Vector3(0, 0, 1)
Definition: Vector3.hh:51
Vector3< int > Vector3i
Definition: Vector3.hh:743
T min(const std::vector< T > &_values)
get the minimum value of vector of values
Definition: Helpers.hh:528
Vector3< T > operator+(const T _s) const
Addition operators.
Definition: Vector3.hh:357
T clamp(T _v, T _min, T _max)
Simple clamping function.
Definition: Helpers.hh:392
Vector3 Perpendicular() const
Return a vector that is perpendicular to this one.
Definition: Vector3.hh:228
T Sum() const
Return the sum of the values.
Definition: Vector3.hh:86
friend std::istream & operator>>(std::istream &_in, ignition::math::Vector3< T > &_pt)
Stream extraction operator.
Definition: Vector3.hh:722
friend Vector3< T > operator*(T _s, const Vector3< T > &_v)
Multiplication operators.
Definition: Vector3.hh:533