Gazebo Math

API Reference

6.15.1
gz/math/Line3.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 #ifndef GZ_MATH_LINE3_HH_
18 #define GZ_MATH_LINE3_HH_
19 
20 #include <algorithm>
21 #include <gz/math/Vector3.hh>
22 #include <gz/math/config.hh>
23 
24 namespace ignition
25 {
26  namespace math
27  {
28  // Inline bracket to help doxygen filtering.
29  inline namespace IGNITION_MATH_VERSION_NAMESPACE {
30  //
34  template<typename T>
35  class Line3
36  {
38  public: Line3() = default;
39 
42  public: Line3(const Line3<T> &_line)
43  {
44  this->pts[0] = _line[0];
45  this->pts[1] = _line[1];
46  }
47 
51  public: Line3(const math::Vector3<T> &_ptA, const math::Vector3<T> &_ptB)
52  {
53  this->Set(_ptA, _ptB);
54  }
55 
61  public: Line3(const double _x1, const double _y1,
62  const double _x2, const double _y2)
63  {
64  this->Set(_x1, _y1, _x2, _y2);
65  }
66 
74  public: Line3(const double _x1, const double _y1,
75  const double _z1, const double _x2,
76  const double _y2, const double _z2)
77  {
78  this->Set(_x1, _y1, _z1, _x2, _y2, _z2);
79  }
80 
84  public: void Set(const math::Vector3<T> &_ptA,
85  const math::Vector3<T> &_ptB)
86  {
87  this->pts[0] = _ptA;
88  this->pts[1] = _ptB;
89  }
90 
93  public: void SetA(const math::Vector3<T> &_ptA)
94  {
95  this->pts[0] = _ptA;
96  }
97 
100  public: void SetB(const math::Vector3<T> &_ptB)
101  {
102  this->pts[1] = _ptB;
103  }
104 
113  public: void Set(const double _x1, const double _y1,
114  const double _x2, const double _y2,
115  const double _z = 0)
116  {
117  this->pts[0].Set(_x1, _y1, _z);
118  this->pts[1].Set(_x2, _y2, _z);
119  }
120 
128  public: void Set(const double _x1, const double _y1,
129  const double _z1, const double _x2,
130  const double _y2, const double _z2)
131  {
132  this->pts[0].Set(_x1, _y1, _z1);
133  this->pts[1].Set(_x2, _y2, _z2);
134  }
135 
138  public: math::Vector3<T> Direction() const
139  {
140  return (this->pts[1] - this->pts[0]).Normalize();
141  }
142 
145  public: T Length() const
146  {
147  return this->pts[0].Distance(this->pts[1]);
148  }
149 
160  public: bool Distance(const Line3<T> &_line, Line3<T> &_result,
161  const double _epsilon = 1e-6) const
162  {
163  Vector3<T> p13 = this->pts[0] - _line[0];
164  Vector3<T> p43 = _line[1] - _line[0];
165 
166  if (std::abs(p43.X()) < _epsilon && std::abs(p43.Y()) < _epsilon &&
167  std::abs(p43.Z()) < _epsilon)
168  {
169  return false;
170  }
171 
172  Vector3<T> p21 = this->pts[1] - this->pts[0];
173 
174  if (std::abs(p21.X()) < _epsilon && std::abs(p21.Y()) < _epsilon &&
175  std::abs(p21.Z()) < _epsilon)
176  {
177  return false;
178  }
179 
180  double d1343 = p13.Dot(p43);
181  double d4321 = p43.Dot(p21);
182  double d1321 = p13.Dot(p21);
183  double d4343 = p43.Dot(p43);
184  double d2121 = p21.Dot(p21);
185 
186  double denom = d2121 * d4343 - d4321 * d4321;
187 
188  // In this case, we choose the first point in this line,
189  // and the closest point in the provided line.
190  if (std::abs(denom) < _epsilon)
191  {
192  double d1 = this->pts[0].Distance(_line[0]);
193  double d2 = this->pts[0].Distance(_line[1]);
194 
195  double d3 = this->pts[1].Distance(_line[0]);
196  double d4 = this->pts[1].Distance(_line[1]);
197 
198  if (d1 <= d2 && d1 <= d3 && d1 <= d4)
199  {
200  _result.SetA(this->pts[0]);
201  _result.SetB(_line[0]);
202  }
203  else if (d2 <= d3 && d2 <= d4)
204  {
205  _result.SetA(this->pts[0]);
206  _result.SetB(_line[1]);
207  }
208  else if (d3 <= d4)
209  {
210  _result.SetA(this->pts[1]);
211  _result.SetB(_line[0]);
212  }
213  else
214  {
215  _result.SetA(this->pts[1]);
216  _result.SetB(_line[1]);
217  }
218 
219  return true;
220  }
221 
222  double numer = d1343 * d4321 - d1321 * d4343;
223 
224  double mua = clamp(numer / denom, 0.0, 1.0);
225  double mub = clamp((d1343 + d4321 * mua) / d4343, 0.0, 1.0);
226 
227  _result.Set(this->pts[0] + (p21 * mua), _line[0] + (p43 * mub));
228 
229  return true;
230  }
231 
235  public: T Distance(const Vector3<T> &_pt)
236  {
237  auto line = this->pts[1] - this->pts[0];
238  auto ptTo0 = _pt - this->pts[0];
239  auto ptTo1 = _pt - this->pts[1];
240 
241  // Point is projected beyond pt0 or the line has length 0
242  if (ptTo0.Dot(line) <= 0.0)
243  {
244  return ptTo0.Length();
245  }
246 
247  // Point is projected beyond pt1
248  if (ptTo1.Dot(line) >= 0.0)
249  {
250  return ptTo1.Length();
251  }
252 
253  // Distance to point projected onto line
254  // line.Length() will have to be > 0 at this point otherwise it would
255  // return at line 244.
256  auto d = ptTo0.Cross(line);
257  auto lineLength = line.Length();
258  assert(lineLength > 0);
259  return d.Length() / lineLength;
260  }
261 
267  public: bool Intersect(const Line3<T> &_line,
268  double _epsilon = 1e-6) const
269  {
270  static math::Vector3<T> ignore;
271  return this->Intersect(_line, ignore, _epsilon);
272  }
273 
279  public: bool Coplanar(const Line3<T> &_line,
280  const double _epsilon = 1e-6) const
281  {
282  return std::abs((_line[0] - this->pts[0]).Dot(
283  (this->pts[1] - this->pts[0]).Cross(_line[1] - _line[0])))
284  <= _epsilon;
285  }
286 
292  public: bool Parallel(const Line3<T> &_line,
293  const double _epsilon = 1e-6) const
294  {
295  return (this->pts[1] - this->pts[0]).Cross(
296  _line[1] - _line[0]).Length() <= _epsilon;
297  }
298 
307  public: bool Intersect(const Line3<T> &_line, math::Vector3<T> &_pt,
308  double _epsilon = 1e-6) const
309  {
310  // Handle special case when lines are parallel
311  if (this->Parallel(_line, _epsilon))
312  {
313  // Check if _line's starting point is on the line.
314  if (this->Within(_line[0], _epsilon))
315  {
316  _pt = _line[0];
317  return true;
318  }
319  // Check if _line's ending point is on the line.
320  else if (this->Within(_line[1], _epsilon))
321  {
322  _pt = _line[1];
323  return true;
324  }
325  // Otherwise return false.
326  else
327  return false;
328  }
329 
330  // Get the line that is the shortest distance between this and _line
331  math::Line3<T> distLine;
332  this->Distance(_line, distLine, _epsilon);
333 
334  // If the length of the line is less than epsilon, then they
335  // intersect.
336  if (distLine.Length() < _epsilon)
337  {
338  _pt = distLine[0];
339  return true;
340  }
341 
342  return false;
343  }
344 
351  public: bool Within(const math::Vector3<T> &_pt,
352  double _epsilon = 1e-6) const
353  {
354  return _pt.X() <= std::max(this->pts[0].X(),
355  this->pts[1].X()) + _epsilon &&
356  _pt.X() >= std::min(this->pts[0].X(),
357  this->pts[1].X()) - _epsilon &&
358  _pt.Y() <= std::max(this->pts[0].Y(),
359  this->pts[1].Y()) + _epsilon &&
360  _pt.Y() >= std::min(this->pts[0].Y(),
361  this->pts[1].Y()) - _epsilon &&
362  _pt.Z() <= std::max(this->pts[0].Z(),
363  this->pts[1].Z()) + _epsilon &&
364  _pt.Z() >= std::min(this->pts[0].Z(),
365  this->pts[1].Z()) - _epsilon;
366  }
367 
371  public: bool operator==(const Line3<T> &_line) const
372  {
373  return this->pts[0] == _line[0] && this->pts[1] == _line[1];
374  }
375 
379  public: bool operator!=(const Line3<T> &_line) const
380  {
381  return !(*this == _line);
382  }
383 
387  public: math::Vector3<T> operator[](const size_t _index) const
388  {
389  return this->pts[clamp(_index, IGN_ZERO_SIZE_T, IGN_ONE_SIZE_T)];
390  }
391 
396  public: friend std::ostream &operator<<(
397  std::ostream &_out, const Line3<T> &_line)
398  {
399  _out << _line[0] << " " << _line[1];
400  return _out;
401  }
402 
406  public: Line3 &operator=(const Line3<T> &_line)
407  {
408  this->pts[0] = _line[0];
409  this->pts[1] = _line[1];
410 
411  return *this;
412  }
413 
415  private: math::Vector3<T> pts[2];
416  };
417 
421  }
422  }
423 }
424 #endif
Definition: gz/math/AdditivelySeparableScalarField3.hh:27
Line3(const double _x1, const double _y1, const double _z1, const double _x2, const double _y2, const double _z2)
Constructor.
Definition: gz/math/Line3.hh:74
bool Distance(const Line3< T > &_line, Line3< T > &_result, const double _epsilon=1e-6) const
Get the shortest line between this line and the provided line.
Definition: gz/math/Line3.hh:160
T Length() const
Returns the length (magnitude) of the vector.
Definition: gz/math/Vector3.hh:122
Line3(const math::Vector3< T > &_ptA, const math::Vector3< T > &_ptB)
Constructor.
Definition: gz/math/Line3.hh:51
T Distance(const Vector3< T > &_pt)
Calculate shortest distance between line and point.
Definition: gz/math/Line3.hh:235
T X() const
Get the x value.
Definition: gz/math/Vector3.hh:654
bool Within(const math::Vector3< T > &_pt, double _epsilon=1e-6) const
Check if the given point is between the start and end points of the line segment.
Definition: gz/math/Line3.hh:351
void Set(const double _x1, const double _y1, const double _x2, const double _y2, const double _z=0)
Set the start and end point of the line segment, assuming that both points have the same height.
Definition: gz/math/Line3.hh:113
Line3(const double _x1, const double _y1, const double _x2, const double _y2)
2D Constructor where Z coordinates are 0
Definition: gz/math/Line3.hh:61
T Z() const
Get the z value.
Definition: gz/math/Vector3.hh:668
bool Parallel(const Line3< T > &_line, const double _epsilon=1e-6) const
Test if this line and the given line are parallel.
Definition: gz/math/Line3.hh:292
STL class.
void Set(const math::Vector3< T > &_ptA, const math::Vector3< T > &_ptB)
Set the start and end point of the line segment.
Definition: gz/math/Line3.hh:84
Line3 & operator=(const Line3< T > &_line)
Assignment operator.
Definition: gz/math/Line3.hh:406
T clamp(T _v, T _min, T _max)
Simple clamping function.
Definition: gz/math/Helpers.hh:406
The Vector3 class represents the generic vector containing 3 elements. Since it's commonly used to ke...
Definition: gz/math/Vector3.hh:41
T Length() const
Get the length of the line.
Definition: gz/math/Line3.hh:145
bool Intersect(const Line3< T > &_line, math::Vector3< T > &_pt, double _epsilon=1e-6) const
Check if this line intersects the given line segment. The point of intersection is returned in the _p...
Definition: gz/math/Line3.hh:307
void SetA(const math::Vector3< T > &_ptA)
Set the start point of the line segment.
Definition: gz/math/Line3.hh:93
Line3< float > Line3f
Definition: gz/math/Line3.hh:420
Line3()=default
Line Constructor.
Line3< int > Line3i
Definition: gz/math/Line3.hh:418
bool Coplanar(const Line3< T > &_line, const double _epsilon=1e-6) const
Test if this line and the given line are coplanar.
Definition: gz/math/Line3.hh:279
A three dimensional line segment. The line is defined by a start and end point.
Definition: gz/math/Line3.hh:35
bool operator!=(const Line3< T > &_line) const
Inequality operator.
Definition: gz/math/Line3.hh:379
T min(T... args)
void SetB(const math::Vector3< T > &_ptB)
Set the end point of the line segment.
Definition: gz/math/Line3.hh:100
Line3< double > Line3d
Definition: gz/math/Line3.hh:419
Line3(const Line3< T > &_line)
Copy constructor.
Definition: gz/math/Line3.hh:42
math::Vector3< T > operator[](const size_t _index) const
Get the start or end point.
Definition: gz/math/Line3.hh:387
void Set(const double _x1, const double _y1, const double _z1, const double _x2, const double _y2, const double _z2)
Set the start and end point of the line segment.
Definition: gz/math/Line3.hh:128
math::Vector3< T > Direction() const
Get the direction of the line.
Definition: gz/math/Line3.hh:138
friend std::ostream & operator<<(std::ostream &_out, const Line3< T > &_line)
Stream extraction operator.
Definition: gz/math/Line3.hh:396
static const size_t IGN_ZERO_SIZE_T
size_t type with a value of 0
Definition: gz/math/Helpers.hh:227
bool Intersect(const Line3< T > &_line, double _epsilon=1e-6) const
Check if this line intersects the given line segment.
Definition: gz/math/Line3.hh:267
static const size_t IGN_ONE_SIZE_T
size_t type with a value of 1
Definition: gz/math/Helpers.hh:230
T Dot(const Vector3< T > &_v) const
Return the dot product of this vector and another vector.
Definition: gz/math/Vector3.hh:205
bool operator==(const Line3< T > &_line) const
Equality operator.
Definition: gz/math/Line3.hh:371
T max(T... args)
T Y() const
Get the y value.
Definition: gz/math/Vector3.hh:661