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 IGNITION_MATH_LINE3_HH_
18 #define IGNITION_MATH_LINE3_HH_
19 
20 #include <algorithm>
21 #include <ignition/math/Vector3.hh>
22 #include <ignition/math/config.hh>
23 
24 namespace ignition
25 {
26  namespace math
27  {
28  inline namespace IGNITION_MATH_VERSION_NAMESPACE
29  {
33  template<typename T>
34  class Line3
35  {
37  public: Line3() = default;
38 
41  public: Line3(const Line3<T> &_line)
42  {
43  this->pts[0] = _line[0];
44  this->pts[1] = _line[1];
45  }
46 
50  public: Line3(const math::Vector3<T> &_ptA, const math::Vector3<T> &_ptB)
51  {
52  this->Set(_ptA, _ptB);
53  }
54 
60  public: Line3(const double _x1, const double _y1,
61  const double _x2, const double _y2)
62  {
63  this->Set(_x1, _y1, _x2, _y2);
64  }
65 
73  public: Line3(const double _x1, const double _y1,
74  const double _z1, const double _x2,
75  const double _y2, const double _z2)
76  {
77  this->Set(_x1, _y1, _z1, _x2, _y2, _z2);
78  }
79 
83  public: void Set(const math::Vector3<T> &_ptA,
84  const math::Vector3<T> &_ptB)
85  {
86  this->pts[0] = _ptA;
87  this->pts[1] = _ptB;
88  }
89 
92  public: void SetA(const math::Vector3<T> &_ptA)
93  {
94  this->pts[0] = _ptA;
95  }
96 
99  public: void SetB(const math::Vector3<T> &_ptB)
100  {
101  this->pts[1] = _ptB;
102  }
103 
112  public: void Set(const double _x1, const double _y1,
113  const double _x2, const double _y2,
114  const double _z = 0)
115  {
116  this->pts[0].Set(_x1, _y1, _z);
117  this->pts[1].Set(_x2, _y2, _z);
118  }
119 
127  public: void Set(const double _x1, const double _y1,
128  const double _z1, const double _x2,
129  const double _y2, const double _z2)
130  {
131  this->pts[0].Set(_x1, _y1, _z1);
132  this->pts[1].Set(_x2, _y2, _z2);
133  }
134 
137  public: math::Vector3<T> Direction() const
138  {
139  return (this->pts[1] - this->pts[0]).Normalize();
140  }
141 
144  public: T Length() const
145  {
146  return this->pts[0].Distance(this->pts[1]);
147  }
148 
158  public: bool Distance(const Line3<T> &_line, Line3<T> &_result,
159  const double _epsilon = 1e-6) const
160  {
161  Vector3<T> p13 = this->pts[0] - _line[0];
162  Vector3<T> p43 = _line[1] - _line[0];
163 
164  if (std::abs(p43.X()) < _epsilon && std::abs(p43.Y()) < _epsilon &&
165  std::abs(p43.Z()) < _epsilon)
166  {
167  return false;
168  }
169 
170  Vector3<T> p21 = this->pts[1] - this->pts[0];
171 
172  if (std::abs(p21.X()) < _epsilon && std::abs(p21.Y()) < _epsilon &&
173  std::abs(p21.Z()) < _epsilon)
174  {
175  return false;
176  }
177 
178  double d1343 = p13.Dot(p43);
179  double d4321 = p43.Dot(p21);
180  double d1321 = p13.Dot(p21);
181  double d4343 = p43.Dot(p43);
182  double d2121 = p21.Dot(p21);
183 
184  double denom = d2121 * d4343 - d4321 * d4321;
185 
186  // In this case, we choose the first point in this line,
187  // and the closest point in the provided line.
188  if (std::abs(denom) < _epsilon)
189  {
190  double d1 = this->pts[0].Distance(_line[0]);
191  double d2 = this->pts[0].Distance(_line[1]);
192 
193  double d3 = this->pts[1].Distance(_line[0]);
194  double d4 = this->pts[1].Distance(_line[1]);
195 
196  if (d1 <= d2 && d1 <= d3 && d1 <= d4)
197  {
198  _result.SetA(this->pts[0]);
199  _result.SetB(_line[0]);
200  }
201  else if (d2 <= d3 && d2 <= d4)
202  {
203  _result.SetA(this->pts[0]);
204  _result.SetB(_line[1]);
205  }
206  else if (d3 <= d4)
207  {
208  _result.SetA(this->pts[1]);
209  _result.SetB(_line[0]);
210  }
211  else
212  {
213  _result.SetA(this->pts[1]);
214  _result.SetB(_line[1]);
215  }
216 
217  return true;
218  }
219 
220  double numer = d1343 * d4321 - d1321 * d4343;
221 
222  double mua = clamp(numer / denom, 0.0, 1.0);
223  double mub = clamp((d1343 + d4321 * mua) / d4343, 0.0, 1.0);
224 
225  _result.Set(this->pts[0] + (p21 * mua), _line[0] + (p43 * mub));
226 
227  return true;
228  }
229 
235  public: bool Intersect(const Line3<T> &_line,
236  double _epsilon = 1e-6) const
237  {
238  static math::Vector3<T> ignore;
239  return this->Intersect(_line, ignore, _epsilon);
240  }
241 
247  public: bool Coplanar(const Line3<T> &_line,
248  const double _epsilon = 1e-6) const
249  {
250  return std::abs((_line[0] - this->pts[0]).Dot(
251  (this->pts[1] - this->pts[0]).Cross(_line[1] - _line[0])))
252  <= _epsilon;
253  }
254 
260  public: bool Parallel(const Line3<T> &_line,
261  const double _epsilon = 1e-6) const
262  {
263  return (this->pts[1] - this->pts[0]).Cross(
264  _line[1] - _line[0]).Length() <= _epsilon;
265  }
266 
275  public: bool Intersect(const Line3<T> &_line, math::Vector3<T> &_pt,
276  double _epsilon = 1e-6) const
277  {
278  // Handle special case when lines are parallel
279  if (this->Parallel(_line, _epsilon))
280  {
281  // Check if _line's starting point is on the line.
282  if (this->Within(_line[0], _epsilon))
283  {
284  _pt = _line[0];
285  return true;
286  }
287  // Check if _line's ending point is on the line.
288  else if (this->Within(_line[1], _epsilon))
289  {
290  _pt = _line[1];
291  return true;
292  }
293  // Otherwise return false.
294  else
295  return false;
296  }
297 
298  // Get the line that is the shortest distance between this and _line
299  math::Line3<T> distLine;
300  this->Distance(_line, distLine, _epsilon);
301 
302  // If the length of the line is less than epsilon, then they
303  // intersect.
304  if (distLine.Length() < _epsilon)
305  {
306  _pt = distLine[0];
307  return true;
308  }
309 
310  return false;
311  }
312 
319  public: bool Within(const math::Vector3<T> &_pt,
320  double _epsilon = 1e-6) const
321  {
322  return _pt.X() <= std::max(this->pts[0].X(),
323  this->pts[1].X()) + _epsilon &&
324  _pt.X() >= std::min(this->pts[0].X(),
325  this->pts[1].X()) - _epsilon &&
326  _pt.Y() <= std::max(this->pts[0].Y(),
327  this->pts[1].Y()) + _epsilon &&
328  _pt.Y() >= std::min(this->pts[0].Y(),
329  this->pts[1].Y()) - _epsilon &&
330  _pt.Z() <= std::max(this->pts[0].Z(),
331  this->pts[1].Z()) + _epsilon &&
332  _pt.Z() >= std::min(this->pts[0].Z(),
333  this->pts[1].Z()) - _epsilon;
334  }
335 
339  public: bool operator==(const Line3<T> &_line) const
340  {
341  return this->pts[0] == _line[0] && this->pts[1] == _line[1];
342  }
343 
347  public: bool operator!=(const Line3<T> &_line) const
348  {
349  return !(*this == _line);
350  }
351 
355  public: math::Vector3<T> operator[](const size_t _index) const
356  {
357  return this->pts[clamp(_index, IGN_ZERO_SIZE_T, IGN_ONE_SIZE_T)];
358  }
359 
364  public: friend std::ostream &operator<<(
365  std::ostream &_out, const Line3<T> &_line)
366  {
367  _out << _line[0] << " " << _line[1];
368  return _out;
369  }
370 
374  public: Line3 &operator=(const Line3<T> &_line)
375  {
376  this->pts[0] = _line[0];
377  this->pts[1] = _line[1];
378 
379  return *this;
380  }
381 
383  private: math::Vector3<T> pts[2];
384  };
385 
389  }
390  }
391 }
392 #endif
A three dimensional line segment.
Definition: Line3.hh:35
bool Parallel(const Line3< T > &_line, const double _epsilon=1e-6) const
Test if this line and the given line are parallel.
Definition: Line3.hh:260
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: Line3.hh:319
Line3 & operator=(const Line3< T > &_line)
Assignment operator.
Definition: Line3.hh:374
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: Line3.hh:112
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: Line3.hh:127
math::Vector3< T > operator[](const size_t _index) const
Get the start or end point.
Definition: Line3.hh:355
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: Line3.hh:158
Line3(const math::Vector3< T > &_ptA, const math::Vector3< T > &_ptB)
Constructor.
Definition: Line3.hh:50
Line3(const Line3< T > &_line)
Copy constructor.
Definition: Line3.hh:41
bool Coplanar(const Line3< T > &_line, const double _epsilon=1e-6) const
Test if this line and the given line are coplanar.
Definition: Line3.hh:247
void SetA(const math::Vector3< T > &_ptA)
Set the start point of the line segment.
Definition: Line3.hh:92
bool Intersect(const Line3< T > &_line, double _epsilon=1e-6) const
Check if this line intersects the given line segment.
Definition: Line3.hh:235
math::Vector3< T > Direction() const
Get the direction of the line.
Definition: Line3.hh:137
void Set(const math::Vector3< T > &_ptA, const math::Vector3< T > &_ptB)
Set the start and end point of the line segment.
Definition: Line3.hh:83
bool operator!=(const Line3< T > &_line) const
Inequality operator.
Definition: Line3.hh:347
bool Intersect(const Line3< T > &_line, math::Vector3< T > &_pt, double _epsilon=1e-6) const
Check if this line intersects the given line segment.
Definition: Line3.hh:275
bool operator==(const Line3< T > &_line) const
Equality operator.
Definition: Line3.hh:339
friend std::ostream & operator<<(std::ostream &_out, const Line3< T > &_line)
Stream extraction operator.
Definition: Line3.hh:364
Line3(const double _x1, const double _y1, const double _z1, const double _x2, const double _y2, const double _z2)
Constructor.
Definition: Line3.hh:73
T Length() const
Get the length of the line.
Definition: Line3.hh:144
Line3(const double _x1, const double _y1, const double _x2, const double _y2)
2D Constructor where Z coordinates are 0
Definition: Line3.hh:60
void SetB(const math::Vector3< T > &_ptB)
Set the end point of the line segment.
Definition: Line3.hh:99
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:40
T Z() const
Get the z value.
Definition: Vector3.hh:661
T Y() const
Get the y value.
Definition: Vector3.hh:654
T Dot(const Vector3< T > &_v) const
Return the dot product of this vector and another vector.
Definition: Vector3.hh:198
T X() const
Get the x value.
Definition: Vector3.hh:647
Line3< float > Line3f
Definition: Line3.hh:388
T clamp(T _v, T _min, T _max)
Simple clamping function.
Definition: Helpers.hh:395
Line3< double > Line3d
Definition: Line3.hh:387
Line3< int > Line3i
Definition: Line3.hh:386
T min(const std::vector< T > &_values)
get the minimum value of vector of values
Definition: Helpers.hh:531
static const size_t IGN_ZERO_SIZE_T
size_t type with a value of 0
Definition: Helpers.hh:216
static const size_t IGN_ONE_SIZE_T
size_t type with a value of 1
Definition: Helpers.hh:219
T max(const std::vector< T > &_values)
get the maximum value of vector of values
Definition: Helpers.hh:518
Definition: Angle.hh:40