27 #pragma warning(disable : 4201)
31 template <
typename Type>
44 inline Type*
data() {
return u; }
45 inline const Type*
data()
const {
return u; }
61 inline Type
norm2()
const {
return (
x *
x) + (
y *
y); }
63 inline Type
norm()
const {
return std::sqrt(
norm2()); }
67 if (n > 0) *
this /= std::sqrt(n);
131 template <
class Type>
146 inline const Type*
data()
const {
return u; }
159 inline explicit Tuple3Tpl(
const Type p[]) :
x(p[0]),
y(p[1]),
z(p[2]) {}
222 template <
typename Type>
269 return Vector3Tpl(
static_cast<Type
>(a[0]),
static_cast<Type
>(a[1]),
270 static_cast<Type
>(a[2]));
274 return Vector3Tpl(
static_cast<Type
>(a[0]),
static_cast<Type
>(a[1]),
275 static_cast<Type
>(a[2]));
279 return Vector3Tpl(
static_cast<Type
>(a[0]),
static_cast<Type
>(a[1]),
280 static_cast<Type
>(a[2]));
285 return Vector3Tpl(
static_cast<Type
>(a[0]),
static_cast<Type
>(a[1]),
286 static_cast<Type
>(a[2]));
290 return Eigen::Matrix<Type, 3, 1>(
static_cast<Type
>(a[0]),
291 static_cast<Type
>(a[1]),
292 static_cast<Type
>(a[2]));
296 return Eigen::Matrix<Type, 3, 1>(
static_cast<Type
>(a[0]),
297 static_cast<Type
>(a[1]),
298 static_cast<Type
>(a[2]));
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]));
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) {
320 static_cast<Type
>(container[i](1)),
321 static_cast<Type
>(container[i](2)));
327 inline Vector3Tpl(
const Eigen::Matrix<float, 3, 1>& v) { *
this = v; }
328 inline Vector3Tpl(
const Eigen::Matrix<double, 3, 1>& v) { *
this = 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));
338 this->
x =
static_cast<Type
>(v(0));
339 this->
y =
static_cast<Type
>(v(1));
340 this->
z =
static_cast<Type
>(v(2));
346 x +=
static_cast<Type
>(v(0));
347 y +=
static_cast<Type
>(v(1));
348 z +=
static_cast<Type
>(v(2));
353 x -=
static_cast<Type
>(v(0));
354 y -=
static_cast<Type
>(v(1));
355 z -=
static_cast<Type
>(v(2));
368 static_cast<Type
>(
y - v(1)),
369 static_cast<Type
>(
z - v(2)));
375 static_cast<Type
>(
y + v(1)),
376 static_cast<Type
>(
z + v(2)));
385 inline Type
operator&&(
const Eigen::Matrix<double, 3, 1>& v)
const {
391 stream <<
"[x->" << v.
x <<
", y->" << v.
y <<
", z->" << v.
x <<
"]"
403 return x >
y ? (
x >
z ?
x :
z) : (
y >
z ?
y :
z);
409 return x * v.
x +
y * v.
y +
z * v.
z;
414 (
x * v.
y) - (
y * v.
x));
420 return static_cast<double>(
x) *
x +
static_cast<double>(
y) *
y +
421 static_cast<double>(
z) *
z;
424 inline Type
norm()
const {
return static_cast<Type
>(std::sqrt(
norm2d())); }
430 if (n > 0) *
this /=
static_cast<Type
>(std::sqrt(n));
500 static inline void vdivide(
const Type p[], Type s, Type r[]) {
505 static inline void vdivide(Type p[], Type s) {
510 static inline void vmultiply(
const Type p[], Type s, Type r[]) {
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]);
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]);
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]);
533 static inline void vcopy(
const Type p[], Type q[]) {
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) {
544 static inline void vadd(
const Type p[],
const Type q[], Type r[]) {
550 static inline void vsubstract(
const Type p[],
const Type q[], Type r[]) {
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]);
565 r[0] = p[0] + (b * q[0]);
566 r[1] = p[1] + (b * q[1]);
567 r[2] = p[2] + (b * q[2]);
571 if (n > 0)
vdivide(p, std::sqrt(n));
573 static inline Type
vnorm2(
const Type p[]) {
574 return (p[0] * p[0]) + (p[1] * p[1]) + (p[2] * p[2]);
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]);
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]));
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]));
594 static inline Type
vnorm(
const Type p[]) {
return std::sqrt(
vnorm2(p)); }
595 static inline double vnormd(
const Type p[]) {
598 static inline Type
vdistance(
const Type p[],
const Type q[]) {
601 static inline double vdistanced(
const Type p[],
const Type q[]) {
626 if (productNorm < std::numeric_limits<Type>::epsilon()) {
627 return std::numeric_limits<Type>::quiet_NaN();
630 Type cosAngle =
vdot(p, q) / productNorm;
632 static_cast<Type
>(-1.0)));
637 if (productNorm < std::numeric_limits<double>::epsilon()) {
638 return std::numeric_limits<double>::quiet_NaN();
641 double cosAngle =
vdotd(p, q) / productNorm;
647 template <
class Type>
662 inline const Type*
data()
const {
return u; }
672 inline Tuple4Tpl(Type a, Type b, Type c, Type d) :
x(a),
y(b),
z(c),
w(d) {}
676 :
x(p[0]),
y(p[1]),
z(p[2]),
w(p[3]) {}
680 return Tuple4Tpl(
static_cast<Type
>(a[0]),
static_cast<Type
>(a[1]),
681 static_cast<Type
>(a[2]),
static_cast<Type
>(a[3]));
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]));
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]));
697 inline Tuple4Tpl(
const Eigen::Matrix<float, 4, 1>& v) { *
this = v; }
698 inline Tuple4Tpl(
const Eigen::Matrix<double, 4, 1>& v) { *
this = 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));
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));
811 #pragma warning(default : 4201)
Vector3Tpl< float > operator*(float s, const Vector3Tpl< float > &v)
Multiplication of a 3D vector by a scalar (front) operator (float version)
3-Tuple structure (templated version)
Tuple3Tpl & operator*=(Type v)
In-place multiplication (by a scalar) operator.
Tuple3Tpl & operator/=(Type v)
In-place division (by a scalar) operator.
Tuple3Tpl operator/(Type s) const
Division operator.
Tuple3Tpl operator*(Type s) const
Multiplication operator.
Tuple3Tpl operator-() const
Inverse operator.
Tuple3Tpl & operator+=(const Tuple3Tpl &v)
In-place addition operator.
Tuple3Tpl operator-(const Tuple3Tpl &v) const
Subtraction operator.
Tuple3Tpl()
Default constructor.
Tuple3Tpl operator+(const Tuple3Tpl &v) const
Addition operator.
std::size_t dimensions() const
Tuple3Tpl(Type a, Type b, Type c)
Constructor from a triplet of values.
const Type * data() const
Tuple3Tpl(const Type p[])
Constructor from an array of 3 elements.
Tuple3Tpl & operator-=(const Tuple3Tpl &v)
In-place subtraction operator.
4-Tuple structure (templated version)
Tuple4Tpl(const Eigen::Matrix< float, 4, 1 > &v)
Copy Function.
Tuple4Tpl(Type a, Type b, Type c, Type d)
Constructor from a triplet of values.
Tuple4Tpl & operator=(const Eigen::Matrix< float, 4, 1 > &v)
Assignment Function.
Tuple4Tpl & operator=(const Eigen::Matrix< double, 4, 1 > &v)
Tuple4Tpl(const Eigen::Matrix< double, 4, 1 > &v)
const Type * data() const
Tuple4Tpl operator/(Type s) const
Division operator.
Type & operator[](unsigned i)
Direct coordinate access.
Type & operator()(unsigned i)
Direct coordinate access.
static Tuple4Tpl fromArray(const Eigen::Matrix< Type, 4, 1 > &a)
Constructor from a Eigen vector.
Tuple4Tpl operator-(const Tuple4Tpl &v) const
Subtraction operator.
Tuple4Tpl operator*(Type s) const
Multiplication operator.
Tuple4Tpl & operator+=(const Tuple4Tpl &v)
In-place addition operator.
Tuple4Tpl operator-() const
Inverse operator.
static Eigen::Matrix< Type, 4, 1 > fromArray(const Tuple4Tpl< double > &a)
Tuple4Tpl(const Type p[])
Constructor from an array of 4 elements.
Tuple4Tpl operator+(const Tuple4Tpl &v) const
Addition operator.
Tuple4Tpl & operator-=(const Tuple4Tpl &v)
In-place subtraction operator.
static Eigen::Matrix< Type, 4, 1 > fromArray(const Tuple4Tpl< float > &a)
std::size_t dimensions() const
Tuple4Tpl()
Default constructor.
const Type & operator()(unsigned i) const
Direct coordinate access (const)
Tuple4Tpl & operator*=(Type v)
In-place multiplication (by a scalar) operator.
Tuple4Tpl & operator/=(Type v)
In-place division (by a scalar) operator.
const Type & operator[](unsigned i) const
Direct coordinate access (const)
Vector2Tpl & operator-()
Inverse operator.
Vector2Tpl(Type _x, Type _y)
Constructor from a couple of coordinates.
Type dot(const Vector2Tpl &v) const
Dot product.
Vector2Tpl & operator+=(const Vector2Tpl &v)
In-place addition operator.
const Type * data() const
Vector2Tpl & operator*=(Type v)
In-place multiplication (by a scalar) operator.
Vector2Tpl & operator-=(const Vector2Tpl &v)
In-place subtraction operator.
Type & operator[](unsigned i)
Direct coordinate access.
std::size_t dimensions() const
Vector2Tpl operator*(Type s) const
Multiplication operator.
Vector2Tpl operator+(const Vector2Tpl &v) const
Addition operator.
Type norm() const
Returns vector norm.
Vector2Tpl operator/(Type s) const
Division operator.
Type cross(const Vector2Tpl &v) const
Cross product.
void normalize()
Sets vector norm to unity.
const Type & operator[](unsigned i) const
Direct coordinate access (const)
Vector2Tpl operator-(const Vector2Tpl &v) const
Subtraction operator.
Vector2Tpl & operator/=(Type v)
In-place division (by a scalar) operator.
Vector2Tpl(Type s=0)
Default constructor.
Type norm2() const
Returns vector square norm.
3D Vector (templated version)
Vector3Tpl(const Eigen::Matrix< double, 3, 1 > &v)
void normalize()
Sets vector norm to unity.
static void vmultiply(Type p[], Type s)
Vector3Tpl operator-(const Vector3Tpl &v) const
Subtraction operator.
double normd() const
Returns vector norm (forces double precision output)
static void vset(Type p[], Type x, Type y, Type z)
Type dot(const Vector3Tpl &v) const
Dot product.
Type norm2() const
Returns vector square norm.
Vector3Tpl(const Eigen::Matrix< float, 3, 1 > &v)
Copy Function.
Vector3Tpl & operator-=(const Vector3Tpl &v)
In-place subtraction operator.
double norm2d() const
Returns vector square norm (forces double precision output)
static Eigen::Matrix< Type, 3, 1 > fromArray(const Vector3Tpl< float > &a)
Vector3Tpl & operator*=(const Eigen::Matrix< double, 3, 1 > &v)
In-place product operator.
Vector3Tpl orthogonal() const
Returns a normalized vector which is orthogonal to this one.
Vector3Tpl< PointCoordinateType > toPC() const
Vector3Tpl(const Vector2Tpl< Type > &t2D, Type c)
Constructor from a 2D vector (and a third value)
Type prod() const
x,y,z product
const Type & operator[](unsigned i) const
Direct coordinate access (const)
static Vector3Tpl fromArray(const Eigen::Matrix< double, 3, 1 > &a)
Constructor from a Eigen vector.
static Type vnorm(const Type p[])
Vector3Tpl & operator*=(Type v)
In-place multiplication (by a scalar) operator.
static Type vdistance(const Type p[], const Type q[])
Type & operator()(unsigned i)
Direct coordinate access.
static void vorthogonal(const Type p[], Type q[])
static void vmultiply(const Type p[], Type s, Type r[])
static void vdivide(Type p[], Type s)
Vector3Tpl operator-() const
Inverse operator.
static double vangle_radd(const Type p[], const Type q[])
Vector3Tpl(Type _x, Type _y, Type _z)
Constructor from a triplet of coordinates.
Type norm() const
Returns vector norm.
static Type vdistance2(const Type p[], const Type q[])
static std::vector< Vector3Tpl > fromArrayContainer(const std::vector< Eigen::Matrix< double, 3, 1 >> &container)
const Type & operator()(unsigned i) const
Direct coordinate access (const)
static double vnormd(const Type p[])
Vector3Tpl & operator=(const Eigen::Matrix< float, 3, 1 > &v)
Vector3Tpl cross(const Vector3Tpl &v) const
Cross product.
Vector3Tpl & operator+=(const Vector3Tpl &v)
In-place addition operator.
Vector3Tpl< double > toDouble() const
Cast operator to a double vector (explicit call version)
static Vector3Tpl fromArray(const int a[3])
Constructor from an int array.
static std::vector< Eigen::Matrix< double, 3, 1 > > fromArrayContainer(const std::vector< Vector3Tpl< Type >> &container)
static double vdistanced(const Type p[], const Type q[])
double angle_radd(const Vector3Tpl &v) const
Vector3Tpl operator*(Type s) const
Multiplication operator.
Vector3Tpl operator/(Type s) const
Division operator.
static void vsubstract(const Type p[], const Type q[], Type r[])
static void vadd(const Type p[], const Type q[], Type r[])
static void vcopy(const Type p[], Type q[])
static void vnormalize(Type p[])
Vector3Tpl & operator=(const Eigen::Matrix< double, 3, 1 > &v)
Assignment Function.
Vector3Tpl operator+(const Vector3Tpl &v) const
Addition operator.
static void vcombination(Type a, const Type p[], Type b, const Type q[], Type r[])
static Vector3Tpl fromArray(const float a[3])
Constructor from a float array.
Type maxCoeff() const
x,y,z maximum
static double vdotd(const Type p[], const Type q[])
friend std::ostream & operator<<(std::ostream &stream, const Vector3Tpl &v)
Vector3Tpl & operator-=(const Eigen::Matrix< double, 3, 1 > &v)
In-place subtraction operator.
Vector3Tpl(const Type p[])
Constructor from an array of 3 elements.
static Type vdot(const Type p[], const Type q[])
Vector3Tpl< float > toFloat() const
Type operator&&(const Vector3Tpl &v) const
Dot product operator.
static void vset(Type p[], Type s)
static Vector3Tpl fromArray(const double a[3])
Constructor from a double array.
Vector3Tpl operator*(const Vector3Tpl &v) const
Cross product operator.
Vector3Tpl & operator+=(const Eigen::Matrix< double, 3, 1 > &v)
In-place addition operator.
Vector3Tpl operator*(const Eigen::Matrix< double, 3, 1 > &v) const
Cross product operator.
Type operator&&(const Eigen::Matrix< double, 3, 1 > &v) const
Dot product operator.
static Eigen::Matrix< Type, 3, 1 > fromArray(const Vector3Tpl< double > &a)
Vector3Tpl operator+(const Eigen::Matrix< double, 3, 1 > &v) const
Addition operator.
static void vdivide(const Type p[], Type s, Type r[])
static Type vangle_rad(const Type p[], const Type q[])
Vector3Tpl()
Default constructor.
Type angle_rad(const Vector3Tpl &v) const
Returns the angle to another vector (in radians - in [0, pi].
static Type vnorm2(const Type p[])
static void vcombination(const Type p[], Type b, const Type q[], Type r[])
Vector3Tpl operator-(const Eigen::Matrix< double, 3, 1 > &v) const
Subtraction operator.
static double vnorm2d(const Type p[])
static void vcross(const Type p[], const Type q[], Type r[])
static double vdistance2d(const Type p[], const Type q[])
Vector3Tpl & operator/=(Type v)
In-place division (by a scalar) operator.
Type & operator[](unsigned i)
Direct coordinate access.
__host__ __device__ int2 abs(int2 v)
QTextStream & endl(QTextStream &stream)