All Classes Namespaces Files Functions Variables Typedefs Friends Macros Pages
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->data[0] * this->data[0] +
117  this->data[1] * this->data[1] +
118  this->data[2] * this->data[2]);
119  }
120 
123  public: T SquaredLength() const
124  {
125  return this->data[0] * this->data[0] +
126  this->data[1] * this->data[1] +
127  this->data[2] * this->data[2];
128  }
129 
132  public: Vector3 Normalize()
133  {
134  T d = sqrt(this->data[0] * this->data[0] +
135  this->data[1] * this->data[1] +
136  this->data[2] * this->data[2]);
137 
138  if (!equal<T>(d, static_cast<T>(0.0)))
139  {
140  this->data[0] /= d;
141  this->data[1] /= d;
142  this->data[2] /= d;
143  }
144 
145  return *this;
146  }
147 
150  public: Vector3 Round()
151  {
152  this->data[0] = nearbyint(this->data[0]);
153  this->data[1] = nearbyint(this->data[1]);
154  this->data[2] = nearbyint(this->data[2]);
155  return *this;
156  }
157 
160  public: Vector3 Rounded() const
161  {
162  Vector3<T> result = *this;
163  result.Round();
164  return result;
165  }
166 
171  public: inline void Set(T _x = 0, T _y = 0, T _z = 0)
172  {
173  this->data[0] = _x;
174  this->data[1] = _y;
175  this->data[2] = _z;
176  }
177 
181  public: Vector3 Cross(const Vector3<T> &_v) const
182  {
183  return Vector3(this->data[1] * _v[2] - this->data[2] * _v[1],
184  this->data[2] * _v[0] - this->data[0] * _v[2],
185  this->data[0] * _v[1] - this->data[1] * _v[0]);
186  }
187 
191  public: T Dot(const Vector3<T> &_v) const
192  {
193  return this->data[0] * _v[0] +
194  this->data[1] * _v[1] +
195  this->data[2] * _v[2];
196  }
197 
200  public: Vector3 Abs() const
201  {
202  return Vector3(std::abs(this->data[0]),
203  std::abs(this->data[1]),
204  std::abs(this->data[2]));
205  }
206 
209  public: Vector3 Perpendicular() const
210  {
211  static const T sqrZero = 1e-06 * 1e-06;
212 
213  Vector3<T> perp = this->Cross(Vector3(1, 0, 0));
214 
215  // Check the length of the vector
216  if (perp.SquaredLength() < sqrZero)
217  {
218  perp = this->Cross(Vector3(0, 1, 0));
219  }
220 
221  return perp;
222  }
223 
229  public: static Vector3 Normal(const Vector3<T> &_v1,
230  const Vector3<T> &_v2, const Vector3<T> &_v3)
231  {
232  Vector3<T> a = _v2 - _v1;
233  Vector3<T> b = _v3 - _v1;
234  Vector3<T> n = a.Cross(b);
235  return n;
236  }
237 
242  public: T DistToLine(const Vector3<T> &_pt1, const Vector3 &_pt2)
243  {
244  T d = ((*this) - _pt1).Cross((*this) - _pt2).Length();
245  d = d / (_pt2 - _pt1).Length();
246  return d;
247  }
248 
252  public: void Max(const Vector3<T> &_v)
253  {
254  if (_v[0] > this->data[0])
255  this->data[0] = _v[0];
256  if (_v[1] > this->data[1])
257  this->data[1] = _v[1];
258  if (_v[2] > this->data[2])
259  this->data[2] = _v[2];
260  }
261 
265  public: void Min(const Vector3<T> &_v)
266  {
267  if (_v[0] < this->data[0])
268  this->data[0] = _v[0];
269  if (_v[1] < this->data[1])
270  this->data[1] = _v[1];
271  if (_v[2] < this->data[2])
272  this->data[2] = _v[2];
273  }
274 
277  public: T Max() const
278  {
279  return std::max(std::max(this->data[0], this->data[1]), this->data[2]);
280  }
281 
284  public: T Min() const
285  {
286  return std::min(std::min(this->data[0], this->data[1]), this->data[2]);
287  }
288 
292  public: Vector3 &operator=(const Vector3<T> &_v)
293  {
294  this->data[0] = _v[0];
295  this->data[1] = _v[1];
296  this->data[2] = _v[2];
297 
298  return *this;
299  }
300 
304  public: Vector3 &operator=(T _v)
305  {
306  this->data[0] = _v;
307  this->data[1] = _v;
308  this->data[2] = _v;
309 
310  return *this;
311  }
312 
316  public: Vector3 operator+(const Vector3<T> &_v) const
317  {
318  return Vector3(this->data[0] + _v[0],
319  this->data[1] + _v[1],
320  this->data[2] + _v[2]);
321  }
322 
326  public: const Vector3 &operator+=(const Vector3<T> &_v)
327  {
328  this->data[0] += _v[0];
329  this->data[1] += _v[1];
330  this->data[2] += _v[2];
331 
332  return *this;
333  }
334 
337  public: inline Vector3 operator-() const
338  {
339  return Vector3(-this->data[0], -this->data[1], -this->data[2]);
340  }
341 
345  public: inline Vector3<T> operator-(const Vector3<T> &_pt) const
346  {
347  return Vector3(this->data[0] - _pt[0],
348  this->data[1] - _pt[1],
349  this->data[2] - _pt[2]);
350  }
351 
355  public: const Vector3<T> &operator-=(const Vector3<T> &_pt)
356  {
357  this->data[0] -= _pt[0];
358  this->data[1] -= _pt[1];
359  this->data[2] -= _pt[2];
360 
361  return *this;
362  }
363 
368  public: const Vector3<T> operator/(const Vector3<T> &_pt) const
369  {
370  return Vector3(this->data[0] / _pt[0],
371  this->data[1] / _pt[1],
372  this->data[2] / _pt[2]);
373  }
374 
379  public: const Vector3<T> &operator/=(const Vector3<T> &_pt)
380  {
381  this->data[0] /= _pt[0];
382  this->data[1] /= _pt[1];
383  this->data[2] /= _pt[2];
384 
385  return *this;
386  }
387 
392  public: const Vector3<T> operator/(T _v) const
393  {
394  return Vector3(this->data[0] / _v,
395  this->data[1] / _v,
396  this->data[2] / _v);
397  }
398 
403  public: const Vector3<T> &operator/=(T _v)
404  {
405  this->data[0] /= _v;
406  this->data[1] /= _v;
407  this->data[2] /= _v;
408 
409  return *this;
410  }
411 
416  public: Vector3<T> operator*(const Vector3<T> &_p) const
417  {
418  return Vector3(this->data[0] * _p[0],
419  this->data[1] * _p[1],
420  this->data[2] * _p[2]);
421  }
422 
427  public: const Vector3<T> &operator*=(const Vector3<T> &_v)
428  {
429  this->data[0] *= _v[0];
430  this->data[1] *= _v[1];
431  this->data[2] *= _v[2];
432 
433  return *this;
434  }
435 
439  public: inline Vector3<T> operator*(T _s) const
440  {
441  return Vector3<T>(this->data[0] * _s,
442  this->data[1] * _s,
443  this->data[2] * _s);
444  }
445 
450  public: friend inline Vector3<T> operator*(T _s, const Vector3<T> &_v)
451  {
452  return Vector3<T>(_v.X() * _s, _v.Y() * _s, _v.Z() * _s);
453  }
454 
458  public: const Vector3<T> &operator*=(T _v)
459  {
460  this->data[0] *= _v;
461  this->data[1] *= _v;
462  this->data[2] *= _v;
463 
464  return *this;
465  }
466 
471  public: bool operator==(const Vector3<T> &_v) const
472  {
473  return equal<T>(this->data[0], _v[0], static_cast<T>(0.001)) &&
474  equal<T>(this->data[1], _v[1], static_cast<T>(0.001)) &&
475  equal<T>(this->data[2], _v[2], static_cast<T>(0.001));
476  }
477 
482  public: bool operator!=(const Vector3<T> &_v) const
483  {
484  return !(*this == _v);
485  }
486 
489  public: bool IsFinite() const
490  {
491  // std::isfinite works with floating point values,
492  // need to explicit cast to avoid ambiguity in vc++.
493  return std::isfinite(static_cast<double>(this->data[0])) &&
494  std::isfinite(static_cast<double>(this->data[1])) &&
495  std::isfinite(static_cast<double>(this->data[2]));
496  }
497 
499  public: inline void Correct()
500  {
501  // std::isfinite works with floating point values,
502  // need to explicit cast to avoid ambiguity in vc++.
503  if (!std::isfinite(static_cast<double>(this->data[0])))
504  this->data[0] = 0;
505  if (!std::isfinite(static_cast<double>(this->data[1])))
506  this->data[1] = 0;
507  if (!std::isfinite(static_cast<double>(this->data[2])))
508  this->data[2] = 0;
509  }
510 
516  public: T operator[](size_t _index) const
517  {
518  if (_index > 2)
519  throw IndexException();
520  return this->data[_index];
521  }
522 
525  public: void Round(int _precision)
526  {
527  this->data[0] = precision(this->data[0], _precision);
528  this->data[1] = precision(this->data[1], _precision);
529  this->data[2] = precision(this->data[2], _precision);
530  }
531 
536  public: bool Equal(const Vector3<T> &_v) const
537  {
538  return equal<T>(this->data[0], _v[0]) &&
539  equal<T>(this->data[1], _v[1]) &&
540  equal<T>(this->data[2], _v[2]);
541  }
542 
545  public: inline T X() const
546  {
547  return this->data[0];
548  }
549 
552  public: inline T Y() const
553  {
554  return this->data[1];
555  }
556 
559  public: inline T Z() const
560  {
561  return this->data[2];
562  }
563 
566  public: inline T &X()
567  {
568  return this->data[0];
569  }
570 
573  public: inline T &Y()
574  {
575  return this->data[1];
576  }
577 
580  public: inline T &Z()
581  {
582  return this->data[2];
583  }
584 
587  public: inline void X(const T &_v)
588  {
589  this->data[0] = _v;
590  }
591 
594  public: inline void Y(const T &_v)
595  {
596  this->data[1] = _v;
597  }
598 
601  public: inline void Z(const T &_v)
602  {
603  this->data[2] = _v;
604  }
605 
610  public: friend std::ostream &operator<<(
611  std::ostream &_out, const ignition::math::Vector3<T> &_pt)
612  {
613  _out << precision(_pt[0], 6) << " " << precision(_pt[1], 6) << " "
614  << precision(_pt[2], 6);
615  return _out;
616  }
617 
622  public: friend std::istream &operator>>(
623  std::istream &_in, ignition::math::Vector3<T> &_pt)
624  {
625  // Skip white spaces
626  _in.setf(std::ios_base::skipws);
627  T x, y, z;
628  _in >> x >> y >> z;
629  _pt.Set(x, y, z);
630  return _in;
631  }
632 
634  private: T data[3];
635  };
636 
637  template<typename T> const Vector3<T> Vector3<T>::Zero(0, 0, 0);
638  template<typename T> const Vector3<T> Vector3<T>::One(1, 1, 1);
639  template<typename T> const Vector3<T> Vector3<T>::UnitX(1, 0, 0);
640  template<typename T> const Vector3<T> Vector3<T>::UnitY(0, 1, 0);
641  template<typename T> const Vector3<T> Vector3<T>::UnitZ(0, 0, 1);
642 
646  }
647 }
648 #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:573
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 \ return the length.
Definition: Vector3.hh:114
void Set(T _x=0, T _y=0, T _z=0)
Set the contents of the vector.
Definition: Vector3.hh:171
bool IsFinite() const
See if a point is finite (e.g., not nan)
Definition: Vector3.hh:489
T Sum() const
Return the sum of the values.
Definition: Vector3.hh:87
T precision(const T &_a, const unsigned int &_precision)
get value at a specified precision
Definition: Helpers.hh:188
Vector3 operator-() const
Negation operator.
Definition: Vector3.hh:337
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:316
T max(const std::vector< T > &_values)
get the maximum value of vector of values
Definition: Helpers.hh:150
bool Equal(const Vector3< T > &_v) const
Equality test.
Definition: Vector3.hh:536
T & Z()
Get a mutable reference to the z value.
Definition: Vector3.hh:580
Vector3< double > Vector3d
Definition: Vector3.hh:644
Vector3()
Constructor.
Definition: Vector3.hh:55
const Vector3< T > & operator/=(T _v)
Division assignment operator.
Definition: Vector3.hh:403
T X() const
Get the x value.
Definition: Vector3.hh:545
void Correct()
Corrects any nan values.
Definition: Vector3.hh:499
Vector3 Abs() const
Get the absolute value of the vector.
Definition: Vector3.hh:200
void Z(const T &_v)
Set the z value.
Definition: Vector3.hh:601
Vector3 Normalize()
Normalize the vector length.
Definition: Vector3.hh:132
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:191
bool operator==(const Vector3< T > &_v) const
Equal to operator.
Definition: Vector3.hh:471
Vector3 Perpendicular() const
Return a vector that is perpendicular to this one.
Definition: Vector3.hh:209
T operator[](size_t _index) const
Array subscript operator.
Definition: Vector3.hh:516
const Vector3< T > & operator/=(const Vector3< T > &_pt)
Division assignment operator.
Definition: Vector3.hh:379
Vector3 Cross(const Vector3< T > &_v) const
Return the cross product of this vector with another vector.
Definition: Vector3.hh:181
T & X()
Get a mutable reference to the x value.
Definition: Vector3.hh:566
T Y() const
Get the y value.
Definition: Vector3.hh:552
T Min() const
Get the minimum value in the vector.
Definition: Vector3.hh:284
void Round(int _precision)
Round all values to _precision decimal places.
Definition: Vector3.hh:525
Vector3 Rounded() const
Get a rounded version of this vector.
Definition: Vector3.hh:160
const Vector3< T > operator/(T _v) const
Division operator.
Definition: Vector3.hh:392
Vector3 & operator=(const Vector3< T > &_v)
Assignment operator.
Definition: Vector3.hh:292
static const Vector3 One
math::Vector3(1, 1, 1)
Definition: Vector3.hh:43
void Min(const Vector3< T > &_v)
Set this vector's components to the minimum of itself and the passed in vector.
Definition: Vector3.hh:265
const Vector3< T > & operator*=(const Vector3< T > &_v)
Multiplication assignment operators.
Definition: Vector3.hh:427
void Y(const T &_v)
Set the y value.
Definition: Vector3.hh:594
Vector3< T > operator*(T _s) const
Multiplication operators.
Definition: Vector3.hh:439
const Vector3 & operator+=(const Vector3< T > &_v)
Addition assignment operator.
Definition: Vector3.hh:326
void Max(const Vector3< T > &_v)
Set this vector's components to the maximum of itself and the passed in vector.
Definition: Vector3.hh:252
T Z() const
Get the z value.
Definition: Vector3.hh:559
Exception that is thrown when an out-of-bounds index is encountered.
Definition: IndexException.hh:30
Vector3< float > Vector3f
Definition: Vector3.hh:645
T DistToLine(const Vector3< T > &_pt1, const Vector3 &_pt2)
Get distance to a line.
Definition: Vector3.hh:242
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:610
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:229
void X(const T &_v)
Set the x value.
Definition: Vector3.hh:587
T Distance(T _x, T _y, T _z) const
Calc distance to the given point.
Definition: Vector3.hh:107
const Vector3< T > & operator-=(const Vector3< T > &_pt)
Subtraction assignment operators.
Definition: Vector3.hh:355
Vector3< T > operator*(const Vector3< T > &_p) const
Multiplication operator.
Definition: Vector3.hh:416
T SquaredLength() const
Return the square of the length (magnitude) of the vector.
Definition: Vector3.hh:123
bool operator!=(const Vector3< T > &_v) const
Not equal to operator.
Definition: Vector3.hh:482
static const Vector3 UnitX
math::Vector3(1, 0, 0)
Definition: Vector3.hh:46
const Vector3< T > & operator*=(T _v)
Multiplication operator.
Definition: Vector3.hh:458
Vector3 Round()
Round to near whole number, return the result.
Definition: Vector3.hh:150
Vector3 & operator=(T _v)
Assignment operator.
Definition: Vector3.hh:304
static const Vector3 UnitZ
math::Vector3(0, 0, 1)
Definition: Vector3.hh:52
Vector3< int > Vector3i
Definition: Vector3.hh:643
T min(const std::vector< T > &_values)
get the minimum value of vector of values
Definition: Helpers.hh:163
const Vector3< T > operator/(const Vector3< T > &_pt) const
Division operator.
Definition: Vector3.hh:368
T Max() const
Get the maximum value in the vector.
Definition: Vector3.hh:277
friend std::istream & operator>>(std::istream &_in, ignition::math::Vector3< T > &_pt)
Stream extraction operator.
Definition: Vector3.hh:622
friend Vector3< T > operator*(T _s, const Vector3< T > &_v)
Multiplication operators.
Definition: Vector3.hh:450
Vector3< T > operator-(const Vector3< T > &_pt) const
Subtraction operators.
Definition: Vector3.hh:345