MRPT  2.0.4
TLine3D.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9
10 #include "math-precomp.h" // Precompiled headers
11
12 #include <mrpt/math/TLine2D.h>
13 #include <mrpt/math/TLine3D.h>
14 #include <mrpt/math/epsilon.h>
15 #include <mrpt/math/geometry.h> // distance()
16 #include <mrpt/math/ops_containers.h> // squareNorm()
17 #include <mrpt/serialization/CArchive.h> // impl of << operator
18
19 using namespace mrpt::math;
20
21 static_assert(std::is_trivially_copyable_v<TLine3D>);
22
23 void TLine3D::generate2DObject(TLine2D& l) const { l = TLine2D(*this); }
24
25 bool TLine3D::contains(const TPoint3D& point) const
26 {
27  double dx = point.x - pBase.x;
28  double dy = point.y - pBase.y;
29  double dz = point.z - pBase.z;
30  if (std::abs(dx) < getEpsilon() && std::abs(dy) < getEpsilon() &&
31  std::abs(dz) < getEpsilon())
32  return true;
33  // dx dy dz
34  // if -----------=-----------=-----------, point is inside the line.
35  // director[0] director[1] director[2]
36  return (std::abs(dx * director[1] - dy * director[0]) < getEpsilon()) &&
37  (std::abs(dx * director[2] - dz * director[0]) < getEpsilon()) &&
38  (std::abs(dy * director[2] - dz * director[1]) < getEpsilon());
39 }
40 double TLine3D::distance(const TPoint3D& point) const
41 {
42  // Let d be line's base point minus the argument. Then,
43  // d·director/(|d|·|director|) equals both vector's cosine.
44  // So, d·director/|director| equals d's projection over director. Then,
45  // distance is sqrt(|d|²-(d·director/|director|)²).
46  double d[3] = {point.x - pBase.x, point.y - pBase.y, point.z - pBase.z};
47  double dv = 0, d2 = 0, v2 = 0;
48  for (size_t i = 0; i < 3; i++)
49  {
50  dv += d[i] * director[i];
51  d2 += d[i] * d[i];
52  v2 += director[i] * director[i];
53  }
54  return sqrt(d2 - (dv * dv) / v2);
55 }
57 {
58  double s = sqrt(squareNorm<3, double>(director));
59  for (double& i : director) i /= s;
60 }
61 TLine3D::TLine3D(const TPoint3D& p1, const TPoint3D& p2)
62 {
63  if (std::abs(math::distance(p1, p2)) < getEpsilon())
64  throw std::logic_error("Both points are the same");
65  pBase = p1;
66  director[0] = p2.x - p1.x;
67  director[1] = p2.y - p1.y;
68  director[2] = p2.z - p1.z;
69 }
71 {
72  pBase = s.point1;
73  director[0] = s.point2.x - s.point1.x;
74  director[1] = s.point2.y - s.point1.y;
75  director[2] = s.point2.z - s.point1.z;
76 }
78 {
79  director[0] = -l.coefs[1];
80  director[1] = l.coefs[0];
81  director[2] = 0;
82  // We assume that either l.coefs[0] or l.coefs[1] is not null. Respectively,
83  // either y or x can be used as free cordinate.
84  if (std::abs(l.coefs[0]) >= getEpsilon())
85  {
86  pBase.x = -l.coefs[2] / l.coefs[0];
87  pBase.y = 0;
88  }
89  else
90  {
91  pBase.x = 0;
92  pBase.y = -l.coefs[1] / l.coefs[0];
93  }
94  pBase.z = 0;
95 }
96
99 {
100  return in >> l.pBase >> l.director[0] >> l.director[1] >> l.director[2];
101 }
104 {
105  return out << l.pBase << l.director[0] << l.director[1] << l.director[2];
106 }
This file implements several operations that operate element-wise on individual or pairs of container...
TPoint3D pBase
Base point.
Definition: TLine3D.h:23
void generate2DObject(TLine2D &l) const
Project into 2D space, discarding the Z coordinate.
Definition: TLine3D.cpp:23
mrpt::serialization::CArchive & operator>>(mrpt::serialization::CArchive &in, CMatrixD::Ptr &pObj)
TPoint3D point1
origin point
Definition: TSegment3D.h:23
This base provides a set of functions for maths stuff.
3D segment, consisting of two points.
Definition: TSegment3D.h:20
std::array< double, 3 > director
Director vector.
Definition: TLine3D.h:25
TPoint3D point2
final point
Definition: TSegment3D.h:24
bool contains(const TPoint3D &point) const
Check whether a point is inside the line.
Definition: TLine3D.cpp:25
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
std::array< double, 3 > coefs
Line coefficients, stored as an array: .
Definition: TLine2D.h:23
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
double getEpsilon()
Gets the value of the geometric epsilon (default = 1e-5)
Definition: geometry.cpp:34
mrpt::vision::TStereoCalibResults out
mrpt::serialization::CArchive & operator<<(mrpt::serialization::CArchive &s, const CVectorFloat &a)
Definition: math.cpp:626
void unitarize()
Unitarize director vector.
Definition: TLine3D.cpp:56
TLine3D()=default
Fast default constructor.
double distance(const TPoint3D &point) const
Distance between the line and a point.
Definition: TLine3D.cpp:40
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1807
3D line, represented by a base point and a director vector.
Definition: TLine3D.h:19
2D line without bounds, represented by its equation .
Definition: TLine2D.h:19

 Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 7ea9b7e81 Mon May 25 11:43:10 2020 +0200 at lun may 25 11:45:15 CEST 2020