MRPT  2.0.4
TPlane.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/TLine3D.h>
13 #include <mrpt/math/TPlane.h>
14 #include <mrpt/math/TPoint3D.h>
15 #include <mrpt/math/epsilon.h>
16 #include <mrpt/math/geometry.h> // getAngle()
17 #include <mrpt/math/ops_containers.h> // dotProduct()
18 #include <mrpt/serialization/CArchive.h> // impl of << operator
19 
20 using namespace mrpt::math;
21 
22 static_assert(std::is_trivially_copyable_v<TPlane>);
23 
24 double TPlane::evaluatePoint(const TPoint3D& point) const
25 {
26  return dotProduct<3, double>(coefs, point) + coefs[3];
27 }
28 bool TPlane::contains(const TPoint3D& point) const
29 {
30  return distance(point) < getEpsilon();
31 }
32 bool TPlane::contains(const TLine3D& line) const
33 {
34  if (!contains(line.pBase)) return false; // Base point must be contained
35  return std::abs(getAngle(*this, line)) <
36  getEpsilon(); // Plane's normal must be normal to director vector
37 }
38 double TPlane::distance(const TPoint3D& point) const
39 {
40  return std::abs(evaluatePoint(point)) / sqrt(squareNorm<3, double>(coefs));
41 }
42 double TPlane::distance(const TLine3D& line) const
43 {
44  if (std::abs(getAngle(*this, line)) >= getEpsilon())
45  return 0; // Plane crosses with line
46  else
47  return distance(line.pBase);
48 }
50 {
51  TVector3D v;
52  for (int i = 0; i < 3; i++) v[i] = coefs[i];
53  return v;
54 }
55 
57 {
58  TVector3D vec;
59  const double s = sqrt(squareNorm<3, double>(coefs));
61  const double k = 1.0 / s;
62  for (int i = 0; i < 3; i++) vec[i] = coefs[i] * k;
63  return vec;
64 }
65 
67 {
68  double s = sqrt(squareNorm<3, double>(coefs));
69  for (double& coef : coefs) coef /= s;
70 }
71 
72 // Returns a 6D pose such as its XY plane coincides with the plane
74 {
75  const TVector3D normal = getUnitaryNormalVector();
77  for (size_t i = 0; i < 3; i++)
78  if (std::abs(coefs[i]) >= getEpsilon())
79  {
80  AXIS(i, 3) = -coefs[3] / coefs[i];
81  break;
82  }
83  outPose.fromHomogeneousMatrix(AXIS);
84 }
86  const TPoint3D& center, TPose3D& pose) const
87 {
88  if (!contains(center))
89  throw std::logic_error("Base point is not in the plane.");
90  const TVector3D normal = getUnitaryNormalVector();
92  for (size_t i = 0; i < 3; i++) AXIS(i, 3) = center[i];
93  pose.fromHomogeneousMatrix(AXIS);
94 }
95 TPlane::TPlane(const TPoint3D& p1, const TPoint3D& p2, const TPoint3D& p3)
96 {
97  double dx1 = p2.x - p1.x;
98  double dy1 = p2.y - p1.y;
99  double dz1 = p2.z - p1.z;
100  double dx2 = p3.x - p1.x;
101  double dy2 = p3.y - p1.y;
102  double dz2 = p3.z - p1.z;
103  coefs[0] = dy1 * dz2 - dy2 * dz1;
104  coefs[1] = dz1 * dx2 - dz2 * dx1;
105  coefs[2] = dx1 * dy2 - dx2 * dy1;
106  if (std::abs(coefs[0]) < getEpsilon() &&
107  std::abs(coefs[1]) < getEpsilon() && std::abs(coefs[2]) < getEpsilon())
108  throw std::logic_error("Points are linearly dependent");
109  coefs[3] = -coefs[0] * p1.x - coefs[1] * p1.y - coefs[2] * p1.z;
110 }
111 TPlane::TPlane(const TPoint3D& p1, const TLine3D& r2)
112 {
113  double dx1 = p1.x - r2.pBase.x;
114  double dy1 = p1.y - r2.pBase.y;
115  double dz1 = p1.z - r2.pBase.z;
116  coefs[0] = dy1 * r2.director[2] - dz1 * r2.director[1];
117  coefs[1] = dz1 * r2.director[0] - dx1 * r2.director[2];
118  coefs[2] = dx1 * r2.director[1] - dy1 * r2.director[0];
119  if (std::abs(coefs[0]) < getEpsilon() &&
120  std::abs(coefs[1]) < getEpsilon() && std::abs(coefs[2]) < getEpsilon())
121  throw std::logic_error("Point is contained in the line");
122  coefs[3] = -coefs[0] * p1.x - coefs[1] * p1.y - coefs[2] * p1.z;
123 }
124 TPlane::TPlane(const TPoint3D& p1, const TVector3D& normal)
125 {
126  const double normal_norm = normal.norm();
127  ASSERT_ABOVE_(normal_norm, getEpsilon());
128 
129  // Ensure we have a unit vector:
130  const auto n = normal * (1. / normal_norm);
131  coefs[0] = n.x;
132  coefs[1] = n.y;
133  coefs[2] = n.z;
134  coefs[3] = -coefs[0] * p1.x - coefs[1] * p1.y - coefs[2] * p1.z;
135 }
136 TPlane::TPlane(const TLine3D& r1, const TLine3D& r2)
137 {
139  coefs[3] =
140  -coefs[0] * r1.pBase.x - coefs[1] * r1.pBase.y - coefs[2] * r1.pBase.z;
141  if (std::abs(coefs[0]) < getEpsilon() &&
142  std::abs(coefs[1]) < getEpsilon() && std::abs(coefs[2]) < getEpsilon())
143  {
144  // Lines are parallel
145  if (r1.contains(r2.pBase)) throw std::logic_error("Lines are the same");
146  // Use a line's director vector and both pBase's difference to create
147  // the plane.
148  double d[3];
149  for (size_t i = 0; i < 3; i++) d[i] = r1.pBase[i] - r2.pBase[i];
150  crossProduct3D(r1.director, d, coefs);
151  coefs[3] = -coefs[0] * r1.pBase.x - coefs[1] * r1.pBase.y -
152  coefs[2] * r1.pBase.z;
153  }
154  else if (std::abs(evaluatePoint(r2.pBase)) >= getEpsilon())
155  throw std::logic_error("Lines do not intersect");
156 }
157 
160 {
161  return in >> p.coefs[0] >> p.coefs[1] >> p.coefs[2] >> p.coefs[3];
162 }
165 {
166  return out << p.coefs[0] << p.coefs[1] << p.coefs[2] << p.coefs[3];
167 }
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
This file implements several operations that operate element-wise on individual or pairs of container...
void fromHomogeneousMatrix(const mrpt::math::CMatrixDouble44 &HG)
Definition: TPose3D.cpp:201
TPoint3D pBase
Base point.
Definition: TLine3D.h:23
TVector3D getUnitaryNormalVector() const
Get normal vector.
Definition: TPlane.cpp:56
void crossProduct3D(const T &v0, const U &v1, V &vOut)
Computes the cross product of two 3D vectors, returning a vector normal to both.
Definition: geometry.h:765
mrpt::serialization::CArchive & operator>>(mrpt::serialization::CArchive &in, CMatrixD::Ptr &pObj)
void getAsPose3D(mrpt::math::TPose3D &outPose) const
Definition: TPlane.cpp:73
This base provides a set of functions for maths stuff.
double evaluatePoint(const TPoint3D &point) const
Evaluate a point in the plane&#39;s equation.
Definition: TPlane.cpp:24
std::array< double, 3 > director
Director vector.
Definition: TLine3D.h:25
void getAsPose3DForcingOrigin(const TPoint3D &center, TPose3D &pose) const
Definition: TPlane.cpp:85
3D Plane, represented by its equation
Definition: TPlane.h:22
void unitarize()
Unitarize normal vector.
Definition: TPlane.cpp:66
double getAngle(const TPlane &p1, const TPlane &p2)
Computes the angle between two planes.
Definition: geometry.cpp:846
bool contains(const TPoint3D &point) const
Check whether a point is inside the line.
Definition: TLine3D.cpp:25
double distance(const TPoint3D &point) const
Distance to 3D point.
Definition: TPlane.cpp:38
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
CMatrixDouble44 generateAxisBaseFromDirectionAndAxis(const mrpt::math::TVector3D &vec, uint8_t coord)
Creates a homogeneus matrix (4x4) such that the coordinate given (0 for x, 1 for y, 2 for z) corresponds to the provided vector.
Definition: geometry.cpp:1993
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
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:155
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
T norm() const
Point norm: |v| = sqrt(x^2+y^2+z^2)
Definition: TPoint3D.h:150
bool contains(const TPoint3D &point) const
Check whether a point is contained into the plane.
Definition: TPlane.cpp:28
TPlane()=default
Fast default constructor.
std::array< double, 4 > coefs
Plane coefficients, stored as an array: .
Definition: TPlane.h:26
TVector3D getNormalVector() const
Get plane&#39;s normal vector.
Definition: TPlane.cpp:49
3D line, represented by a base point and a director vector.
Definition: TLine3D.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