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_LINE3_HH_
18 #define _IGNITION_LINE3_HH_
19 
20 #include <algorithm>
21 #include <ignition/math/Vector3.hh>
23 
24 namespace ignition
25 {
26  namespace math
27  {
31  template<typename T>
32  class Line3
33  {
35  public: Line3() = default;
36 
39  public: Line3(const Line3<T> &_line)
40  {
41  this->pts[0] = _line[0];
42  this->pts[1] = _line[1];
43  }
44 
48  public: Line3(const math::Vector3<T> &_ptA, const math::Vector3<T> &_ptB)
49  {
50  this->Set(_ptA, _ptB);
51  }
52 
58  public: Line3(const double _x1, const double _y1,
59  const double _x2, const double _y2)
60  {
61  this->Set(_x1, _y1, _x2, _y2);
62  }
63 
71  public: Line3(const double _x1, const double _y1,
72  const double _z1, const double _x2,
73  const double _y2, const double _z2)
74  {
75  this->Set(_x1, _y1, _z1, _x2, _y2, _z2);
76  }
77 
81  public: void Set(const math::Vector3<T> &_ptA,
82  const math::Vector3<T> &_ptB)
83  {
84  this->pts[0] = _ptA;
85  this->pts[1] = _ptB;
86  }
87 
90  public: void SetA(const math::Vector3<T> &_ptA)
91  {
92  this->pts[0] = _ptA;
93  }
94 
97  public: void SetB(const math::Vector3<T> &_ptB)
98  {
99  this->pts[1] = _ptB;
100  }
101 
110  public: void Set(const double _x1, const double _y1,
111  const double _x2, const double _y2,
112  const double _z = 0)
113  {
114  this->pts[0].Set(_x1, _y1, _z);
115  this->pts[1].Set(_x2, _y2, _z);
116  }
117 
125  public: void Set(const double _x1, const double _y1,
126  const double _z1, const double _x2,
127  const double _y2, const double _z2)
128  {
129  this->pts[0].Set(_x1, _y1, _z1);
130  this->pts[1].Set(_x2, _y2, _z2);
131  }
132 
135  public: math::Vector3<T> Direction() const
136  {
137  return (this->pts[1] - this->pts[0]).Normalize();
138  }
139 
142  public: T Length() const
143  {
144  return this->pts[0].Distance(this->pts[1]);
145  }
146 
156  public: bool Distance(const Line3<T> &_line, Line3<T> &_result,
157  const double _epsilon = 1e-6) const
158  {
159  Vector3<T> p13 = this->pts[0] - _line[0];
160  Vector3<T> p43 = _line[1] - _line[0];
161 
162  if (std::abs(p43.X()) < _epsilon && std::abs(p43.Y()) < _epsilon &&
163  std::abs(p43.Z()) < _epsilon)
164  {
165  return false;
166  }
167 
168  Vector3<T> p21 = this->pts[1] - this->pts[0];
169 
170  if (std::abs(p21.X()) < _epsilon && std::abs(p21.Y()) < _epsilon &&
171  std::abs(p21.Z()) < _epsilon)
172  {
173  return false;
174  }
175 
176  double d1343 = p13.Dot(p43);
177  double d4321 = p43.Dot(p21);
178  double d1321 = p13.Dot(p21);
179  double d4343 = p43.Dot(p43);
180  double d2121 = p21.Dot(p21);
181 
182  double denom = d2121 * d4343 - d4321 * d4321;
183 
184  // In this case, we choose the first point in this line,
185  // and the closest point in the provided line.
186  if (std::abs(denom) < _epsilon)
187  {
188  double d1 = this->pts[0].Distance(_line[0]);
189  double d2 = this->pts[0].Distance(_line[1]);
190 
191  double d3 = this->pts[1].Distance(_line[0]);
192  double d4 = this->pts[1].Distance(_line[1]);
193 
194  if (d1 <= d2 && d1 <= d3 && d1 <= d4)
195  {
196  _result.SetA(this->pts[0]);
197  _result.SetB(_line[0]);
198  }
199  else if (d2 <= d3 && d2 <= d4)
200  {
201  _result.SetA(this->pts[0]);
202  _result.SetB(_line[1]);
203  }
204  else if (d3 <= d4)
205  {
206  _result.SetA(this->pts[1]);
207  _result.SetB(_line[0]);
208  }
209  else
210  {
211  _result.SetA(this->pts[1]);
212  _result.SetB(_line[1]);
213  }
214 
215  return true;
216  }
217 
218  double numer = d1343 * d4321 - d1321 * d4343;
219 
220  double mua = clamp(numer / denom, 0.0, 1.0);
221  double mub = clamp((d1343 + d4321 * mua) / d4343, 0.0, 1.0);
222 
223  _result.Set(this->pts[0] + (p21 * mua), _line[0] + (p43 * mub));
224 
225  return true;
226  }
227 
233  public: bool Intersect(const Line3<T> &_line,
234  double _epsilon = 1e-6) const
235  {
236  static math::Vector3<T> ignore;
237  return this->Intersect(_line, ignore, _epsilon);
238  }
239 
245  public: bool Coplanar(const Line3<T> &_line,
246  const double _epsilon = 1e-6) const
247  {
248  return std::abs((_line[0] - this->pts[0]).Dot(
249  (this->pts[1] - this->pts[0]).Cross(_line[1] - _line[0])))
250  <= _epsilon;
251  }
252 
258  public: bool Parallel(const Line3<T> &_line,
259  const double _epsilon = 1e-6) const
260  {
261  return (this->pts[1] - this->pts[0]).Cross(
262  _line[1] - _line[0]).Length() <= _epsilon;
263  }
264 
273  public: bool Intersect(const Line3<T> &_line, math::Vector3<T> &_pt,
274  double _epsilon = 1e-6) const
275  {
276  // Handle special case when lines are parallel
277  if (this->Parallel(_line, _epsilon))
278  {
279  // Check if _line's starting point is on the line.
280  if (this->Within(_line[0], _epsilon))
281  {
282  _pt = _line[0];
283  return true;
284  }
285  // Check if _line's ending point is on the line.
286  else if (this->Within(_line[1], _epsilon))
287  {
288  _pt = _line[1];
289  return true;
290  }
291  // Otherwise return false.
292  else
293  return false;
294  }
295 
296  // Get the line that is the shortest distance between this and _line
297  math::Line3<T> distLine;
298  this->Distance(_line, distLine, _epsilon);
299 
300  // If the length of the line is less than epsilon, then they
301  // intersect.
302  if (distLine.Length() < _epsilon)
303  {
304  _pt = distLine[0];
305  return true;
306  }
307 
308  return false;
309  }
310 
317  public: bool Within(const math::Vector3<T> &_pt,
318  double _epsilon = 1e-6) const
319  {
320  return _pt.X() <= std::max(this->pts[0].X(),
321  this->pts[1].X()) + _epsilon &&
322  _pt.X() >= std::min(this->pts[0].X(),
323  this->pts[1].X()) - _epsilon &&
324  _pt.Y() <= std::max(this->pts[0].Y(),
325  this->pts[1].Y()) + _epsilon &&
326  _pt.Y() >= std::min(this->pts[0].Y(),
327  this->pts[1].Y()) - _epsilon &&
328  _pt.Z() <= std::max(this->pts[0].Z(),
329  this->pts[1].Z()) + _epsilon &&
330  _pt.Z() >= std::min(this->pts[0].Z(),
331  this->pts[1].Z()) - _epsilon;
332  }
333 
337  public: bool operator==(const Line3<T> &_line) const
338  {
339  return this->pts[0] == _line[0] && this->pts[1] == _line[1];
340  }
341 
345  public: bool operator!=(const Line3<T> &_line) const
346  {
347  return !(*this == _line);
348  }
349 
353  public: math::Vector3<T> operator[](const size_t _index) const
354  {
355  if (_index > 1)
356  throw IndexException();
357  return this->pts[_index];
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 #endif
math::Vector3< T > Direction() const
Get the direction of the line.
Definition: Line3.hh:135
T Length() const
Get the length of the line.
Definition: Line3.hh:142
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:125
void SetA(const math::Vector3< T > &_ptA)
Set the start point of the line segment.
Definition: Line3.hh:90
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:245
bool Intersect(const Line3< T > &_line, double _epsilon=1e-6) const
Check if this line intersects the given line segment.
Definition: Line3.hh:233
Line3(const Line3< T > &_line)
Copy constructor.
Definition: Line3.hh:39
T max(const std::vector< T > &_values)
get the maximum value of vector of values
Definition: Helpers.hh:230
T X() const
Get the x value.
Definition: Vector3.hh:630
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:273
Line3< int > Line3i
Definition: Line3.hh:386
Line3()=default
Line Constructor.
Line3< float > Line3f
Definition: Line3.hh:388
friend std::ostream & operator<<(std::ostream &_out, const Line3< T > &_line)
Stream extraction operator.
Definition: Line3.hh:364
T Dot(const Vector3< T > &_v) const
Return the dot product of this vector and another vector.
Definition: Vector3.hh:187
void SetB(const math::Vector3< T > &_ptB)
Set the end point of the line segment.
Definition: Line3.hh:97
bool operator!=(const Line3< T > &_line) const
Inequality operator.
Definition: Line3.hh:345
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:81
T Y() const
Get the y value.
Definition: Vector3.hh:637
T Z() const
Get the z value.
Definition: Vector3.hh:644
Exception that is thrown when an out-of-bounds index is encountered.
Definition: IndexException.hh:37
bool operator==(const Line3< T > &_line) const
Equality operator.
Definition: Line3.hh:337
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:110
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:317
Line3(const math::Vector3< T > &_ptA, const math::Vector3< T > &_ptB)
Constructor.
Definition: Line3.hh:48
math::Vector3< T > operator[](const size_t _index) const
Get the start or end point.
Definition: Line3.hh:353
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:37
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:156
Line3 & operator=(const Line3< T > &_line)
Assignment operator.
Definition: Line3.hh:374
A three dimensional line segment.
Definition: Line3.hh:32
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:258
Line3< double > Line3d
Definition: Line3.hh:387
Definition: AffineException.hh:30
Line3(const double _x1, const double _y1, const double _z1, const double _x2, const double _y2, const double _z2)
Constructor.
Definition: Line3.hh:71
Line3(const double _x1, const double _y1, const double _x2, const double _y2)
2D Constructor where Z coordinates are 0
Definition: Line3.hh:58
T min(const std::vector< T > &_values)
get the minimum value of vector of values
Definition: Helpers.hh:243
T clamp(T _v, T _min, T _max)
Simple clamping function.
Definition: Helpers.hh:131