Loading...
Searching...
No Matches
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
26#include <ignition/math/config.hh>
27
28namespace ignition
29{
30 namespace math
31 {
32 inline namespace IGNITION_MATH_VERSION_NAMESPACE
33 {
38 template<typename T>
39 class Vector3
40 {
42 public: static const Vector3 Zero;
43
45 public: static const Vector3 One;
46
48 public: static const Vector3 UnitX;
49
51 public: static const Vector3 UnitY;
52
54 public: static const Vector3 UnitZ;
55
57 public: Vector3()
58 {
59 this->data[0] = 0;
60 this->data[1] = 0;
61 this->data[2] = 0;
62 }
63
68 public: Vector3(const T &_x, const T &_y, const T &_z)
69 {
70 this->data[0] = _x;
71 this->data[1] = _y;
72 this->data[2] = _z;
73 }
74
77 public: Vector3(const Vector3<T> &_v)
78 {
79 this->data[0] = _v[0];
80 this->data[1] = _v[1];
81 this->data[2] = _v[2];
82 }
83
85 public: virtual ~Vector3() {}
86
89 public: T Sum() const
90 {
91 return this->data[0] + this->data[1] + this->data[2];
92 }
93
97 public: T Distance(const Vector3<T> &_pt) const
98 {
99 return sqrt((this->data[0]-_pt[0])*(this->data[0]-_pt[0]) +
100 (this->data[1]-_pt[1])*(this->data[1]-_pt[1]) +
101 (this->data[2]-_pt[2])*(this->data[2]-_pt[2]));
102 }
103
109 public: T Distance(T _x, T _y, T _z) const
110 {
111 return this->Distance(Vector3(_x, _y, _z));
112 }
113
116 public: T Length() const
117 {
118 return sqrt(this->SquaredLength());
119 }
120
123 public: T SquaredLength() const
124 {
125 return std::pow(this->data[0], 2)
126 + std::pow(this->data[1], 2)
127 + std::pow(this->data[2], 2);
128 }
129
133 {
134 T d = this->Length();
135
136 if (!equal<T>(d, static_cast<T>(0.0)))
137 {
138 this->data[0] /= d;
139 this->data[1] /= d;
140 this->data[2] /= d;
141 }
142
143 return *this;
144 }
145
148 public: Vector3 Normalized() const
149 {
150 Vector3<T> result = *this;
151 result.Normalize();
152 return result;
153 }
154
157 public: Vector3 Round()
158 {
159 this->data[0] = nearbyint(this->data[0]);
160 this->data[1] = nearbyint(this->data[1]);
161 this->data[2] = nearbyint(this->data[2]);
162 return *this;
163 }
164
167 public: Vector3 Rounded() const
168 {
169 Vector3<T> result = *this;
170 result.Round();
171 return result;
172 }
173
178 public: inline void Set(T _x = 0, T _y = 0, T _z = 0)
179 {
180 this->data[0] = _x;
181 this->data[1] = _y;
182 this->data[2] = _z;
183 }
184
188 public: Vector3 Cross(const Vector3<T> &_v) const
189 {
190 return Vector3(this->data[1] * _v[2] - this->data[2] * _v[1],
191 this->data[2] * _v[0] - this->data[0] * _v[2],
192 this->data[0] * _v[1] - this->data[1] * _v[0]);
193 }
194
198 public: T Dot(const Vector3<T> &_v) const
199 {
200 return this->data[0] * _v[0] +
201 this->data[1] * _v[1] +
202 this->data[2] * _v[2];
203 }
204
213 public: T AbsDot(const Vector3<T> &_v) const
214 {
215 return std::abs(this->data[0] * _v[0]) +
216 std::abs(this->data[1] * _v[1]) +
217 std::abs(this->data[2] * _v[2]);
218 }
219
222 public: Vector3 Abs() const
223 {
224 return Vector3(std::abs(this->data[0]),
225 std::abs(this->data[1]),
226 std::abs(this->data[2]));
227 }
228
231 public: Vector3 Perpendicular() const
232 {
233 static const T sqrZero = 1e-06 * 1e-06;
234
235 Vector3<T> perp = this->Cross(Vector3(1, 0, 0));
236
237 // Check the length of the vector
238 if (perp.SquaredLength() < sqrZero)
239 {
240 perp = this->Cross(Vector3(0, 1, 0));
241 }
242
243 return perp;
244 }
245
251 public: static Vector3 Normal(const Vector3<T> &_v1,
252 const Vector3<T> &_v2, const Vector3<T> &_v3)
253 {
254 Vector3<T> a = _v2 - _v1;
255 Vector3<T> b = _v3 - _v1;
256 Vector3<T> n = a.Cross(b);
257 return n.Normalize();
258 }
259
264 public: T DistToLine(const Vector3<T> &_pt1, const Vector3 &_pt2)
265 {
266 T d = ((*this) - _pt1).Cross((*this) - _pt2).Length();
267 d = d / (_pt2 - _pt1).Length();
268 return d;
269 }
270
274 public: void Max(const Vector3<T> &_v)
275 {
276 if (_v[0] > this->data[0])
277 this->data[0] = _v[0];
278 if (_v[1] > this->data[1])
279 this->data[1] = _v[1];
280 if (_v[2] > this->data[2])
281 this->data[2] = _v[2];
282 }
283
287 public: void Min(const Vector3<T> &_v)
288 {
289 if (_v[0] < this->data[0])
290 this->data[0] = _v[0];
291 if (_v[1] < this->data[1])
292 this->data[1] = _v[1];
293 if (_v[2] < this->data[2])
294 this->data[2] = _v[2];
295 }
296
299 public: T Max() const
300 {
301 return std::max(std::max(this->data[0], this->data[1]), this->data[2]);
302 }
303
306 public: T Min() const
307 {
308 return std::min(std::min(this->data[0], this->data[1]), this->data[2]);
309 }
310
314 public: Vector3 &operator=(const Vector3<T> &_v)
315 {
316 this->data[0] = _v[0];
317 this->data[1] = _v[1];
318 this->data[2] = _v[2];
319
320 return *this;
321 }
322
326 public: Vector3 &operator=(T _v)
327 {
328 this->data[0] = _v;
329 this->data[1] = _v;
330 this->data[2] = _v;
331
332 return *this;
333 }
334
338 public: Vector3 operator+(const Vector3<T> &_v) const
339 {
340 return Vector3(this->data[0] + _v[0],
341 this->data[1] + _v[1],
342 this->data[2] + _v[2]);
343 }
344
348 public: const Vector3 &operator+=(const Vector3<T> &_v)
349 {
350 this->data[0] += _v[0];
351 this->data[1] += _v[1];
352 this->data[2] += _v[2];
353
354 return *this;
355 }
356
360 public: inline Vector3<T> operator+(const T _s) const
361 {
362 return Vector3<T>(this->data[0] + _s,
363 this->data[1] + _s,
364 this->data[2] + _s);
365 }
366
371 public: friend inline Vector3<T> operator+(const T _s,
372 const Vector3<T> &_v)
373 {
374 return {_v.X() + _s, _v.Y() + _s, _v.Z() + _s};
375 }
376
380 public: const Vector3<T> &operator+=(const T _s)
381 {
382 this->data[0] += _s;
383 this->data[1] += _s;
384 this->data[2] += _s;
385
386 return *this;
387 }
388
391 public: inline Vector3 operator-() const
392 {
393 return Vector3(-this->data[0], -this->data[1], -this->data[2]);
394 }
395
399 public: inline Vector3<T> operator-(const Vector3<T> &_pt) const
400 {
401 return Vector3(this->data[0] - _pt[0],
402 this->data[1] - _pt[1],
403 this->data[2] - _pt[2]);
404 }
405
409 public: const Vector3<T> &operator-=(const Vector3<T> &_pt)
410 {
411 this->data[0] -= _pt[0];
412 this->data[1] -= _pt[1];
413 this->data[2] -= _pt[2];
414
415 return *this;
416 }
417
421 public: inline Vector3<T> operator-(const T _s) const
422 {
423 return Vector3<T>(this->data[0] - _s,
424 this->data[1] - _s,
425 this->data[2] - _s);
426 }
427
432 public: friend inline Vector3<T> operator-(const T _s,
433 const Vector3<T> &_v)
434 {
435 return {_s - _v.X(), _s - _v.Y(), _s - _v.Z()};
436 }
437
441 public: const Vector3<T> &operator-=(const T _s)
442 {
443 this->data[0] -= _s;
444 this->data[1] -= _s;
445 this->data[2] -= _s;
446
447 return *this;
448 }
449
454 public: const Vector3<T> operator/(const Vector3<T> &_pt) const
455 {
456 return Vector3(this->data[0] / _pt[0],
457 this->data[1] / _pt[1],
458 this->data[2] / _pt[2]);
459 }
460
465 public: const Vector3<T> &operator/=(const Vector3<T> &_pt)
466 {
467 this->data[0] /= _pt[0];
468 this->data[1] /= _pt[1];
469 this->data[2] /= _pt[2];
470
471 return *this;
472 }
473
478 public: const Vector3<T> operator/(T _v) const
479 {
480 return Vector3(this->data[0] / _v,
481 this->data[1] / _v,
482 this->data[2] / _v);
483 }
484
489 public: const Vector3<T> &operator/=(T _v)
490 {
491 this->data[0] /= _v;
492 this->data[1] /= _v;
493 this->data[2] /= _v;
494
495 return *this;
496 }
497
502 public: Vector3<T> operator*(const Vector3<T> &_p) const
503 {
504 return Vector3(this->data[0] * _p[0],
505 this->data[1] * _p[1],
506 this->data[2] * _p[2]);
507 }
508
513 public: const Vector3<T> &operator*=(const Vector3<T> &_v)
514 {
515 this->data[0] *= _v[0];
516 this->data[1] *= _v[1];
517 this->data[2] *= _v[2];
518
519 return *this;
520 }
521
525 public: inline Vector3<T> operator*(T _s) const
526 {
527 return Vector3<T>(this->data[0] * _s,
528 this->data[1] * _s,
529 this->data[2] * _s);
530 }
531
536 public: friend inline Vector3<T> operator*(T _s, const Vector3<T> &_v)
537 {
538 return {_v.X() * _s, _v.Y() * _s, _v.Z() * _s};
539 }
540
544 public: const Vector3<T> &operator*=(T _v)
545 {
546 this->data[0] *= _v;
547 this->data[1] *= _v;
548 this->data[2] *= _v;
549
550 return *this;
551 }
552
558 public: bool Equal(const Vector3 &_v, const T &_tol) const
559 {
560 return equal<T>(this->data[0], _v[0], _tol)
561 && equal<T>(this->data[1], _v[1], _tol)
562 && equal<T>(this->data[2], _v[2], _tol);
563 }
564
569 public: bool operator==(const Vector3<T> &_v) const
570 {
571 return this->Equal(_v, static_cast<T>(1e-3));
572 }
573
578 public: bool operator!=(const Vector3<T> &_v) const
579 {
580 return !(*this == _v);
581 }
582
585 public: bool IsFinite() const
586 {
587 // std::isfinite works with floating point values,
588 // need to explicit cast to avoid ambiguity in vc++.
589 return std::isfinite(static_cast<double>(this->data[0])) &&
590 std::isfinite(static_cast<double>(this->data[1])) &&
591 std::isfinite(static_cast<double>(this->data[2]));
592 }
593
595 public: inline void Correct()
596 {
597 // std::isfinite works with floating point values,
598 // need to explicit cast to avoid ambiguity in vc++.
599 if (!std::isfinite(static_cast<double>(this->data[0])))
600 this->data[0] = 0;
601 if (!std::isfinite(static_cast<double>(this->data[1])))
602 this->data[1] = 0;
603 if (!std::isfinite(static_cast<double>(this->data[2])))
604 this->data[2] = 0;
605 }
606
611 public: T &operator[](const std::size_t _index)
612 {
613 return this->data[clamp(_index, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)];
614 }
615
620 public: T operator[](const std::size_t _index) const
621 {
622 return this->data[clamp(_index, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)];
623 }
624
627 public: void Round(int _precision)
628 {
629 this->data[0] = precision(this->data[0], _precision);
630 this->data[1] = precision(this->data[1], _precision);
631 this->data[2] = precision(this->data[2], _precision);
632 }
633
638 public: bool Equal(const Vector3<T> &_v) const
639 {
640 return equal<T>(this->data[0], _v[0]) &&
641 equal<T>(this->data[1], _v[1]) &&
642 equal<T>(this->data[2], _v[2]);
643 }
644
647 public: inline T X() const
648 {
649 return this->data[0];
650 }
651
654 public: inline T Y() const
655 {
656 return this->data[1];
657 }
658
661 public: inline T Z() const
662 {
663 return this->data[2];
664 }
665
668 public: inline T &X()
669 {
670 return this->data[0];
671 }
672
675 public: inline T &Y()
676 {
677 return this->data[1];
678 }
679
682 public: inline T &Z()
683 {
684 return this->data[2];
685 }
686
689 public: inline void X(const T &_v)
690 {
691 this->data[0] = _v;
692 }
693
696 public: inline void Y(const T &_v)
697 {
698 this->data[1] = _v;
699 }
700
703 public: inline void Z(const T &_v)
704 {
705 this->data[2] = _v;
706 }
707
712 public: bool operator<(const Vector3<T> &_pt) const
713 {
714 return this->data[0] < _pt[0] || this->data[1] < _pt[1] ||
715 this->data[2] < _pt[2];
716 }
717
722 public: friend std::ostream &operator<<(
723 std::ostream &_out, const ignition::math::Vector3<T> &_pt)
724 {
725 _out << precision(_pt[0], 6) << " " << precision(_pt[1], 6) << " "
726 << precision(_pt[2], 6);
727 return _out;
728 }
729
734 public: friend std::istream &operator>>(
735 std::istream &_in, ignition::math::Vector3<T> &_pt)
736 {
737 // Skip white spaces
738 _in.setf(std::ios_base::skipws);
739 T x, y, z;
740 _in >> x >> y >> z;
741 _pt.Set(x, y, z);
742 return _in;
743 }
744
746 private: T data[3];
747 };
748
749 template<typename T> const Vector3<T> Vector3<T>::Zero(0, 0, 0);
750 template<typename T> const Vector3<T> Vector3<T>::One(1, 1, 1);
751 template<typename T> const Vector3<T> Vector3<T>::UnitX(1, 0, 0);
752 template<typename T> const Vector3<T> Vector3<T>::UnitY(0, 1, 0);
753 template<typename T> const Vector3<T> Vector3<T>::UnitZ(0, 0, 1);
754
758 }
759 }
760}
761#endif
The Vector3 class represents the generic vector containing 3 elements.
Definition Vector3.hh:40
void X(const T &_v)
Set the x value.
Definition Vector3.hh:689
friend Vector3< T > operator*(T _s, const Vector3< T > &_v)
Multiplication operators.
Definition Vector3.hh:536
static const Vector3 One
math::Vector3(1, 1, 1)
Definition Vector3.hh:45
Vector3 Abs() const
Get the absolute value of the vector.
Definition Vector3.hh:222
Vector3()
Constructor.
Definition Vector3.hh:57
Vector3< T > operator*(T _s) const
Multiplication operators.
Definition Vector3.hh:525
Vector3 Perpendicular() const
Return a vector that is perpendicular to this one.
Definition Vector3.hh:231
Vector3 Round()
Round to near whole number, return the result.
Definition Vector3.hh:157
const Vector3< T > & operator-=(const T _s)
Subtraction assignment operator.
Definition Vector3.hh:441
bool IsFinite() const
See if a point is finite (e.g., not nan)
Definition Vector3.hh:585
T Sum() const
Return the sum of the values.
Definition Vector3.hh:89
void Y(const T &_v)
Set the y value.
Definition Vector3.hh:696
friend std::ostream & operator<<(std::ostream &_out, const ignition::math::Vector3< T > &_pt)
Stream insertion operator.
Definition Vector3.hh:722
void Round(int _precision)
Round all values to _precision decimal places.
Definition Vector3.hh:627
void Set(T _x=0, T _y=0, T _z=0)
Set the contents of the vector.
Definition Vector3.hh:178
void Correct()
Corrects any nan values.
Definition Vector3.hh:595
void Min(const Vector3< T > &_v)
Set this vector's components to the minimum of itself and the passed in vector.
Definition Vector3.hh:287
static const Vector3 UnitY
math::Vector3(0, 1, 0)
Definition Vector3.hh:51
T Z() const
Get the z value.
Definition Vector3.hh:661
T SquaredLength() const
Return the square of the length (magnitude) of the vector.
Definition Vector3.hh:123
bool Equal(const Vector3 &_v, const T &_tol) const
Equality test with tolerance.
Definition Vector3.hh:558
T & X()
Get a mutable reference to the x value.
Definition Vector3.hh:668
const Vector3 & operator+=(const Vector3< T > &_v)
Addition assignment operator.
Definition Vector3.hh:348
T Max() const
Get the maximum value in the vector.
Definition Vector3.hh:299
friend std::istream & operator>>(std::istream &_in, ignition::math::Vector3< T > &_pt)
Stream extraction operator.
Definition Vector3.hh:734
void Max(const Vector3< T > &_v)
Set this vector's components to the maximum of itself and the passed in vector.
Definition Vector3.hh:274
Vector3 & operator=(T _v)
Assignment operator.
Definition Vector3.hh:326
T Distance(T _x, T _y, T _z) const
Calc distance to the given point.
Definition Vector3.hh:109
T Y() const
Get the y value.
Definition Vector3.hh:654
Vector3< T > operator+(const T _s) const
Addition operators.
Definition Vector3.hh:360
T & operator[](const std::size_t _index)
Array subscript operator.
Definition Vector3.hh:611
const Vector3< T > & operator*=(T _v)
Multiplication operator.
Definition Vector3.hh:544
T Length() const
Returns the length (magnitude) of the vector.
Definition Vector3.hh:116
T Min() const
Get the minimum value in the vector.
Definition Vector3.hh:306
bool Equal(const Vector3< T > &_v) const
Equality test.
Definition Vector3.hh:638
T AbsDot(const Vector3< T > &_v) const
Return the absolute dot product of this vector and another vector.
Definition Vector3.hh:213
T operator[](const std::size_t _index) const
Const-qualified array subscript operator.
Definition Vector3.hh:620
Vector3 Normalize()
Normalize the vector length.
Definition Vector3.hh:132
const Vector3< T > & operator-=(const Vector3< T > &_pt)
Subtraction assignment operators.
Definition Vector3.hh:409
T & Z()
Get a mutable reference to the z value.
Definition Vector3.hh:682
Vector3(const Vector3< T > &_v)
Copy constructor.
Definition Vector3.hh:77
friend Vector3< T > operator-(const T _s, const Vector3< T > &_v)
Subtraction operators.
Definition Vector3.hh:432
Vector3< T > operator-(const Vector3< T > &_pt) const
Subtraction operators.
Definition Vector3.hh:399
Vector3 Rounded() const
Get a rounded version of this vector.
Definition Vector3.hh:167
virtual ~Vector3()
Destructor.
Definition Vector3.hh:85
T Distance(const Vector3< T > &_pt) const
Calc distance to the given point.
Definition Vector3.hh:97
Vector3< T > operator-(const T _s) const
Subtraction operators.
Definition Vector3.hh:421
static const Vector3 UnitX
math::Vector3(1, 0, 0)
Definition Vector3.hh:48
Vector3 Normalized() const
Return a normalized vector.
Definition Vector3.hh:148
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:251
T Dot(const Vector3< T > &_v) const
Return the dot product of this vector and another vector.
Definition Vector3.hh:198
Vector3 operator+(const Vector3< T > &_v) const
Addition operator.
Definition Vector3.hh:338
void Z(const T &_v)
Set the z value.
Definition Vector3.hh:703
const Vector3< T > & operator/=(T _v)
Division assignment operator.
Definition Vector3.hh:489
Vector3 Cross(const Vector3< T > &_v) const
Return the cross product of this vector with another vector.
Definition Vector3.hh:188
const Vector3< T > operator/(const Vector3< T > &_pt) const
Division operator.
Definition Vector3.hh:454
Vector3 operator-() const
Negation operator.
Definition Vector3.hh:391
T X() const
Get the x value.
Definition Vector3.hh:647
const Vector3< T > & operator*=(const Vector3< T > &_v)
Multiplication assignment operators.
Definition Vector3.hh:513
static const Vector3 Zero
math::Vector3(0, 0, 0)
Definition Vector3.hh:42
Vector3(const T &_x, const T &_y, const T &_z)
Constructor.
Definition Vector3.hh:68
Vector3 & operator=(const Vector3< T > &_v)
Assignment operator.
Definition Vector3.hh:314
friend Vector3< T > operator+(const T _s, const Vector3< T > &_v)
Addition operators.
Definition Vector3.hh:371
T & Y()
Get a mutable reference to the y value.
Definition Vector3.hh:675
const Vector3< T > & operator/=(const Vector3< T > &_pt)
Division assignment operator.
Definition Vector3.hh:465
bool operator<(const Vector3< T > &_pt) const
Less than operator.
Definition Vector3.hh:712
Vector3< T > operator*(const Vector3< T > &_p) const
Multiplication operator.
Definition Vector3.hh:502
static const Vector3 UnitZ
math::Vector3(0, 0, 1)
Definition Vector3.hh:54
bool operator==(const Vector3< T > &_v) const
Equal to operator.
Definition Vector3.hh:569
const Vector3< T > & operator+=(const T _s)
Addition assignment operator.
Definition Vector3.hh:380
bool operator!=(const Vector3< T > &_v) const
Not equal to operator.
Definition Vector3.hh:578
T DistToLine(const Vector3< T > &_pt1, const Vector3 &_pt2)
Get distance to a line.
Definition Vector3.hh:264
const Vector3< T > operator/(T _v) const
Division operator.
Definition Vector3.hh:478
T precision(const T &_a, const unsigned int &_precision)
get value at a specified precision
Definition Helpers.hh:579
Vector3< double > Vector3d
Definition Vector3.hh:756
Vector3< float > Vector3f
Definition Vector3.hh:757
T clamp(T _v, T _min, T _max)
Simple clamping function.
Definition Helpers.hh:395
Vector3< int > Vector3i
Definition Vector3.hh:755
static const size_t IGN_TWO_SIZE_T
size_t type with a value of 2
Definition Helpers.hh:222
bool equal(const T &_a, const T &_b, const T &_epsilon=T(1e-6))
check if two values are equal, within a tolerance
Definition Helpers.hh:545
static const size_t IGN_ZERO_SIZE_T
size_t type with a value of 0
Definition Helpers.hh:216
Definition Angle.hh:40