Gazebo Math

API Reference

7.5.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 gz::math
25 {
26  // Inline bracket to help doxygen filtering.
27  inline namespace GZ_MATH_VERSION_NAMESPACE {
28  //
32  template<typename T>
33  class Line3
34  {
36  public: Line3() = default;
37 
40  public: Line3(const Line3<T> &_line) = default;
41 
45  public: Line3(const math::Vector3<T> &_ptA, const math::Vector3<T> &_ptB)
46  {
47  this->Set(_ptA, _ptB);
48  }
49 
55  public: Line3(const double _x1, const double _y1,
56  const double _x2, const double _y2)
57  {
58  this->Set(_x1, _y1, _x2, _y2);
59  }
60 
68  public: Line3(const double _x1, const double _y1,
69  const double _z1, const double _x2,
70  const double _y2, const double _z2)
71  {
72  this->Set(_x1, _y1, _z1, _x2, _y2, _z2);
73  }
74 
78  public: void Set(const math::Vector3<T> &_ptA,
79  const math::Vector3<T> &_ptB)
80  {
81  this->pts[0] = _ptA;
82  this->pts[1] = _ptB;
83  }
84 
87  public: void SetA(const math::Vector3<T> &_ptA)
88  {
89  this->pts[0] = _ptA;
90  }
91 
94  public: void SetB(const math::Vector3<T> &_ptB)
95  {
96  this->pts[1] = _ptB;
97  }
98 
107  public: void Set(const double _x1, const double _y1,
108  const double _x2, const double _y2,
109  const double _z = 0)
110  {
111  this->pts[0].Set(_x1, _y1, _z);
112  this->pts[1].Set(_x2, _y2, _z);
113  }
114 
122  public: void Set(const double _x1, const double _y1,
123  const double _z1, const double _x2,
124  const double _y2, const double _z2)
125  {
126  this->pts[0].Set(_x1, _y1, _z1);
127  this->pts[1].Set(_x2, _y2, _z2);
128  }
129 
132  public: math::Vector3<T> Direction() const
133  {
134  return (this->pts[1] - this->pts[0]).Normalize();
135  }
136 
139  public: T Length() const
140  {
141  return this->pts[0].Distance(this->pts[1]);
142  }
143 
154  public: bool Distance(const Line3<T> &_line, Line3<T> &_result,
155  const double _epsilon = 1e-6) const
156  {
157  Vector3<T> p13 = this->pts[0] - _line[0];
158  Vector3<T> p43 = _line[1] - _line[0];
159 
160  if (std::abs(p43.X()) < _epsilon && std::abs(p43.Y()) < _epsilon &&
161  std::abs(p43.Z()) < _epsilon)
162  {
163  return false;
164  }
165 
166  Vector3<T> p21 = this->pts[1] - this->pts[0];
167 
168  if (std::abs(p21.X()) < _epsilon && std::abs(p21.Y()) < _epsilon &&
169  std::abs(p21.Z()) < _epsilon)
170  {
171  return false;
172  }
173 
174  double d1343 = p13.Dot(p43);
175  double d4321 = p43.Dot(p21);
176  double d1321 = p13.Dot(p21);
177  double d4343 = p43.Dot(p43);
178  double d2121 = p21.Dot(p21);
179 
180  double denom = d2121 * d4343 - d4321 * d4321;
181 
182  // In this case, we choose the first point in this line,
183  // and the closest point in the provided line.
184  if (std::abs(denom) < _epsilon)
185  {
186  double d1 = this->pts[0].Distance(_line[0]);
187  double d2 = this->pts[0].Distance(_line[1]);
188 
189  double d3 = this->pts[1].Distance(_line[0]);
190  double d4 = this->pts[1].Distance(_line[1]);
191 
192  if (d1 <= d2 && d1 <= d3 && d1 <= d4)
193  {
194  _result.SetA(this->pts[0]);
195  _result.SetB(_line[0]);
196  }
197  else if (d2 <= d3 && d2 <= d4)
198  {
199  _result.SetA(this->pts[0]);
200  _result.SetB(_line[1]);
201  }
202  else if (d3 <= d4)
203  {
204  _result.SetA(this->pts[1]);
205  _result.SetB(_line[0]);
206  }
207  else
208  {
209  _result.SetA(this->pts[1]);
210  _result.SetB(_line[1]);
211  }
212 
213  return true;
214  }
215 
216  double numer = d1343 * d4321 - d1321 * d4343;
217 
218  double mua = clamp(numer / denom, 0.0, 1.0);
219  double mub = clamp((d1343 + d4321 * mua) / d4343, 0.0, 1.0);
220 
221  _result.Set(this->pts[0] + (p21 * mua), _line[0] + (p43 * mub));
222 
223  return true;
224  }
225 
229  public: T Distance(const Vector3<T> &_pt)
230  {
231  auto line = this->pts[1] - this->pts[0];
232  auto ptTo0 = _pt - this->pts[0];
233  auto ptTo1 = _pt - this->pts[1];
234 
235  // Point is projected beyond pt0 or the line has length 0
236  if (ptTo0.Dot(line) <= 0.0)
237  {
238  return ptTo0.Length();
239  }
240 
241  // Point is projected beyond pt1
242  if (ptTo1.Dot(line) >= 0.0)
243  {
244  return ptTo1.Length();
245  }
246 
247  // Distance to point projected onto line
248  // line.Length() will have to be > 0 at this point otherwise it would
249  // return at line 244.
250  auto d = ptTo0.Cross(line);
251  auto lineLength = line.Length();
252  assert(lineLength > 0);
253  return d.Length() / lineLength;
254  }
255 
261  public: bool Intersect(const Line3<T> &_line,
262  double _epsilon = 1e-6) const
263  {
264  static math::Vector3<T> ignore;
265  return this->Intersect(_line, ignore, _epsilon);
266  }
267 
273  public: bool Coplanar(const Line3<T> &_line,
274  const double _epsilon = 1e-6) const
275  {
276  return std::abs((_line[0] - this->pts[0]).Dot(
277  (this->pts[1] - this->pts[0]).Cross(_line[1] - _line[0])))
278  <= _epsilon;
279  }
280 
286  public: bool Parallel(const Line3<T> &_line,
287  const double _epsilon = 1e-6) const
288  {
289  return (this->pts[1] - this->pts[0]).Cross(
290  _line[1] - _line[0]).Length() <= _epsilon;
291  }
292 
301  public: bool Intersect(const Line3<T> &_line, math::Vector3<T> &_pt,
302  double _epsilon = 1e-6) const
303  {
304  // Handle special case when lines are parallel
305  if (this->Parallel(_line, _epsilon))
306  {
307  // Check if _line's starting point is on the line.
308  if (this->Within(_line[0], _epsilon))
309  {
310  _pt = _line[0];
311  return true;
312  }
313  // Check if _line's ending point is on the line.
314  else if (this->Within(_line[1], _epsilon))
315  {
316  _pt = _line[1];
317  return true;
318  }
319  // Otherwise return false.
320  else
321  return false;
322  }
323 
324  // Get the line that is the shortest distance between this and _line
325  math::Line3<T> distLine;
326  this->Distance(_line, distLine, _epsilon);
327 
328  // If the length of the line is less than epsilon, then they
329  // intersect.
330  if (distLine.Length() < _epsilon)
331  {
332  _pt = distLine[0];
333  return true;
334  }
335 
336  return false;
337  }
338 
345  public: bool Within(const math::Vector3<T> &_pt,
346  double _epsilon = 1e-6) const
347  {
348  return _pt.X() <= std::max(this->pts[0].X(),
349  this->pts[1].X()) + _epsilon &&
350  _pt.X() >= std::min(this->pts[0].X(),
351  this->pts[1].X()) - _epsilon &&
352  _pt.Y() <= std::max(this->pts[0].Y(),
353  this->pts[1].Y()) + _epsilon &&
354  _pt.Y() >= std::min(this->pts[0].Y(),
355  this->pts[1].Y()) - _epsilon &&
356  _pt.Z() <= std::max(this->pts[0].Z(),
357  this->pts[1].Z()) + _epsilon &&
358  _pt.Z() >= std::min(this->pts[0].Z(),
359  this->pts[1].Z()) - _epsilon;
360  }
361 
365  public: bool operator==(const Line3<T> &_line) const
366  {
367  return this->pts[0] == _line[0] && this->pts[1] == _line[1];
368  }
369 
373  public: bool operator!=(const Line3<T> &_line) const
374  {
375  return !(*this == _line);
376  }
377 
381  public: math::Vector3<T> operator[](const size_t _index) const
382  {
383  return this->pts[clamp(_index, GZ_ZERO_SIZE_T, GZ_ONE_SIZE_T)];
384  }
385 
390  public: friend std::ostream &operator<<(
391  std::ostream &_out, const Line3<T> &_line)
392  {
393  _out << _line[0] << " " << _line[1];
394  return _out;
395  }
396 
400  public: Line3 &operator=(const Line3<T> &_line) = default;
401 
403  private: math::Vector3<T> pts[2];
404  };
405 
409  } // namespace GZ_MATH_VERSION_NAMESPACE_
410 } // namespace gz::math
411 #endif // GZ_MATH_LINE3_HH_