All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator 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 
206  public: T AbsDot(const Vector3<T> &_v) const
207  {
208  return std::abs(this->data[0] * _v[0]) +
209  std::abs(this->data[1] * _v[1]) +
210  std::abs(this->data[2] * _v[2]);
211  }
212 
215  public: Vector3 Abs() const
216  {
217  return Vector3(std::abs(this->data[0]),
218  std::abs(this->data[1]),
219  std::abs(this->data[2]));
220  }
221 
224  public: Vector3 Perpendicular() const
225  {
226  static const T sqrZero = 1e-06 * 1e-06;
227 
228  Vector3<T> perp = this->Cross(Vector3(1, 0, 0));
229 
230  // Check the length of the vector
231  if (perp.SquaredLength() < sqrZero)
232  {
233  perp = this->Cross(Vector3(0, 1, 0));
234  }
235 
236  return perp;
237  }
238 
244  public: static Vector3 Normal(const Vector3<T> &_v1,
245  const Vector3<T> &_v2, const Vector3<T> &_v3)
246  {
247  Vector3<T> a = _v2 - _v1;
248  Vector3<T> b = _v3 - _v1;
249  Vector3<T> n = a.Cross(b);
250  return n.Normalize();
251  }
252 
257  public: T DistToLine(const Vector3<T> &_pt1, const Vector3 &_pt2)
258  {
259  T d = ((*this) - _pt1).Cross((*this) - _pt2).Length();
260  d = d / (_pt2 - _pt1).Length();
261  return d;
262  }
263 
267  public: void Max(const Vector3<T> &_v)
268  {
269  if (_v[0] > this->data[0])
270  this->data[0] = _v[0];
271  if (_v[1] > this->data[1])
272  this->data[1] = _v[1];
273  if (_v[2] > this->data[2])
274  this->data[2] = _v[2];
275  }
276 
280  public: void Min(const Vector3<T> &_v)
281  {
282  if (_v[0] < this->data[0])
283  this->data[0] = _v[0];
284  if (_v[1] < this->data[1])
285  this->data[1] = _v[1];
286  if (_v[2] < this->data[2])
287  this->data[2] = _v[2];
288  }
289 
292  public: T Max() const
293  {
294  return std::max(std::max(this->data[0], this->data[1]), this->data[2]);
295  }
296 
299  public: T Min() const
300  {
301  return std::min(std::min(this->data[0], this->data[1]), this->data[2]);
302  }
303 
307  public: Vector3 &operator=(const Vector3<T> &_v)
308  {
309  this->data[0] = _v[0];
310  this->data[1] = _v[1];
311  this->data[2] = _v[2];
312 
313  return *this;
314  }
315 
319  public: Vector3 &operator=(T _v)
320  {
321  this->data[0] = _v;
322  this->data[1] = _v;
323  this->data[2] = _v;
324 
325  return *this;
326  }
327 
331  public: Vector3 operator+(const Vector3<T> &_v) const
332  {
333  return Vector3(this->data[0] + _v[0],
334  this->data[1] + _v[1],
335  this->data[2] + _v[2]);
336  }
337 
341  public: const Vector3 &operator+=(const Vector3<T> &_v)
342  {
343  this->data[0] += _v[0];
344  this->data[1] += _v[1];
345  this->data[2] += _v[2];
346 
347  return *this;
348  }
349 
352  public: inline Vector3 operator-() const
353  {
354  return Vector3(-this->data[0], -this->data[1], -this->data[2]);
355  }
356 
360  public: inline Vector3<T> operator-(const Vector3<T> &_pt) const
361  {
362  return Vector3(this->data[0] - _pt[0],
363  this->data[1] - _pt[1],
364  this->data[2] - _pt[2]);
365  }
366 
370  public: const Vector3<T> &operator-=(const Vector3<T> &_pt)
371  {
372  this->data[0] -= _pt[0];
373  this->data[1] -= _pt[1];
374  this->data[2] -= _pt[2];
375 
376  return *this;
377  }
378 
383  public: const Vector3<T> operator/(const Vector3<T> &_pt) const
384  {
385  return Vector3(this->data[0] / _pt[0],
386  this->data[1] / _pt[1],
387  this->data[2] / _pt[2]);
388  }
389 
394  public: const Vector3<T> &operator/=(const Vector3<T> &_pt)
395  {
396  this->data[0] /= _pt[0];
397  this->data[1] /= _pt[1];
398  this->data[2] /= _pt[2];
399 
400  return *this;
401  }
402 
407  public: const Vector3<T> operator/(T _v) const
408  {
409  return Vector3(this->data[0] / _v,
410  this->data[1] / _v,
411  this->data[2] / _v);
412  }
413 
418  public: const Vector3<T> &operator/=(T _v)
419  {
420  this->data[0] /= _v;
421  this->data[1] /= _v;
422  this->data[2] /= _v;
423 
424  return *this;
425  }
426 
431  public: Vector3<T> operator*(const Vector3<T> &_p) const
432  {
433  return Vector3(this->data[0] * _p[0],
434  this->data[1] * _p[1],
435  this->data[2] * _p[2]);
436  }
437 
442  public: const Vector3<T> &operator*=(const Vector3<T> &_v)
443  {
444  this->data[0] *= _v[0];
445  this->data[1] *= _v[1];
446  this->data[2] *= _v[2];
447 
448  return *this;
449  }
450 
454  public: inline Vector3<T> operator*(T _s) const
455  {
456  return Vector3<T>(this->data[0] * _s,
457  this->data[1] * _s,
458  this->data[2] * _s);
459  }
460 
465  public: friend inline Vector3<T> operator*(T _s, const Vector3<T> &_v)
466  {
467  return Vector3<T>(_v.X() * _s, _v.Y() * _s, _v.Z() * _s);
468  }
469 
473  public: const Vector3<T> &operator*=(T _v)
474  {
475  this->data[0] *= _v;
476  this->data[1] *= _v;
477  this->data[2] *= _v;
478 
479  return *this;
480  }
481 
486  public: bool operator==(const Vector3<T> &_v) const
487  {
488  return equal<T>(this->data[0], _v[0], static_cast<T>(0.001)) &&
489  equal<T>(this->data[1], _v[1], static_cast<T>(0.001)) &&
490  equal<T>(this->data[2], _v[2], static_cast<T>(0.001));
491  }
492 
497  public: bool operator!=(const Vector3<T> &_v) const
498  {
499  return !(*this == _v);
500  }
501 
504  public: bool IsFinite() const
505  {
506  // std::isfinite works with floating point values,
507  // need to explicit cast to avoid ambiguity in vc++.
508  return std::isfinite(static_cast<double>(this->data[0])) &&
509  std::isfinite(static_cast<double>(this->data[1])) &&
510  std::isfinite(static_cast<double>(this->data[2]));
511  }
512 
514  public: inline void Correct()
515  {
516  // std::isfinite works with floating point values,
517  // need to explicit cast to avoid ambiguity in vc++.
518  if (!std::isfinite(static_cast<double>(this->data[0])))
519  this->data[0] = 0;
520  if (!std::isfinite(static_cast<double>(this->data[1])))
521  this->data[1] = 0;
522  if (!std::isfinite(static_cast<double>(this->data[2])))
523  this->data[2] = 0;
524  }
525 
531  public: T operator[](size_t _index) const
532  {
533  if (_index > 2)
534  throw IndexException();
535  return this->data[_index];
536  }
537 
540  public: void Round(int _precision)
541  {
542  this->data[0] = precision(this->data[0], _precision);
543  this->data[1] = precision(this->data[1], _precision);
544  this->data[2] = precision(this->data[2], _precision);
545  }
546 
551  public: bool Equal(const Vector3<T> &_v) const
552  {
553  return equal<T>(this->data[0], _v[0]) &&
554  equal<T>(this->data[1], _v[1]) &&
555  equal<T>(this->data[2], _v[2]);
556  }
557 
560  public: inline T X() const
561  {
562  return this->data[0];
563  }
564 
567  public: inline T Y() const
568  {
569  return this->data[1];
570  }
571 
574  public: inline T Z() const
575  {
576  return this->data[2];
577  }
578 
581  public: inline T &X()
582  {
583  return this->data[0];
584  }
585 
588  public: inline T &Y()
589  {
590  return this->data[1];
591  }
592 
595  public: inline T &Z()
596  {
597  return this->data[2];
598  }
599 
602  public: inline void X(const T &_v)
603  {
604  this->data[0] = _v;
605  }
606 
609  public: inline void Y(const T &_v)
610  {
611  this->data[1] = _v;
612  }
613 
616  public: inline void Z(const T &_v)
617  {
618  this->data[2] = _v;
619  }
620 
625  public: friend std::ostream &operator<<(
626  std::ostream &_out, const ignition::math::Vector3<T> &_pt)
627  {
628  _out << precision(_pt[0], 6) << " " << precision(_pt[1], 6) << " "
629  << precision(_pt[2], 6);
630  return _out;
631  }
632 
637  public: friend std::istream &operator>>(
638  std::istream &_in, ignition::math::Vector3<T> &_pt)
639  {
640  // Skip white spaces
641  _in.setf(std::ios_base::skipws);
642  T x, y, z;
643  _in >> x >> y >> z;
644  _pt.Set(x, y, z);
645  return _in;
646  }
647 
649  private: T data[3];
650  };
651 
652  template<typename T> const Vector3<T> Vector3<T>::Zero(0, 0, 0);
653  template<typename T> const Vector3<T> Vector3<T>::One(1, 1, 1);
654  template<typename T> const Vector3<T> Vector3<T>::UnitX(1, 0, 0);
655  template<typename T> const Vector3<T> Vector3<T>::UnitY(0, 1, 0);
656  template<typename T> const Vector3<T> Vector3<T>::UnitZ(0, 0, 1);
657 
661  }
662 }
663 #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:588
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
T AbsDot(const Vector3< T > &_v) const
Return the absolute dot product of this vector and another vector.
Definition: Vector3.hh:206
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:504
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:250
Vector3 operator-() const
Negation operator.
Definition: Vector3.hh:352
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:331
T max(const std::vector< T > &_values)
get the maximum value of vector of values
Definition: Helpers.hh:211
bool Equal(const Vector3< T > &_v) const
Equality test.
Definition: Vector3.hh:551
T & Z()
Get a mutable reference to the z value.
Definition: Vector3.hh:595
Vector3< double > Vector3d
Definition: Vector3.hh:659
Vector3()
Constructor.
Definition: Vector3.hh:55
const Vector3< T > & operator/=(T _v)
Division assignment operator.
Definition: Vector3.hh:418
T X() const
Get the x value.
Definition: Vector3.hh:560
void Correct()
Corrects any nan values.
Definition: Vector3.hh:514
Vector3 Abs() const
Get the absolute value of the vector.
Definition: Vector3.hh:215
void Z(const T &_v)
Set the z value.
Definition: Vector3.hh:616
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:486
Vector3 Perpendicular() const
Return a vector that is perpendicular to this one.
Definition: Vector3.hh:224
T operator[](size_t _index) const
Array subscript operator.
Definition: Vector3.hh:531
const Vector3< T > & operator/=(const Vector3< T > &_pt)
Division assignment operator.
Definition: Vector3.hh:394
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:581
T Y() const
Get the y value.
Definition: Vector3.hh:567
T Min() const
Get the minimum value in the vector.
Definition: Vector3.hh:299
void Round(int _precision)
Round all values to _precision decimal places.
Definition: Vector3.hh:540
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:407
Vector3 & operator=(const Vector3< T > &_v)
Assignment operator.
Definition: Vector3.hh:307
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:280
const Vector3< T > & operator*=(const Vector3< T > &_v)
Multiplication assignment operators.
Definition: Vector3.hh:442
void Y(const T &_v)
Set the y value.
Definition: Vector3.hh:609
Vector3< T > operator*(T _s) const
Multiplication operators.
Definition: Vector3.hh:454
const Vector3 & operator+=(const Vector3< T > &_v)
Addition assignment operator.
Definition: Vector3.hh:341
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:267
T Z() const
Get the z value.
Definition: Vector3.hh:574
Exception that is thrown when an out-of-bounds index is encountered.
Definition: IndexException.hh:37
Vector3< float > Vector3f
Definition: Vector3.hh:660
T DistToLine(const Vector3< T > &_pt1, const Vector3 &_pt2)
Get distance to a line.
Definition: Vector3.hh:257
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:625
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:244
void X(const T &_v)
Set the x value.
Definition: Vector3.hh:602
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:370
Vector3< T > operator*(const Vector3< T > &_p) const
Multiplication operator.
Definition: Vector3.hh:431
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:497
static const Vector3 UnitX
math::Vector3(1, 0, 0)
Definition: Vector3.hh:46
const Vector3< T > & operator*=(T _v)
Multiplication operator.
Definition: Vector3.hh:473
Vector3 Round()
Round to near whole number, return the result.
Definition: Vector3.hh:150
Vector3 & operator=(T _v)
Assignment operator.
Definition: Vector3.hh:319
static const Vector3 UnitZ
math::Vector3(0, 0, 1)
Definition: Vector3.hh:52
Vector3< int > Vector3i
Definition: Vector3.hh:658
T min(const std::vector< T > &_values)
get the minimum value of vector of values
Definition: Helpers.hh:224
const Vector3< T > operator/(const Vector3< T > &_pt) const
Division operator.
Definition: Vector3.hh:383
T Max() const
Get the maximum value in the vector.
Definition: Vector3.hh:292
friend std::istream & operator>>(std::istream &_in, ignition::math::Vector3< T > &_pt)
Stream extraction operator.
Definition: Vector3.hh:637
friend Vector3< T > operator*(T _s, const Vector3< T > &_v)
Multiplication operators.
Definition: Vector3.hh:465
Vector3< T > operator-(const Vector3< T > &_pt) const
Subtraction operators.
Definition: Vector3.hh:360