Loading...
Searching...
No Matches
Vector4.hh
Go to the documentation of this file.
1/*
2 * Copyright (C) 2012 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_VECTOR4_HH_
18#define IGNITION_MATH_VECTOR4_HH_
19
22#include <ignition/math/config.hh>
23
24namespace ignition
25{
26 namespace math
27 {
28 inline namespace IGNITION_MATH_VERSION_NAMESPACE
29 {
32 template<typename T>
33 class Vector4
34 {
36 public: static const Vector4<T> Zero;
37
39 public: static const Vector4<T> One;
40
42 public: Vector4()
43 {
44 this->data[0] = this->data[1] = this->data[2] = this->data[3] = 0;
45 }
46
52 public: Vector4(const T &_x, const T &_y, const T &_z, const T &_w)
53 {
54 this->data[0] = _x;
55 this->data[1] = _y;
56 this->data[2] = _z;
57 this->data[3] = _w;
58 }
59
62 public: Vector4(const Vector4<T> &_v)
63 {
64 this->data[0] = _v[0];
65 this->data[1] = _v[1];
66 this->data[2] = _v[2];
67 this->data[3] = _v[3];
68 }
69
71 public: virtual ~Vector4() {}
72
76 public: T Distance(const Vector4<T> &_pt) const
77 {
78 return sqrt((this->data[0]-_pt[0])*(this->data[0]-_pt[0]) +
79 (this->data[1]-_pt[1])*(this->data[1]-_pt[1]) +
80 (this->data[2]-_pt[2])*(this->data[2]-_pt[2]) +
81 (this->data[3]-_pt[3])*(this->data[3]-_pt[3]));
82 }
83
86 public: T Length() const
87 {
88 return sqrt(this->SquaredLength());
89 }
90
93 public: T SquaredLength() const
94 {
95 return std::pow(this->data[0], 2)
96 + std::pow(this->data[1], 2)
97 + std::pow(this->data[2], 2)
98 + std::pow(this->data[3], 2);
99 }
100
102 public: void Normalize()
103 {
104 T d = this->Length();
105
106 if (!equal<T>(d, static_cast<T>(0.0)))
107 {
108 this->data[0] /= d;
109 this->data[1] /= d;
110 this->data[2] /= d;
111 this->data[3] /= d;
112 }
113 }
114
120 public: void Set(T _x = 0, T _y = 0, T _z = 0, T _w = 0)
121 {
122 this->data[0] = _x;
123 this->data[1] = _y;
124 this->data[2] = _z;
125 this->data[3] = _w;
126 }
127
131 public: Vector4<T> &operator=(const Vector4<T> &_v)
132 {
133 this->data[0] = _v[0];
134 this->data[1] = _v[1];
135 this->data[2] = _v[2];
136 this->data[3] = _v[3];
137
138 return *this;
139 }
140
143 public: Vector4<T> &operator=(T _value)
144 {
145 this->data[0] = _value;
146 this->data[1] = _value;
147 this->data[2] = _value;
148 this->data[3] = _value;
149
150 return *this;
151 }
152
156 public: Vector4<T> operator+(const Vector4<T> &_v) const
157 {
158 return Vector4<T>(this->data[0] + _v[0],
159 this->data[1] + _v[1],
160 this->data[2] + _v[2],
161 this->data[3] + _v[3]);
162 }
163
167 public: const Vector4<T> &operator+=(const Vector4<T> &_v)
168 {
169 this->data[0] += _v[0];
170 this->data[1] += _v[1];
171 this->data[2] += _v[2];
172 this->data[3] += _v[3];
173
174 return *this;
175 }
176
180 public: inline Vector4<T> operator+(const T _s) const
181 {
182 return Vector4<T>(this->data[0] + _s,
183 this->data[1] + _s,
184 this->data[2] + _s,
185 this->data[3] + _s);
186 }
187
192 public: friend inline Vector4<T> operator+(const T _s,
193 const Vector4<T> &_v)
194 {
195 return _v + _s;
196 }
197
201 public: const Vector4<T> &operator+=(const T _s)
202 {
203 this->data[0] += _s;
204 this->data[1] += _s;
205 this->data[2] += _s;
206 this->data[3] += _s;
207
208 return *this;
209 }
210
213 public: inline Vector4 operator-() const
214 {
215 return Vector4(-this->data[0], -this->data[1],
216 -this->data[2], -this->data[3]);
217 }
218
222 public: Vector4<T> operator-(const Vector4<T> &_v) const
223 {
224 return Vector4<T>(this->data[0] - _v[0],
225 this->data[1] - _v[1],
226 this->data[2] - _v[2],
227 this->data[3] - _v[3]);
228 }
229
233 public: const Vector4<T> &operator-=(const Vector4<T> &_v)
234 {
235 this->data[0] -= _v[0];
236 this->data[1] -= _v[1];
237 this->data[2] -= _v[2];
238 this->data[3] -= _v[3];
239
240 return *this;
241 }
242
246 public: inline Vector4<T> operator-(const T _s) const
247 {
248 return Vector4<T>(this->data[0] - _s,
249 this->data[1] - _s,
250 this->data[2] - _s,
251 this->data[3] - _s);
252 }
253
258 public: friend inline Vector4<T> operator-(const T _s,
259 const Vector4<T> &_v)
260 {
261 return {_s - _v.X(), _s - _v.Y(), _s - _v.Z(), _s - _v.W()};
262 }
263
267 public: const Vector4<T> &operator-=(const T _s)
268 {
269 this->data[0] -= _s;
270 this->data[1] -= _s;
271 this->data[2] -= _s;
272 this->data[3] -= _s;
273
274 return *this;
275 }
276
282 public: const Vector4<T> operator/(const Vector4<T> &_v) const
283 {
284 return Vector4<T>(this->data[0] / _v[0],
285 this->data[1] / _v[1],
286 this->data[2] / _v[2],
287 this->data[3] / _v[3]);
288 }
289
295 public: const Vector4<T> &operator/=(const Vector4<T> &_v)
296 {
297 this->data[0] /= _v[0];
298 this->data[1] /= _v[1];
299 this->data[2] /= _v[2];
300 this->data[3] /= _v[3];
301
302 return *this;
303 }
304
310 public: const Vector4<T> operator/(T _v) const
311 {
312 return Vector4<T>(this->data[0] / _v, this->data[1] / _v,
313 this->data[2] / _v, this->data[3] / _v);
314 }
315
319 public: const Vector4<T> &operator/=(T _v)
320 {
321 this->data[0] /= _v;
322 this->data[1] /= _v;
323 this->data[2] /= _v;
324 this->data[3] /= _v;
325
326 return *this;
327 }
328
334 public: const Vector4<T> operator*(const Vector4<T> &_pt) const
335 {
336 return Vector4<T>(this->data[0] * _pt[0],
337 this->data[1] * _pt[1],
338 this->data[2] * _pt[2],
339 this->data[3] * _pt[3]);
340 }
341
345 public: const Vector4<T> operator*(const Matrix4<T> &_m) const
346 {
347 return Vector4<T>(
348 this->data[0]*_m(0, 0) + this->data[1]*_m(1, 0) +
349 this->data[2]*_m(2, 0) + this->data[3]*_m(3, 0),
350 this->data[0]*_m(0, 1) + this->data[1]*_m(1, 1) +
351 this->data[2]*_m(2, 1) + this->data[3]*_m(3, 1),
352 this->data[0]*_m(0, 2) + this->data[1]*_m(1, 2) +
353 this->data[2]*_m(2, 2) + this->data[3]*_m(3, 2),
354 this->data[0]*_m(0, 3) + this->data[1]*_m(1, 3) +
355 this->data[2]*_m(2, 3) + this->data[3]*_m(3, 3));
356 }
357
363 public: const Vector4<T> &operator*=(const Vector4<T> &_pt)
364 {
365 this->data[0] *= _pt[0];
366 this->data[1] *= _pt[1];
367 this->data[2] *= _pt[2];
368 this->data[3] *= _pt[3];
369
370 return *this;
371 }
372
376 public: const Vector4<T> operator*(T _v) const
377 {
378 return Vector4<T>(this->data[0] * _v, this->data[1] * _v,
379 this->data[2] * _v, this->data[3] * _v);
380 }
381
386 public: friend inline const Vector4 operator*(const T _s,
387 const Vector4 &_v)
388 {
389 return Vector4(_v * _s);
390 }
391
395 public: const Vector4<T> &operator*=(T _v)
396 {
397 this->data[0] *= _v;
398 this->data[1] *= _v;
399 this->data[2] *= _v;
400 this->data[3] *= _v;
401
402 return *this;
403 }
404
410 public: bool Equal(const Vector4 &_v, const T &_tol) const
411 {
412 return equal<T>(this->data[0], _v[0], _tol)
413 && equal<T>(this->data[1], _v[1], _tol)
414 && equal<T>(this->data[2], _v[2], _tol)
415 && equal<T>(this->data[3], _v[3], _tol);
416 }
417
422 public: bool operator==(const Vector4<T> &_v) const
423 {
424 return this->Equal(_v, static_cast<T>(1e-6));
425 }
426
431 public: bool operator!=(const Vector4<T> &_pt) const
432 {
433 return !(*this == _pt);
434 }
435
438 public: bool IsFinite() const
439 {
440 // std::isfinite works with floating point values,
441 // need to explicit cast to avoid ambiguity in vc++.
442 return std::isfinite(static_cast<double>(this->data[0])) &&
443 std::isfinite(static_cast<double>(this->data[1])) &&
444 std::isfinite(static_cast<double>(this->data[2])) &&
445 std::isfinite(static_cast<double>(this->data[3]));
446 }
447
452 public: T &operator[](const std::size_t _index)
453 {
454 return this->data[clamp(_index, IGN_ZERO_SIZE_T, IGN_THREE_SIZE_T)];
455 }
456
461 public: T operator[](const std::size_t _index) const
462 {
463 return this->data[clamp(_index, IGN_ZERO_SIZE_T, IGN_THREE_SIZE_T)];
464 }
465
468 public: T &X()
469 {
470 return this->data[0];
471 }
472
475 public: T &Y()
476 {
477 return this->data[1];
478 }
479
482 public: T &Z()
483 {
484 return this->data[2];
485 }
486
489 public: T &W()
490 {
491 return this->data[3];
492 }
493
496 public: T X() const
497 {
498 return this->data[0];
499 }
500
503 public: T Y() const
504 {
505 return this->data[1];
506 }
507
510 public: T Z() const
511 {
512 return this->data[2];
513 }
514
517 public: T W() const
518 {
519 return this->data[3];
520 }
521
524 public: inline void X(const T &_v)
525 {
526 this->data[0] = _v;
527 }
528
531 public: inline void Y(const T &_v)
532 {
533 this->data[1] = _v;
534 }
535
538 public: inline void Z(const T &_v)
539 {
540 this->data[2] = _v;
541 }
542
545 public: inline void W(const T &_v)
546 {
547 this->data[3] = _v;
548 }
549
554 public: friend std::ostream &operator<<(
555 std::ostream &_out, const ignition::math::Vector4<T> &_pt)
556 {
557 _out << _pt[0] << " " << _pt[1] << " " << _pt[2] << " " << _pt[3];
558 return _out;
559 }
560
565 public: friend std::istream &operator>>(
566 std::istream &_in, ignition::math::Vector4<T> &_pt)
567 {
568 T x, y, z, w;
569
570 // Skip white spaces
571 _in.setf(std::ios_base::skipws);
572 _in >> x >> y >> z >> w;
573 _pt.Set(x, y, z, w);
574 return _in;
575 }
576
578 private: T data[4];
579 };
580
581 template<typename T>
582 const Vector4<T> Vector4<T>::Zero(0, 0, 0, 0);
583
584 template<typename T>
585 const Vector4<T> Vector4<T>::One(1, 1, 1, 1);
586
590 }
591 }
592}
593#endif
A 4x4 matrix class.
Definition Matrix4.hh:37
T Generic x, y, z, w vector.
Definition Vector4.hh:34
T SquaredLength() const
Return the square of the length (magnitude) of the vector.
Definition Vector4.hh:93
void Normalize()
Normalize the vector length.
Definition Vector4.hh:102
bool IsFinite() const
See if a point is finite (e.g., not nan)
Definition Vector4.hh:438
const Vector4< T > & operator/=(T _v)
Division operator.
Definition Vector4.hh:319
T W() const
Get the w value.
Definition Vector4.hh:517
Vector4< T > operator+(const T _s) const
Addition operators.
Definition Vector4.hh:180
const Vector4< T > & operator-=(const T _s)
Subtraction assignment operator.
Definition Vector4.hh:267
bool operator!=(const Vector4< T > &_pt) const
Not equal to operator.
Definition Vector4.hh:431
const Vector4< T > operator*(const Vector4< T > &_pt) const
Multiplication operator.
Definition Vector4.hh:334
Vector4(const Vector4< T > &_v)
Copy constructor.
Definition Vector4.hh:62
const Vector4< T > operator*(const Matrix4< T > &_m) const
Matrix multiplication operator.
Definition Vector4.hh:345
const Vector4< T > & operator/=(const Vector4< T > &_v)
Division assignment operator.
Definition Vector4.hh:295
T Z() const
Get the z value.
Definition Vector4.hh:510
T & operator[](const std::size_t _index)
Array subscript operator.
Definition Vector4.hh:452
T Y() const
Get the y value.
Definition Vector4.hh:503
Vector4()
Constructor.
Definition Vector4.hh:42
const Vector4< T > & operator+=(const T _s)
Addition assignment operator.
Definition Vector4.hh:201
bool operator==(const Vector4< T > &_v) const
Equal to operator.
Definition Vector4.hh:422
static const Vector4< T > One
math::Vector4(1, 1, 1, 1)
Definition Vector4.hh:39
const Vector4< T > operator/(const Vector4< T > &_v) const
Division assignment operator.
Definition Vector4.hh:282
void Z(const T &_v)
Set the z value.
Definition Vector4.hh:538
Vector4(const T &_x, const T &_y, const T &_z, const T &_w)
Constructor with component values.
Definition Vector4.hh:52
friend Vector4< T > operator-(const T _s, const Vector4< T > &_v)
Subtraction operators.
Definition Vector4.hh:258
void Set(T _x=0, T _y=0, T _z=0, T _w=0)
Set the contents of the vector.
Definition Vector4.hh:120
T operator[](const std::size_t _index) const
Const-qualified array subscript operator.
Definition Vector4.hh:461
friend std::ostream & operator<<(std::ostream &_out, const ignition::math::Vector4< T > &_pt)
Stream insertion operator.
Definition Vector4.hh:554
const Vector4< T > operator/(T _v) const
Division assignment operator.
Definition Vector4.hh:310
friend std::istream & operator>>(std::istream &_in, ignition::math::Vector4< T > &_pt)
Stream extraction operator.
Definition Vector4.hh:565
friend Vector4< T > operator+(const T _s, const Vector4< T > &_v)
Addition operators.
Definition Vector4.hh:192
bool Equal(const Vector4 &_v, const T &_tol) const
Equality test with tolerance.
Definition Vector4.hh:410
Vector4< T > & operator=(const Vector4< T > &_v)
Assignment operator.
Definition Vector4.hh:131
Vector4< T > operator-(const Vector4< T > &_v) const
Subtraction operator.
Definition Vector4.hh:222
T & X()
Return a mutable x value.
Definition Vector4.hh:468
T & Y()
Return a mutable y value.
Definition Vector4.hh:475
Vector4< T > operator-(const T _s) const
Subtraction operators.
Definition Vector4.hh:246
const Vector4< T > & operator+=(const Vector4< T > &_v)
Addition operator.
Definition Vector4.hh:167
virtual ~Vector4()
Destructor.
Definition Vector4.hh:71
T Distance(const Vector4< T > &_pt) const
Calc distance to the given point.
Definition Vector4.hh:76
Vector4< T > & operator=(T _value)
Assignment operator.
Definition Vector4.hh:143
T Length() const
Returns the length (magnitude) of the vector.
Definition Vector4.hh:86
T & W()
Return a mutable w value.
Definition Vector4.hh:489
void Y(const T &_v)
Set the y value.
Definition Vector4.hh:531
const Vector4< T > operator*(T _v) const
Multiplication operators.
Definition Vector4.hh:376
void X(const T &_v)
Set the x value.
Definition Vector4.hh:524
Vector4< T > operator+(const Vector4< T > &_v) const
Addition operator.
Definition Vector4.hh:156
T & Z()
Return a mutable z value.
Definition Vector4.hh:482
friend const Vector4 operator*(const T _s, const Vector4 &_v)
Scalar left multiplication operators.
Definition Vector4.hh:386
T X() const
Get the x value.
Definition Vector4.hh:496
static const Vector4< T > Zero
math::Vector4(0, 0, 0, 0)
Definition Vector4.hh:36
const Vector4< T > & operator*=(const Vector4< T > &_pt)
Multiplication assignment operator.
Definition Vector4.hh:363
Vector4 operator-() const
Negation operator.
Definition Vector4.hh:213
const Vector4< T > & operator-=(const Vector4< T > &_v)
Subtraction assigment operators.
Definition Vector4.hh:233
const Vector4< T > & operator*=(T _v)
Multiplication assignment operator.
Definition Vector4.hh:395
void W(const T &_v)
Set the w value.
Definition Vector4.hh:545
Vector4< int > Vector4i
Definition Vector4.hh:587
Vector4< double > Vector4d
Definition Vector4.hh:588
static const size_t IGN_THREE_SIZE_T
size_t type with a value of 3
Definition Helpers.hh:225
T clamp(T _v, T _min, T _max)
Simple clamping function.
Definition Helpers.hh:395
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
Vector4< float > Vector4f
Definition Vector4.hh:589
Definition Angle.hh:40