Gazebo Math

API Reference

7.5.1
gz/math/Line2.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 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_LINE2_HH_
18 #define GZ_MATH_LINE2_HH_
19 
20 #include <algorithm>
21 #include <gz/math/Vector2.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 Line2
34  {
38  public: Line2(const math::Vector2<T> &_ptA, const math::Vector2<T> &_ptB)
39  {
40  this->Set(_ptA, _ptB);
41  }
42 
48  public: Line2(double _x1, double _y1, double _x2, double _y2)
49  {
50  this->Set(_x1, _y1, _x2, _y2);
51  }
52 
56  public: void Set(const math::Vector2<T> &_ptA,
57  const math::Vector2<T> &_ptB)
58  {
59  this->pts[0] = _ptA;
60  this->pts[1] = _ptB;
61  }
62 
68  public: void Set(double _x1, double _y1, double _x2, double _y2)
69  {
70  this->pts[0].Set(_x1, _y1);
71  this->pts[1].Set(_x2, _y2);
72  }
73 
80  public: double CrossProduct(const Line2<T> &_line) const
81  {
82  return (this->pts[0].X() - this->pts[1].X()) *
83  (_line[0].Y() -_line[1].Y()) -
84  (this->pts[0].Y() - this->pts[1].Y()) *
85  (_line[0].X() - _line[1].X());
86  }
87 
90  // (_pt.y - a.y) * (b.x - a.x) - (_pt.x - a.x) * (b.y - a.y)
93  public: double CrossProduct(const Vector2<T> &_pt) const
94  {
95  return (_pt.Y() - this->pts[0].Y()) *
96  (this->pts[1].X() - this->pts[0].X()) -
97  (_pt.X() - this->pts[0].X()) *
98  (this->pts[1].Y() - this->pts[0].Y());
99  }
100 
107  public: bool Collinear(const math::Vector2<T> &_pt,
108  double _epsilon = 1e-6) const
109  {
110  return math::equal(this->CrossProduct(_pt),
111  0., _epsilon);
112  }
113 
121  public: bool Parallel(const math::Line2<T> &_line,
122  double _epsilon = 1e-6) const
123  {
124  return math::equal(this->CrossProduct(_line),
125  0., _epsilon);
126  }
127 
135  public: bool Collinear(const math::Line2<T> &_line,
136  double _epsilon = 1e-6) const
137  {
138  return this->Parallel(_line, _epsilon) &&
139  this->Intersect(_line, _epsilon);
140  }
141 
147  public: bool OnSegment(const math::Vector2<T> &_pt,
148  double _epsilon = 1e-6) const
149  {
150  return this->Collinear(_pt, _epsilon) && this->Within(_pt, _epsilon);
151  }
152 
160  public: bool Within(const math::Vector2<T> &_pt,
161  double _epsilon = 1e-6) const
162  {
163  return _pt.X() <= std::max(this->pts[0].X(),
164  this->pts[1].X()) + _epsilon &&
165  _pt.X() >= std::min(this->pts[0].X(),
166  this->pts[1].X()) - _epsilon &&
167  _pt.Y() <= std::max(this->pts[0].Y(),
168  this->pts[1].Y()) + _epsilon &&
169  _pt.Y() >= std::min(this->pts[0].Y(),
170  this->pts[1].Y()) - _epsilon;
171  }
172 
178  public: bool Intersect(const Line2<T> &_line,
179  double _epsilon = 1e-6) const
180  {
181  static math::Vector2<T> ignore;
182  return this->Intersect(_line, ignore, _epsilon);
183  }
184 
193  public: bool Intersect(const Line2<T> &_line, math::Vector2<T> &_pt,
194  double _epsilon = 1e-6) const
195  {
196  double d = this->CrossProduct(_line);
197 
198  // d is zero if the two line are collinear. Must check special
199  // cases.
200  if (math::equal(d, 0.0, _epsilon))
201  {
202  // Check if _line's starting point is on the line.
203  if (this->Within(_line[0], _epsilon))
204  {
205  _pt = _line[0];
206  return true;
207  }
208  // Check if _line's ending point is on the line.
209  else if (this->Within(_line[1], _epsilon))
210  {
211  _pt = _line[1];
212  return true;
213  }
214  // Other wise return false.
215  else
216  return false;
217  }
218 
219  _pt.X((_line[0].X() - _line[1].X()) *
220  (this->pts[0].X() * this->pts[1].Y() -
221  this->pts[0].Y() * this->pts[1].X()) -
222  (this->pts[0].X() - this->pts[1].X()) *
223  (_line[0].X() * _line[1].Y() - _line[0].Y() * _line[1].X()));
224 
225  _pt.Y((_line[0].Y() - _line[1].Y()) *
226  (this->pts[0].X() * this->pts[1].Y() -
227  this->pts[0].Y() * this->pts[1].X()) -
228  (this->pts[0].Y() - this->pts[1].Y()) *
229  (_line[0].X() * _line[1].Y() - _line[0].Y() * _line[1].X()));
230 
231  _pt /= d;
232 
233  if (_pt.X() < std::min(this->pts[0].X(), this->pts[1].X()) ||
234  _pt.X() > std::max(this->pts[0].X(), this->pts[1].X()) ||
235  _pt.X() < std::min(_line[0].X(), _line[1].X()) ||
236  _pt.X() > std::max(_line[0].X(), _line[1].X()))
237  {
238  return false;
239  }
240 
241  if (_pt.Y() < std::min(this->pts[0].Y(), this->pts[1].Y()) ||
242  _pt.Y() > std::max(this->pts[0].Y(), this->pts[1].Y()) ||
243  _pt.Y() < std::min(_line[0].Y(), _line[1].Y()) ||
244  _pt.Y() > std::max(_line[0].Y(), _line[1].Y()))
245  {
246  return false;
247  }
248 
249  return true;
250  }
251 
254  public: T Length() const
255  {
256  return sqrt((this->pts[0].X() - this->pts[1].X()) *
257  (this->pts[0].X() - this->pts[1].X()) +
258  (this->pts[0].Y() - this->pts[1].Y()) *
259  (this->pts[0].Y() - this->pts[1].Y()));
260  }
261 
264  public: double Slope() const
265  {
266  if (math::equal(this->pts[1].X(), this->pts[0].X()))
267  return NAN_D;
268 
269  return (this->pts[1].Y() - this->pts[0].Y()) /
270  static_cast<double>(this->pts[1].X() - this->pts[0].X());
271  }
272 
276  public: bool operator==(const Line2<T> &_line) const
277  {
278  return this->pts[0] == _line[0] && this->pts[1] == _line[1];
279  }
280 
284  public: bool operator!=(const Line2<T> &_line) const
285  {
286  return !(*this == _line);
287  }
288 
292  public: math::Vector2<T> operator[](size_t _index) const
293  {
294  return this->pts[clamp(_index, GZ_ZERO_SIZE_T, GZ_ONE_SIZE_T)];
295  }
296 
301  public: friend std::ostream &operator<<(
302  std::ostream &_out, const Line2<T> &_line)
303  {
304  _out << _line[0] << " " << _line[1];
305  return _out;
306  }
307 
308  private: math::Vector2<T> pts[2];
309  };
310 
311 
315  } // namespace GZ_MATH_VERSION_NAMESPACE
316 } // namespace gz::math
317 #endif // GZ_MATH_LINE2_HH_