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 
26  const TPoint3D& basePoint, const TVector3D& directorVector)
27 {
28  TLine3D l;
29  l.pBase = basePoint;
30  l.director = directorVector;
31  return l;
32 }
33 
35 {
36  return TLine3D(p1, p2);
37 }
38 
39 bool TLine3D::contains(const TPoint3D& point) const
40 {
41  double dx = point.x - pBase.x;
42  double dy = point.y - pBase.y;
43  double dz = point.z - pBase.z;
44  if (std::abs(dx) < getEpsilon() && std::abs(dy) < getEpsilon() &&
45  std::abs(dz) < getEpsilon())
46  return true;
47  // dx dy dz
48  // if -----------=-----------=-----------, point is inside the line.
49  // director[0] director[1] director[2]
50  return (std::abs(dx * director[1] - dy * director[0]) < getEpsilon()) &&
51  (std::abs(dx * director[2] - dz * director[0]) < getEpsilon()) &&
52  (std::abs(dy * director[2] - dz * director[1]) < getEpsilon());
53 }
54 double TLine3D::distance(const TPoint3D& point) const
55 {
56  // Let d be line's base point minus the argument. Then,
57  // d·director/(|d|·|director|) equals both vector's cosine.
58  // So, d·director/|director| equals d's projection over director. Then,
59  // distance is sqrt(|d|²-(d·director/|director|)²).
60  double d[3] = {point.x - pBase.x, point.y - pBase.y, point.z - pBase.z};
61  double dv = 0, d2 = 0, v2 = 0;
62  for (size_t i = 0; i < 3; i++)
63  {
64  dv += d[i] * director[i];
65  d2 += d[i] * d[i];
66  v2 += director[i] * director[i];
67  }
68  return sqrt(d2 - (dv * dv) / v2);
69 }
71 {
72  const double norm = director.norm();
73  ASSERT_(norm > 0);
74  director *= 1.0 / norm;
75 }
76 TLine3D::TLine3D(const TPoint3D& p1, const TPoint3D& p2)
77 {
78  if (std::abs(math::distance(p1, p2)) < getEpsilon())
79  throw std::logic_error("Both points are the same");
80  pBase = p1;
81  director[0] = p2.x - p1.x;
82  director[1] = p2.y - p1.y;
83  director[2] = p2.z - p1.z;
84 }
86 {
87  pBase = s.point1;
88  director[0] = s.point2.x - s.point1.x;
89  director[1] = s.point2.y - s.point1.y;
90  director[2] = s.point2.z - s.point1.z;
91 }
93 {
94  director[0] = -l.coefs[1];
95  director[1] = l.coefs[0];
96  director[2] = 0;
97  // We assume that either l.coefs[0] or l.coefs[1] is not null. Respectively,
98  // either y or x can be used as free cordinate.
99  if (std::abs(l.coefs[0]) >= getEpsilon())
100  {
101  pBase.x = -l.coefs[2] / l.coefs[0];
102  pBase.y = 0;
103  }
104  else
105  {
106  pBase.x = 0;
107  pBase.y = -l.coefs[1] / l.coefs[0];
108  }
109  pBase.z = 0;
110 }
111 
114 {
115  return in >> l.pBase >> l.director[0] >> l.director[1] >> l.director[2];
116 }
119 {
120  return out << l.pBase << l.director[0] << l.director[1] << l.director[2];
121 }
mrpt::math::TLine3D::generate2DObject
void generate2DObject(TLine2D &l) const
Project into 2D space, discarding the Z coordinate.
Definition: TLine3D.cpp:23
ops_containers.h
geometry.h
mrpt::math::TPoint3D_< double >
mrpt::math::operator<<
mrpt::serialization::CArchive & operator<<(mrpt::serialization::CArchive &s, const CVectorFloat &a)
Definition: math.cpp:626
mrpt::math::TLine3D::pBase
TPoint3D pBase
Base point.
Definition: TLine3D.h:47
mrpt::math::TPoint3D_data::z
T z
Definition: TPoint3D.h:29
math-precomp.h
out
mrpt::vision::TStereoCalibResults out
Definition: chessboard_stereo_camera_calib_unittest.cpp:25
mrpt::math::distance
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1807
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::serialization::CArchive
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
mrpt::math::TSegment3D::point1
TPoint3D point1
origin point
Definition: TSegment3D.h:23
mrpt::math::TLine3D::contains
bool contains(const TPoint3D &point) const
Check whether a point is inside the line.
Definition: TLine3D.cpp:39
TLine2D.h
mrpt::math::TPoint3D_::norm
T norm() const
Point norm: |v| = sqrt(x^2+y^2+z^2)
Definition: TPoint3D.h:150
mrpt::math::norm
CONTAINER::Scalar norm(const CONTAINER &v)
Definition: ops_containers.h:133
mrpt::math::TSegment3D::point2
TPoint3D point2
final point
Definition: TSegment3D.h:24
mrpt::math::TLine3D::FromPointAndDirector
static TLine3D FromPointAndDirector(const TPoint3D &basePoint, const TVector3D &directorVector)
Static constructor from a point and a director vector.
Definition: TLine3D.cpp:25
mrpt::math::getEpsilon
double getEpsilon()
Gets the value of the geometric epsilon (default = 1e-5)
Definition: geometry.cpp:34
mrpt::math::TLine2D::coefs
std::array< double, 3 > coefs
Line coefficients, stored as an array: .
Definition: TLine2D.h:52
mrpt::math::TLine3D::TLine3D
TLine3D()=default
Fast default constructor.
mrpt::math::TLine3D::FromTwoPoints
static TLine3D FromTwoPoints(const TPoint3D &p1, const TPoint3D &p2)
Static constructor from two points.
Definition: TLine3D.cpp:34
mrpt::math::TSegment3D
3D segment, consisting of two points.
Definition: TSegment3D.h:20
mrpt::math::TLine3D::distance
double distance(const TPoint3D &point) const
Distance between the line and a point.
Definition: TLine3D.cpp:54
TLine3D.h
mrpt::math::TPoint3D_data::y
T y
Definition: TPoint3D.h:29
mrpt::math::TPoint3D_data::x
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::math::TLine2D
2D line without bounds, represented by its equation .
Definition: TLine2D.h:19
mrpt::math::TLine3D
3D line, represented by a base point and a director vector.
Definition: TLine3D.h:19
mrpt::math::operator>>
mrpt::serialization::CArchive & operator>>(mrpt::serialization::CArchive &in, CMatrixD::Ptr &pObj)
CArchive.h
mrpt::math::TLine3D::unitarize
void unitarize()
Unitarize director vector.
Definition: TLine3D.cpp:70
mrpt::math::TLine3D::director
TVector3D director
Director vector.
Definition: TLine3D.h:49
epsilon.h



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Fri Jul 17 08:43:33 UTC 2020