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