ACloudViewer  3.9.4
A Modern Library for 3D Data Processing
CVGeom.h
Go to the documentation of this file.
1 // ----------------------------------------------------------------------------
2 // - CloudViewer: www.cloudViewer.org -
3 // ----------------------------------------------------------------------------
4 // Copyright (c) 2018-2024 www.cloudViewer.org
5 // SPDX-License-Identifier: MIT
6 // ----------------------------------------------------------------------------
7 
8 #pragma once
9 
10 // Local
11 #include <Eigen/Core>
12 
13 #include "CVCoreLib.h"
14 #include "CVTypes.h"
15 // clang-format off
16 #include "Eigen.h" // Must be included before <vector> to ensure vector specializations
17 // clang-format on
18 
19 // system
20 #include <algorithm>
21 #include <cmath>
22 #include <limits>
23 #include <vector>
24 
25 #ifdef _MSC_VER
26 // To get rid of the warning about unnamed struct/union
27 #pragma warning(disable : 4201)
28 #endif
29 
31 template <typename Type>
32 class Vector2Tpl {
33 public:
34  union {
35  struct {
36  Type x, y;
37  };
38  Type u[2];
39  };
40 
41  inline std::size_t rows() const { return dimensions(); }
42  inline std::size_t dimensions() const { return 2; }
43 
44  inline Type* data() { return u; }
45  inline const Type* data() const { return u; }
46 
48 
51  inline explicit Vector2Tpl(Type s = 0) : x(s), y(s) {}
52 
54 
58  inline Vector2Tpl(Type _x, Type _y) : x(_x), y(_y) {}
59 
61  inline Type norm2() const { return (x * x) + (y * y); }
63  inline Type norm() const { return std::sqrt(norm2()); }
65  inline void normalize() {
66  Type n = norm2();
67  if (n > 0) *this /= std::sqrt(n);
68  }
69 
71  inline Type dot(const Vector2Tpl& v) const { return (x * v.x) + (y * v.y); }
73 
76  inline Type cross(const Vector2Tpl& v) const { return x * v.y - y * v.x; }
77 
79  inline Vector2Tpl& operator-() {
80  x = -x;
81  y = -y;
82  return *this;
83  }
85  inline Vector2Tpl& operator+=(const Vector2Tpl& v) {
86  x += v.x;
87  y += v.y;
88  return *this;
89  }
91  inline Vector2Tpl& operator-=(const Vector2Tpl& v) {
92  x -= v.x;
93  y -= v.y;
94  return *this;
95  }
97  inline Vector2Tpl& operator*=(Type v) {
98  x *= v;
99  y *= v;
100  return *this;
101  }
103  inline Vector2Tpl& operator/=(Type v) {
104  x /= v;
105  y /= v;
106  return *this;
107  }
109  inline Vector2Tpl operator+(const Vector2Tpl& v) const {
110  return Vector2Tpl(x + v.x, y + v.y);
111  }
113  inline Vector2Tpl operator-(const Vector2Tpl& v) const {
114  return Vector2Tpl(x - v.x, y - v.y);
115  }
117  inline Vector2Tpl operator*(Type s) const {
118  return Vector2Tpl(x * s, y * s);
119  }
121  inline Vector2Tpl operator/(Type s) const {
122  return Vector2Tpl(x / s, y / s);
123  }
125  inline Type& operator[](unsigned i) { return u[i]; }
127  inline const Type& operator[](unsigned i) const { return u[i]; }
128 };
129 
131 template <class Type>
132 class Tuple3Tpl {
133 public:
134  // The 3 tuple values as a union (array/separate values)
135  union {
136  struct {
137  Type x, y, z;
138  };
139  Type u[3];
140  };
141 
142  inline std::size_t rows() const { return dimensions(); }
143  inline std::size_t dimensions() const { return 3; }
144 
145  inline Type* data() { return u; }
146  inline const Type* data() const { return u; }
147 
149 
151  inline Tuple3Tpl() : x(0), y(0), z(0) {}
152 
154 
156  inline Tuple3Tpl(Type a, Type b, Type c) : x(a), y(b), z(c) {}
157 
159  inline explicit Tuple3Tpl(const Type p[]) : x(p[0]), y(p[1]), z(p[2]) {}
160 
162  inline Tuple3Tpl operator-() const {
163  Tuple3Tpl V(-x, -y, -z);
164  return V;
165  }
167  inline Tuple3Tpl& operator+=(const Tuple3Tpl& v) {
168  x += v.x;
169  y += v.y;
170  z += v.z;
171  return *this;
172  }
174  inline Tuple3Tpl& operator-=(const Tuple3Tpl& v) {
175  x -= v.x;
176  y -= v.y;
177  z -= v.z;
178  return *this;
179  }
181  inline Tuple3Tpl& operator*=(Type v) {
182  x *= v;
183  y *= v;
184  z *= v;
185  return *this;
186  }
188  inline Tuple3Tpl& operator/=(Type v) {
189  x /= v;
190  y /= v;
191  z /= v;
192  return *this;
193  }
195  inline Tuple3Tpl operator+(const Tuple3Tpl& v) const {
196  return Tuple3Tpl(x + v.x, y + v.y, z + v.z);
197  }
199  inline Tuple3Tpl operator-(const Tuple3Tpl& v) const {
200  return Tuple3Tpl(x - v.x, y - v.y, z - v.z);
201  }
203  inline Tuple3Tpl operator*(Type s) const {
204  return Tuple3Tpl(x * s, y * s, z * s);
205  }
207  inline Tuple3Tpl operator/(Type s) const {
208  return Tuple3Tpl(x / s, y / s, z / s);
209  }
210 };
211 
220 
222 template <typename Type>
223 class Vector3Tpl : public Tuple3Tpl<Type> {
224 public:
225  // Don't ask me what other x, y, z or u members this class could
226  // use but it seems necessary for compilation on some platforms...
227  using Tuple3Tpl<Type>::x;
228  using Tuple3Tpl<Type>::y;
229  using Tuple3Tpl<Type>::z;
230  using Tuple3Tpl<Type>::u;
231 
233 
235  inline Vector3Tpl() : Tuple3Tpl<Type>() {}
236 
238 
240  inline Vector3Tpl(Type _x, Type _y, Type _z)
241  : Tuple3Tpl<Type>(_x, _y, _z) {}
242 
244  inline explicit Vector3Tpl(const Type p[]) : Tuple3Tpl<Type>(p) {}
245 
247  inline explicit Vector3Tpl(const Vector2Tpl<Type>& t2D, Type c)
248  : Tuple3Tpl<Type>(t2D.x, t2D.y, c) {}
249 
252  operator Vector3Tpl<double>() const { return Vector3Tpl<double>(x, y, z); }
253 
260  }
265  }
266 
268  static inline Vector3Tpl fromArray(const int a[3]) {
269  return Vector3Tpl(static_cast<Type>(a[0]), static_cast<Type>(a[1]),
270  static_cast<Type>(a[2]));
271  }
273  static inline Vector3Tpl fromArray(const float a[3]) {
274  return Vector3Tpl(static_cast<Type>(a[0]), static_cast<Type>(a[1]),
275  static_cast<Type>(a[2]));
276  }
278  static inline Vector3Tpl fromArray(const double a[3]) {
279  return Vector3Tpl(static_cast<Type>(a[0]), static_cast<Type>(a[1]),
280  static_cast<Type>(a[2]));
281  }
282 
284  static inline Vector3Tpl fromArray(const Eigen::Matrix<double, 3, 1>& a) {
285  return Vector3Tpl(static_cast<Type>(a[0]), static_cast<Type>(a[1]),
286  static_cast<Type>(a[2]));
287  }
288  static inline Eigen::Matrix<Type, 3, 1> fromArray(
289  const Vector3Tpl<float>& a) {
290  return Eigen::Matrix<Type, 3, 1>(static_cast<Type>(a[0]),
291  static_cast<Type>(a[1]),
292  static_cast<Type>(a[2]));
293  }
294  static inline Eigen::Matrix<Type, 3, 1> fromArray(
295  const Vector3Tpl<double>& a) {
296  return Eigen::Matrix<Type, 3, 1>(static_cast<Type>(a[0]),
297  static_cast<Type>(a[1]),
298  static_cast<Type>(a[2]));
299  }
300 
301  static inline std::vector<Eigen::Matrix<double, 3, 1>> fromArrayContainer(
302  const std::vector<Vector3Tpl<Type>>& container) {
303  std::vector<Eigen::Matrix<double, 3, 1>> points;
304  points.resize(container.size());
305  for (size_t i = 0; i < container.size(); ++i) {
306  points[i] = Eigen::Matrix<double, 3, 1>(
307  static_cast<double>(container[i][0]),
308  static_cast<double>(container[i][1]),
309  static_cast<double>(container[i][2]));
310  }
311  return points;
312  }
313 
314  static inline std::vector<Vector3Tpl> fromArrayContainer(
315  const std::vector<Eigen::Matrix<double, 3, 1>>& container) {
316  std::vector<Vector3Tpl> points;
317  points.resize(container.size());
318  for (size_t i = 0; i < container.size(); ++i) {
319  points[i] = Vector3Tpl(static_cast<Type>(container[i](0)),
320  static_cast<Type>(container[i](1)),
321  static_cast<Type>(container[i](2)));
322  }
323  return points;
324  }
325 
327  inline Vector3Tpl(const Eigen::Matrix<float, 3, 1>& v) { *this = v; }
328  inline Vector3Tpl(const Eigen::Matrix<double, 3, 1>& v) { *this = v; }
329 
331  inline Vector3Tpl& operator=(const Eigen::Matrix<double, 3, 1>& v) {
332  this->x = static_cast<Type>(v(0));
333  this->y = static_cast<Type>(v(1));
334  this->z = static_cast<Type>(v(2));
335  return *this;
336  }
337  inline Vector3Tpl& operator=(const Eigen::Matrix<float, 3, 1>& v) {
338  this->x = static_cast<Type>(v(0));
339  this->y = static_cast<Type>(v(1));
340  this->z = static_cast<Type>(v(2));
341  return *this;
342  }
343 
345  inline Vector3Tpl& operator+=(const Eigen::Matrix<double, 3, 1>& v) {
346  x += static_cast<Type>(v(0));
347  y += static_cast<Type>(v(1));
348  z += static_cast<Type>(v(2));
349  return *this;
350  }
352  inline Vector3Tpl& operator-=(const Eigen::Matrix<double, 3, 1>& v) {
353  x -= static_cast<Type>(v(0));
354  y -= static_cast<Type>(v(1));
355  z -= static_cast<Type>(v(2));
356  return *this;
357  }
358 
360  inline Vector3Tpl& operator*=(const Eigen::Matrix<double, 3, 1>& v) {
361  *this = *this * v;
362  return *this;
363  }
364 
366  inline Vector3Tpl operator-(const Eigen::Matrix<double, 3, 1>& v) const {
367  return Vector3Tpl(static_cast<Type>(x - v(0)),
368  static_cast<Type>(y - v(1)),
369  static_cast<Type>(z - v(2)));
370  }
371 
373  inline Vector3Tpl operator+(const Eigen::Matrix<double, 3, 1>& v) const {
374  return Vector3Tpl(static_cast<Type>(x + v(0)),
375  static_cast<Type>(y + v(1)),
376  static_cast<Type>(z + v(2)));
377  }
378 
380  inline Vector3Tpl operator*(const Eigen::Matrix<double, 3, 1>& v) const {
381  return cross(v);
382  }
383 
385  inline Type operator&&(const Eigen::Matrix<double, 3, 1>& v) const {
386  return dot(v);
387  }
388 
389  inline friend std::ostream& operator<<(std::ostream& stream,
390  const Vector3Tpl& v) {
391  stream << "[x->" << v.x << ", y->" << v.y << ", z->" << v.x << "]"
392  << std::endl;
393  return stream;
394  }
395 
397  inline Type& operator()(unsigned i) { return u[i]; }
399  inline const Type& operator()(unsigned i) const { return u[i]; }
400 
402  inline Type maxCoeff() const {
403  return x > y ? (x > z ? x : z) : (y > z ? y : z);
404  }
406  inline Type prod() const { return std::abs(x * y * z); }
408  inline Type dot(const Vector3Tpl& v) const {
409  return x * v.x + y * v.y + z * v.z;
410  }
412  inline Vector3Tpl cross(const Vector3Tpl& v) const {
413  return Vector3Tpl((y * v.z) - (z * v.y), (z * v.x) - (x * v.z),
414  (x * v.y) - (y * v.x));
415  }
417  inline Type norm2() const { return x * x + y * y + z * z; }
419  inline double norm2d() const {
420  return static_cast<double>(x) * x + static_cast<double>(y) * y +
421  static_cast<double>(z) * z;
422  }
424  inline Type norm() const { return static_cast<Type>(std::sqrt(norm2d())); }
426  inline double normd() const { return std::sqrt(norm2d()); }
428  inline void normalize() {
429  double n = norm2d();
430  if (n > 0) *this /= static_cast<Type>(std::sqrt(n));
431  }
433  inline Vector3Tpl orthogonal() const {
434  Vector3Tpl ort;
435  vorthogonal(u, ort.u);
436  return ort;
437  }
438 
440  inline Vector3Tpl operator-() const {
441  Vector3Tpl V(-x, -y, -z);
442  return V;
443  }
445  inline Vector3Tpl& operator+=(const Vector3Tpl& v) {
446  x += v.x;
447  y += v.y;
448  z += v.z;
449  return *this;
450  }
452  inline Vector3Tpl& operator-=(const Vector3Tpl& v) {
453  x -= v.x;
454  y -= v.y;
455  z -= v.z;
456  return *this;
457  }
459  inline Vector3Tpl& operator*=(Type v) {
460  x *= v;
461  y *= v;
462  z *= v;
463  return *this;
464  }
466  inline Vector3Tpl& operator/=(Type v) {
467  x /= v;
468  y /= v;
469  z /= v;
470  return *this;
471  }
473  inline Vector3Tpl operator+(const Vector3Tpl& v) const {
474  return Vector3Tpl(x + v.x, y + v.y, z + v.z);
475  }
477  inline Vector3Tpl operator-(const Vector3Tpl& v) const {
478  return Vector3Tpl(x - v.x, y - v.y, z - v.z);
479  }
481  inline Vector3Tpl operator*(Type s) const {
482  return Vector3Tpl(x * s, y * s, z * s);
483  }
485  inline Vector3Tpl operator/(Type s) const {
486  return Vector3Tpl(x / s, y / s, z / s);
487  }
489  inline Vector3Tpl operator*(const Vector3Tpl& v) const { return cross(v); }
491  inline Type operator&&(const Vector3Tpl& v) const { return dot(v); }
493  inline Type& operator[](unsigned i) { return u[i]; }
495  inline const Type& operator[](unsigned i) const { return u[i]; }
497  Type angle_rad(const Vector3Tpl& v) const { return vangle_rad(u, v.u); }
498  double angle_radd(const Vector3Tpl& v) const { return vangle_radd(u, v.u); }
499 
500  static inline void vdivide(const Type p[], Type s, Type r[]) {
501  r[0] = p[0] / s;
502  r[1] = p[1] / s;
503  r[2] = p[2] / s;
504  }
505  static inline void vdivide(Type p[], Type s) {
506  p[0] /= s;
507  p[1] /= s;
508  p[2] /= s;
509  }
510  static inline void vmultiply(const Type p[], Type s, Type r[]) {
511  r[0] = p[0] * s;
512  r[1] = p[1] * s;
513  r[2] = p[2] * s;
514  }
515  static inline void vmultiply(Type p[], Type s) {
516  p[0] *= s;
517  p[1] *= s;
518  p[2] *= s;
519  }
520  static inline Type vdot(const Type p[], const Type q[]) {
521  return (p[0] * q[0]) + (p[1] * q[1]) + (p[2] * q[2]);
522  }
523  static inline double vdotd(const Type p[], const Type q[]) {
524  return (static_cast<double>(p[0]) * q[0]) +
525  (static_cast<double>(p[1]) * q[1]) +
526  (static_cast<double>(p[2]) * q[2]);
527  }
528  static inline void vcross(const Type p[], const Type q[], Type r[]) {
529  r[0] = (p[1] * q[2]) - (p[2] * q[1]);
530  r[1] = (p[2] * q[0]) - (p[0] * q[2]);
531  r[2] = (p[0] * q[1]) - (p[1] * q[0]);
532  }
533  static inline void vcopy(const Type p[], Type q[]) {
534  q[0] = p[0];
535  q[1] = p[1];
536  q[2] = p[2];
537  }
538  static inline void vset(Type p[], Type s) { p[0] = p[1] = p[2] = s; }
539  static inline void vset(Type p[], Type x, Type y, Type z) {
540  p[0] = x;
541  p[1] = y;
542  p[2] = z;
543  }
544  static inline void vadd(const Type p[], const Type q[], Type r[]) {
545  r[0] = p[0] + q[0];
546  r[1] = p[1] + q[1];
547  r[2] = p[2] + q[2];
548  }
549  // note misspelling: should be vsubtract
550  static inline void vsubstract(const Type p[], const Type q[], Type r[]) {
551  r[0] = p[0] - q[0];
552  r[1] = p[1] - q[1];
553  r[2] = p[2] - q[2];
554  }
555  static inline void vcombination(
556  Type a, const Type p[], Type b, const Type q[], Type r[]) {
557  r[0] = (a * p[0]) + (b * q[0]);
558  r[1] = (a * p[1]) + (b * q[1]);
559  r[2] = (a * p[2]) + (b * q[2]);
560  }
561  static inline void vcombination(const Type p[],
562  Type b,
563  const Type q[],
564  Type r[]) {
565  r[0] = p[0] + (b * q[0]);
566  r[1] = p[1] + (b * q[1]);
567  r[2] = p[2] + (b * q[2]);
568  }
569  static inline void vnormalize(Type p[]) {
570  Type n = vnorm2(p);
571  if (n > 0) vdivide(p, std::sqrt(n));
572  }
573  static inline Type vnorm2(const Type p[]) {
574  return (p[0] * p[0]) + (p[1] * p[1]) + (p[2] * p[2]);
575  }
576  static inline double vnorm2d(const Type p[]) {
577  return (static_cast<double>(p[0]) * p[0]) +
578  (static_cast<double>(p[1]) * p[1]) +
579  (static_cast<double>(p[2]) * p[2]);
580  }
581  static inline Type vdistance2(const Type p[], const Type q[]) {
582  return ((p[0] - q[0]) * (p[0] - q[0])) +
583  ((p[1] - q[1]) * (p[1] - q[1])) +
584  ((p[2] - q[2]) * (p[2] - q[2]));
585  }
586  static inline double vdistance2d(const Type p[], const Type q[]) {
587  return ((static_cast<double>(p[0]) - q[0]) *
588  (static_cast<double>(p[0]) - q[0])) +
589  ((static_cast<double>(p[1]) - q[1]) *
590  (static_cast<double>(p[1]) - q[1])) +
591  ((static_cast<double>(p[2]) - q[2]) *
592  (static_cast<double>(p[2]) - q[2]));
593  }
594  static inline Type vnorm(const Type p[]) { return std::sqrt(vnorm2(p)); }
595  static inline double vnormd(const Type p[]) {
596  return std::sqrt(vnorm2d(p));
597  }
598  static inline Type vdistance(const Type p[], const Type q[]) {
599  return std::sqrt(vdistance2(p, q));
600  }
601  static inline double vdistanced(const Type p[], const Type q[]) {
602  return std::sqrt(vdistance2d(p, q));
603  }
604 
605  static inline void vorthogonal(const Type p[], Type q[]) {
606  if (std::abs(p[0]) <= std::abs(p[1]) &&
607  std::abs(p[0]) <= std::abs(p[2])) {
608  q[0] = 0;
609  q[1] = p[2];
610  q[2] = -p[1];
611  } else if (std::abs(p[1]) <= std::abs(p[0]) &&
612  std::abs(p[1]) <= std::abs(p[2])) {
613  q[0] = -p[2];
614  q[1] = 0;
615  q[2] = p[0];
616  } else {
617  q[0] = p[1];
618  q[1] = -p[0];
619  q[2] = 0;
620  }
621  vnormalize(q);
622  }
623 
624  static Type vangle_rad(const Type p[], const Type q[]) {
625  Type productNorm = vnorm(p) * vnorm(q);
626  if (productNorm < std::numeric_limits<Type>::epsilon()) {
627  return std::numeric_limits<Type>::quiet_NaN();
628  }
629 
630  Type cosAngle = vdot(p, q) / productNorm;
631  return acos(std::max(std::min(cosAngle, static_cast<Type>(1.0)),
632  static_cast<Type>(-1.0)));
633  }
634 
635  static double vangle_radd(const Type p[], const Type q[]) {
636  double productNorm = vnormd(p) * vnormd(q);
637  if (productNorm < std::numeric_limits<double>::epsilon()) {
638  return std::numeric_limits<double>::quiet_NaN();
639  }
640 
641  double cosAngle = vdotd(p, q) / productNorm;
642  return acos(std::max(std::min(cosAngle, 1.0), -1.0));
643  }
644 };
645 
647 template <class Type>
648 class Tuple4Tpl {
649 public:
650  // The 4 tuple values as a union (array/separate values)
651  union {
652  struct {
653  Type x, y, z, w;
654  };
655  Type u[4];
656  };
657 
658  inline std::size_t rows() const { return dimensions(); }
659  inline std::size_t dimensions() const { return 4; }
660 
661  inline Type* data() { return u; }
662  inline const Type* data() const { return u; }
663 
665 
667  inline Tuple4Tpl() : x(0), y(0), z(0), w(0) {}
668 
670 
672  inline Tuple4Tpl(Type a, Type b, Type c, Type d) : x(a), y(b), z(c), w(d) {}
673 
675  inline explicit Tuple4Tpl(const Type p[])
676  : x(p[0]), y(p[1]), z(p[2]), w(p[3]) {}
677 
679  static inline Tuple4Tpl fromArray(const Eigen::Matrix<Type, 4, 1>& a) {
680  return Tuple4Tpl(static_cast<Type>(a[0]), static_cast<Type>(a[1]),
681  static_cast<Type>(a[2]), static_cast<Type>(a[3]));
682  }
683  static inline Eigen::Matrix<Type, 4, 1> fromArray(
684  const Tuple4Tpl<float>& a) {
685  return Eigen::Matrix<Type, 4, 1>(
686  static_cast<Type>(a[0]), static_cast<Type>(a[1]),
687  static_cast<Type>(a[2]), static_cast<Type>(a[3]));
688  }
689  static inline Eigen::Matrix<Type, 4, 1> fromArray(
690  const Tuple4Tpl<double>& a) {
691  return Eigen::Matrix<Type, 4, 1>(
692  static_cast<Type>(a[0]), static_cast<Type>(a[1]),
693  static_cast<Type>(a[2]), static_cast<Type>(a[3]));
694  }
695 
697  inline Tuple4Tpl(const Eigen::Matrix<float, 4, 1>& v) { *this = v; }
698  inline Tuple4Tpl(const Eigen::Matrix<double, 4, 1>& v) { *this = v; }
700  inline Tuple4Tpl& operator=(const Eigen::Matrix<float, 4, 1>& v) {
701  this->x = static_cast<Type>(v(0));
702  this->y = static_cast<Type>(v(1));
703  this->z = static_cast<Type>(v(2));
704  this->w = static_cast<Type>(v(3));
705  return *this;
706  }
707  inline Tuple4Tpl& operator=(const Eigen::Matrix<double, 4, 1>& v) {
708  this->x = static_cast<Type>(v(0));
709  this->y = static_cast<Type>(v(1));
710  this->z = static_cast<Type>(v(2));
711  this->w = static_cast<Type>(v(3));
712  return *this;
713  }
714 
716  inline Tuple4Tpl operator-() const {
717  Tuple4Tpl V(-x, -y, -z, -w);
718  return V;
719  }
721  inline Tuple4Tpl& operator+=(const Tuple4Tpl& v) {
722  x += v.x;
723  y += v.y;
724  z += v.z;
725  w += v.w;
726  return *this;
727  }
729  inline Tuple4Tpl& operator-=(const Tuple4Tpl& v) {
730  x -= v.x;
731  y -= v.y;
732  z -= v.z;
733  w -= v.w;
734  return *this;
735  }
737  inline Tuple4Tpl& operator*=(Type v) {
738  x *= v;
739  y *= v;
740  z *= v;
741  w *= v;
742  return *this;
743  }
745  inline Tuple4Tpl& operator/=(Type v) {
746  x /= v;
747  y /= v;
748  z /= v;
749  w /= v;
750  return *this;
751  }
753  inline Tuple4Tpl operator+(const Tuple4Tpl& v) const {
754  return Tuple4Tpl(x + v.x, y + v.y, z + v.z, w + v.w);
755  }
757  inline Tuple4Tpl operator-(const Tuple4Tpl& v) const {
758  return Tuple4Tpl(x - v.x, y - v.y, z - v.z, w - v.w);
759  }
761  inline Tuple4Tpl operator*(Type s) const {
762  return Tuple4Tpl(x * s, y * s, z * s, w * s);
763  }
765  inline Tuple4Tpl operator/(Type s) const {
766  return Tuple4Tpl(x / s, y / s, z / s, w / s);
767  }
768 
770  inline Type& operator[](unsigned i) { return u[i]; }
772  inline const Type& operator[](unsigned i) const { return u[i]; }
774  inline Type& operator()(unsigned i) { return u[i]; }
776  inline const Type& operator()(unsigned i) const { return u[i]; }
777 };
778 
781 
784 
787 
789 inline Vector3Tpl<float> operator*(float s, const Vector3Tpl<float>& v) {
790  return v * s;
791 }
792 // Multiplication of a 3D vector by a scalar (front) operator (double version)
793 inline Vector3Tpl<double> operator*(double s, const Vector3Tpl<double>& v) {
794  return v * s;
795 }
796 
799 
802 
805 
808 
809 #ifdef _MSC_VER
810 // Restore the default warning behavior
811 #pragma warning(default : 4201)
812 #endif
Vector3Tpl< float > operator*(float s, const Vector3Tpl< float > &v)
Multiplication of a 3D vector by a scalar (front) operator (float version)
Definition: CVGeom.h:789
int points
3-Tuple structure (templated version)
Definition: CVGeom.h:132
Tuple3Tpl & operator*=(Type v)
In-place multiplication (by a scalar) operator.
Definition: CVGeom.h:181
Tuple3Tpl & operator/=(Type v)
In-place division (by a scalar) operator.
Definition: CVGeom.h:188
Tuple3Tpl operator/(Type s) const
Division operator.
Definition: CVGeom.h:207
Tuple3Tpl operator*(Type s) const
Multiplication operator.
Definition: CVGeom.h:203
std::size_t rows() const
Definition: CVGeom.h:142
Tuple3Tpl operator-() const
Inverse operator.
Definition: CVGeom.h:162
Tuple3Tpl & operator+=(const Tuple3Tpl &v)
In-place addition operator.
Definition: CVGeom.h:167
Type y
Definition: CVGeom.h:137
Tuple3Tpl operator-(const Tuple3Tpl &v) const
Subtraction operator.
Definition: CVGeom.h:199
Tuple3Tpl()
Default constructor.
Definition: CVGeom.h:151
Tuple3Tpl operator+(const Tuple3Tpl &v) const
Addition operator.
Definition: CVGeom.h:195
Type u[3]
Definition: CVGeom.h:139
std::size_t dimensions() const
Definition: CVGeom.h:143
Tuple3Tpl(Type a, Type b, Type c)
Constructor from a triplet of values.
Definition: CVGeom.h:156
Type * data()
Definition: CVGeom.h:145
Type x
Definition: CVGeom.h:137
const Type * data() const
Definition: CVGeom.h:146
Type z
Definition: CVGeom.h:137
Tuple3Tpl(const Type p[])
Constructor from an array of 3 elements.
Definition: CVGeom.h:159
Tuple3Tpl & operator-=(const Tuple3Tpl &v)
In-place subtraction operator.
Definition: CVGeom.h:174
4-Tuple structure (templated version)
Definition: CVGeom.h:648
Tuple4Tpl(const Eigen::Matrix< float, 4, 1 > &v)
Copy Function.
Definition: CVGeom.h:697
Type x
Definition: CVGeom.h:653
Tuple4Tpl(Type a, Type b, Type c, Type d)
Constructor from a triplet of values.
Definition: CVGeom.h:672
Tuple4Tpl & operator=(const Eigen::Matrix< float, 4, 1 > &v)
Assignment Function.
Definition: CVGeom.h:700
Type y
Definition: CVGeom.h:653
Tuple4Tpl & operator=(const Eigen::Matrix< double, 4, 1 > &v)
Definition: CVGeom.h:707
Tuple4Tpl(const Eigen::Matrix< double, 4, 1 > &v)
Definition: CVGeom.h:698
Type u[4]
Definition: CVGeom.h:655
const Type * data() const
Definition: CVGeom.h:662
Tuple4Tpl operator/(Type s) const
Division operator.
Definition: CVGeom.h:765
Type & operator[](unsigned i)
Direct coordinate access.
Definition: CVGeom.h:770
Type & operator()(unsigned i)
Direct coordinate access.
Definition: CVGeom.h:774
static Tuple4Tpl fromArray(const Eigen::Matrix< Type, 4, 1 > &a)
Constructor from a Eigen vector.
Definition: CVGeom.h:679
Type w
Definition: CVGeom.h:653
Tuple4Tpl operator-(const Tuple4Tpl &v) const
Subtraction operator.
Definition: CVGeom.h:757
Tuple4Tpl operator*(Type s) const
Multiplication operator.
Definition: CVGeom.h:761
Tuple4Tpl & operator+=(const Tuple4Tpl &v)
In-place addition operator.
Definition: CVGeom.h:721
Tuple4Tpl operator-() const
Inverse operator.
Definition: CVGeom.h:716
static Eigen::Matrix< Type, 4, 1 > fromArray(const Tuple4Tpl< double > &a)
Definition: CVGeom.h:689
Tuple4Tpl(const Type p[])
Constructor from an array of 4 elements.
Definition: CVGeom.h:675
Tuple4Tpl operator+(const Tuple4Tpl &v) const
Addition operator.
Definition: CVGeom.h:753
Tuple4Tpl & operator-=(const Tuple4Tpl &v)
In-place subtraction operator.
Definition: CVGeom.h:729
static Eigen::Matrix< Type, 4, 1 > fromArray(const Tuple4Tpl< float > &a)
Definition: CVGeom.h:683
std::size_t dimensions() const
Definition: CVGeom.h:659
std::size_t rows() const
Definition: CVGeom.h:658
Tuple4Tpl()
Default constructor.
Definition: CVGeom.h:667
const Type & operator()(unsigned i) const
Direct coordinate access (const)
Definition: CVGeom.h:776
Tuple4Tpl & operator*=(Type v)
In-place multiplication (by a scalar) operator.
Definition: CVGeom.h:737
Tuple4Tpl & operator/=(Type v)
In-place division (by a scalar) operator.
Definition: CVGeom.h:745
Type * data()
Definition: CVGeom.h:661
Type z
Definition: CVGeom.h:653
const Type & operator[](unsigned i) const
Direct coordinate access (const)
Definition: CVGeom.h:772
2D Vector
Definition: CVGeom.h:32
Vector2Tpl & operator-()
Inverse operator.
Definition: CVGeom.h:79
Vector2Tpl(Type _x, Type _y)
Constructor from a couple of coordinates.
Definition: CVGeom.h:58
Type dot(const Vector2Tpl &v) const
Dot product.
Definition: CVGeom.h:71
Vector2Tpl & operator+=(const Vector2Tpl &v)
In-place addition operator.
Definition: CVGeom.h:85
const Type * data() const
Definition: CVGeom.h:45
Vector2Tpl & operator*=(Type v)
In-place multiplication (by a scalar) operator.
Definition: CVGeom.h:97
Vector2Tpl & operator-=(const Vector2Tpl &v)
In-place subtraction operator.
Definition: CVGeom.h:91
Type & operator[](unsigned i)
Direct coordinate access.
Definition: CVGeom.h:125
std::size_t dimensions() const
Definition: CVGeom.h:42
Vector2Tpl operator*(Type s) const
Multiplication operator.
Definition: CVGeom.h:117
Type x
Definition: CVGeom.h:36
Vector2Tpl operator+(const Vector2Tpl &v) const
Addition operator.
Definition: CVGeom.h:109
Type norm() const
Returns vector norm.
Definition: CVGeom.h:63
Vector2Tpl operator/(Type s) const
Division operator.
Definition: CVGeom.h:121
Type cross(const Vector2Tpl &v) const
Cross product.
Definition: CVGeom.h:76
void normalize()
Sets vector norm to unity.
Definition: CVGeom.h:65
const Type & operator[](unsigned i) const
Direct coordinate access (const)
Definition: CVGeom.h:127
Vector2Tpl operator-(const Vector2Tpl &v) const
Subtraction operator.
Definition: CVGeom.h:113
Type * data()
Definition: CVGeom.h:44
std::size_t rows() const
Definition: CVGeom.h:41
Vector2Tpl & operator/=(Type v)
In-place division (by a scalar) operator.
Definition: CVGeom.h:103
Vector2Tpl(Type s=0)
Default constructor.
Definition: CVGeom.h:51
Type norm2() const
Returns vector square norm.
Definition: CVGeom.h:61
Type y
Definition: CVGeom.h:36
Type u[2]
Definition: CVGeom.h:38
3D Vector (templated version)
Definition: CVGeom.h:223
Vector3Tpl(const Eigen::Matrix< double, 3, 1 > &v)
Definition: CVGeom.h:328
void normalize()
Sets vector norm to unity.
Definition: CVGeom.h:428
static void vmultiply(Type p[], Type s)
Definition: CVGeom.h:515
Vector3Tpl operator-(const Vector3Tpl &v) const
Subtraction operator.
Definition: CVGeom.h:477
double normd() const
Returns vector norm (forces double precision output)
Definition: CVGeom.h:426
static void vset(Type p[], Type x, Type y, Type z)
Definition: CVGeom.h:539
Type dot(const Vector3Tpl &v) const
Dot product.
Definition: CVGeom.h:408
Type norm2() const
Returns vector square norm.
Definition: CVGeom.h:417
Vector3Tpl(const Eigen::Matrix< float, 3, 1 > &v)
Copy Function.
Definition: CVGeom.h:327
Vector3Tpl & operator-=(const Vector3Tpl &v)
In-place subtraction operator.
Definition: CVGeom.h:452
double norm2d() const
Returns vector square norm (forces double precision output)
Definition: CVGeom.h:419
static Eigen::Matrix< Type, 3, 1 > fromArray(const Vector3Tpl< float > &a)
Definition: CVGeom.h:288
Vector3Tpl & operator*=(const Eigen::Matrix< double, 3, 1 > &v)
In-place product operator.
Definition: CVGeom.h:360
Vector3Tpl orthogonal() const
Returns a normalized vector which is orthogonal to this one.
Definition: CVGeom.h:433
Vector3Tpl< PointCoordinateType > toPC() const
Definition: CVGeom.h:263
Vector3Tpl(const Vector2Tpl< Type > &t2D, Type c)
Constructor from a 2D vector (and a third value)
Definition: CVGeom.h:247
Type prod() const
x,y,z product
Definition: CVGeom.h:406
const Type & operator[](unsigned i) const
Direct coordinate access (const)
Definition: CVGeom.h:495
static Vector3Tpl fromArray(const Eigen::Matrix< double, 3, 1 > &a)
Constructor from a Eigen vector.
Definition: CVGeom.h:284
static Type vnorm(const Type p[])
Definition: CVGeom.h:594
Vector3Tpl & operator*=(Type v)
In-place multiplication (by a scalar) operator.
Definition: CVGeom.h:459
static Type vdistance(const Type p[], const Type q[])
Definition: CVGeom.h:598
Type & operator()(unsigned i)
Direct coordinate access.
Definition: CVGeom.h:397
static void vorthogonal(const Type p[], Type q[])
Definition: CVGeom.h:605
static void vmultiply(const Type p[], Type s, Type r[])
Definition: CVGeom.h:510
static void vdivide(Type p[], Type s)
Definition: CVGeom.h:505
Vector3Tpl operator-() const
Inverse operator.
Definition: CVGeom.h:440
static double vangle_radd(const Type p[], const Type q[])
Definition: CVGeom.h:635
Vector3Tpl(Type _x, Type _y, Type _z)
Constructor from a triplet of coordinates.
Definition: CVGeom.h:240
Type norm() const
Returns vector norm.
Definition: CVGeom.h:424
static Type vdistance2(const Type p[], const Type q[])
Definition: CVGeom.h:581
static std::vector< Vector3Tpl > fromArrayContainer(const std::vector< Eigen::Matrix< double, 3, 1 >> &container)
Definition: CVGeom.h:314
const Type & operator()(unsigned i) const
Direct coordinate access (const)
Definition: CVGeom.h:399
static double vnormd(const Type p[])
Definition: CVGeom.h:595
Vector3Tpl & operator=(const Eigen::Matrix< float, 3, 1 > &v)
Definition: CVGeom.h:337
Vector3Tpl cross(const Vector3Tpl &v) const
Cross product.
Definition: CVGeom.h:412
Vector3Tpl & operator+=(const Vector3Tpl &v)
In-place addition operator.
Definition: CVGeom.h:445
Vector3Tpl< double > toDouble() const
Cast operator to a double vector (explicit call version)
Definition: CVGeom.h:255
static Vector3Tpl fromArray(const int a[3])
Constructor from an int array.
Definition: CVGeom.h:268
static std::vector< Eigen::Matrix< double, 3, 1 > > fromArrayContainer(const std::vector< Vector3Tpl< Type >> &container)
Definition: CVGeom.h:301
static double vdistanced(const Type p[], const Type q[])
Definition: CVGeom.h:601
double angle_radd(const Vector3Tpl &v) const
Definition: CVGeom.h:498
Vector3Tpl operator*(Type s) const
Multiplication operator.
Definition: CVGeom.h:481
Vector3Tpl operator/(Type s) const
Division operator.
Definition: CVGeom.h:485
static void vsubstract(const Type p[], const Type q[], Type r[])
Definition: CVGeom.h:550
static void vadd(const Type p[], const Type q[], Type r[])
Definition: CVGeom.h:544
static void vcopy(const Type p[], Type q[])
Definition: CVGeom.h:533
static void vnormalize(Type p[])
Definition: CVGeom.h:569
Vector3Tpl & operator=(const Eigen::Matrix< double, 3, 1 > &v)
Assignment Function.
Definition: CVGeom.h:331
Vector3Tpl operator+(const Vector3Tpl &v) const
Addition operator.
Definition: CVGeom.h:473
static void vcombination(Type a, const Type p[], Type b, const Type q[], Type r[])
Definition: CVGeom.h:555
static Vector3Tpl fromArray(const float a[3])
Constructor from a float array.
Definition: CVGeom.h:273
Type maxCoeff() const
x,y,z maximum
Definition: CVGeom.h:402
static double vdotd(const Type p[], const Type q[])
Definition: CVGeom.h:523
friend std::ostream & operator<<(std::ostream &stream, const Vector3Tpl &v)
Definition: CVGeom.h:389
Vector3Tpl & operator-=(const Eigen::Matrix< double, 3, 1 > &v)
In-place subtraction operator.
Definition: CVGeom.h:352
Vector3Tpl(const Type p[])
Constructor from an array of 3 elements.
Definition: CVGeom.h:244
static Type vdot(const Type p[], const Type q[])
Definition: CVGeom.h:520
Vector3Tpl< float > toFloat() const
Definition: CVGeom.h:258
Type operator&&(const Vector3Tpl &v) const
Dot product operator.
Definition: CVGeom.h:491
static void vset(Type p[], Type s)
Definition: CVGeom.h:538
static Vector3Tpl fromArray(const double a[3])
Constructor from a double array.
Definition: CVGeom.h:278
Vector3Tpl operator*(const Vector3Tpl &v) const
Cross product operator.
Definition: CVGeom.h:489
Vector3Tpl & operator+=(const Eigen::Matrix< double, 3, 1 > &v)
In-place addition operator.
Definition: CVGeom.h:345
Vector3Tpl operator*(const Eigen::Matrix< double, 3, 1 > &v) const
Cross product operator.
Definition: CVGeom.h:380
Type operator&&(const Eigen::Matrix< double, 3, 1 > &v) const
Dot product operator.
Definition: CVGeom.h:385
static Eigen::Matrix< Type, 3, 1 > fromArray(const Vector3Tpl< double > &a)
Definition: CVGeom.h:294
Vector3Tpl operator+(const Eigen::Matrix< double, 3, 1 > &v) const
Addition operator.
Definition: CVGeom.h:373
static void vdivide(const Type p[], Type s, Type r[])
Definition: CVGeom.h:500
static Type vangle_rad(const Type p[], const Type q[])
Definition: CVGeom.h:624
Vector3Tpl()
Default constructor.
Definition: CVGeom.h:235
Type angle_rad(const Vector3Tpl &v) const
Returns the angle to another vector (in radians - in [0, pi].
Definition: CVGeom.h:497
static Type vnorm2(const Type p[])
Definition: CVGeom.h:573
static void vcombination(const Type p[], Type b, const Type q[], Type r[])
Definition: CVGeom.h:561
Vector3Tpl operator-(const Eigen::Matrix< double, 3, 1 > &v) const
Subtraction operator.
Definition: CVGeom.h:366
static double vnorm2d(const Type p[])
Definition: CVGeom.h:576
static void vcross(const Type p[], const Type q[], Type r[])
Definition: CVGeom.h:528
static double vdistance2d(const Type p[], const Type q[])
Definition: CVGeom.h:586
Vector3Tpl & operator/=(Type v)
In-place division (by a scalar) operator.
Definition: CVGeom.h:466
Type & operator[](unsigned i)
Direct coordinate access.
Definition: CVGeom.h:493
int min(int a, int b)
Definition: cutil_math.h:53
__host__ __device__ int2 abs(int2 v)
Definition: cutil_math.h:1267
int max(int a, int b)
Definition: cutil_math.h:48
QTextStream & endl(QTextStream &stream)
Definition: QtCompat.h:718