Gazebo Math

API Reference

6.15.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 ignition
25 {
26  namespace math
27  {
28  // Inline bracket to help doxygen filtering.
29  inline namespace IGNITION_MATH_VERSION_NAMESPACE {
30  //
34  template<typename T>
35  class Line2
36  {
40  public: Line2(const math::Vector2<T> &_ptA, const math::Vector2<T> &_ptB)
41  {
42  this->Set(_ptA, _ptB);
43  }
44 
50  public: Line2(double _x1, double _y1, double _x2, double _y2)
51  {
52  this->Set(_x1, _y1, _x2, _y2);
53  }
54 
58  public: void Set(const math::Vector2<T> &_ptA,
59  const math::Vector2<T> &_ptB)
60  {
61  this->pts[0] = _ptA;
62  this->pts[1] = _ptB;
63  }
64 
70  public: void Set(double _x1, double _y1, double _x2, double _y2)
71  {
72  this->pts[0].Set(_x1, _y1);
73  this->pts[1].Set(_x2, _y2);
74  }
75 
82  public: double CrossProduct(const Line2<T> &_line) const
83  {
84  return (this->pts[0].X() - this->pts[1].X()) *
85  (_line[0].Y() -_line[1].Y()) -
86  (this->pts[0].Y() - this->pts[1].Y()) *
87  (_line[0].X() - _line[1].X());
88  }
89 
92  // (_pt.y - a.y) * (b.x - a.x) - (_pt.x - a.x) * (b.y - a.y)
95  public: double CrossProduct(const Vector2<T> &_pt) const
96  {
97  return (_pt.Y() - this->pts[0].Y()) *
98  (this->pts[1].X() - this->pts[0].X()) -
99  (_pt.X() - this->pts[0].X()) *
100  (this->pts[1].Y() - this->pts[0].Y());
101  }
102 
109  public: bool Collinear(const math::Vector2<T> &_pt,
110  double _epsilon = 1e-6) const
111  {
112  return math::equal(this->CrossProduct(_pt),
113  0., _epsilon);
114  }
115 
123  public: bool Parallel(const math::Line2<T> &_line,
124  double _epsilon = 1e-6) const
125  {
126  return math::equal(this->CrossProduct(_line),
127  0., _epsilon);
128  }
129 
137  public: bool Collinear(const math::Line2<T> &_line,
138  double _epsilon = 1e-6) const
139  {
140  return this->Parallel(_line, _epsilon) &&
141  this->Intersect(_line, _epsilon);
142  }
143 
149  public: bool OnSegment(const math::Vector2<T> &_pt,
150  double _epsilon = 1e-6) const
151  {
152  return this->Collinear(_pt, _epsilon) && this->Within(_pt, _epsilon);
153  }
154 
162  public: bool Within(const math::Vector2<T> &_pt,
163  double _epsilon = 1e-6) const
164  {
165  return _pt.X() <= std::max(this->pts[0].X(),
166  this->pts[1].X()) + _epsilon &&
167  _pt.X() >= std::min(this->pts[0].X(),
168  this->pts[1].X()) - _epsilon &&
169  _pt.Y() <= std::max(this->pts[0].Y(),
170  this->pts[1].Y()) + _epsilon &&
171  _pt.Y() >= std::min(this->pts[0].Y(),
172  this->pts[1].Y()) - _epsilon;
173  }
174 
180  public: bool Intersect(const Line2<T> &_line,
181  double _epsilon = 1e-6) const
182  {
183  static math::Vector2<T> ignore;
184  return this->Intersect(_line, ignore, _epsilon);
185  }
186 
195  public: bool Intersect(const Line2<T> &_line, math::Vector2<T> &_pt,
196  double _epsilon = 1e-6) const
197  {
198  double d = this->CrossProduct(_line);
199 
200  // d is zero if the two line are collinear. Must check special
201  // cases.
202  if (math::equal(d, 0.0, _epsilon))
203  {
204  // Check if _line's starting point is on the line.
205  if (this->Within(_line[0], _epsilon))
206  {
207  _pt = _line[0];
208  return true;
209  }
210  // Check if _line's ending point is on the line.
211  else if (this->Within(_line[1], _epsilon))
212  {
213  _pt = _line[1];
214  return true;
215  }
216  // Other wise return false.
217  else
218  return false;
219  }
220 
221  _pt.X((_line[0].X() - _line[1].X()) *
222  (this->pts[0].X() * this->pts[1].Y() -
223  this->pts[0].Y() * this->pts[1].X()) -
224  (this->pts[0].X() - this->pts[1].X()) *
225  (_line[0].X() * _line[1].Y() - _line[0].Y() * _line[1].X()));
226 
227  _pt.Y((_line[0].Y() - _line[1].Y()) *
228  (this->pts[0].X() * this->pts[1].Y() -
229  this->pts[0].Y() * this->pts[1].X()) -
230  (this->pts[0].Y() - this->pts[1].Y()) *
231  (_line[0].X() * _line[1].Y() - _line[0].Y() * _line[1].X()));
232 
233  _pt /= d;
234 
235  if (_pt.X() < std::min(this->pts[0].X(), this->pts[1].X()) ||
236  _pt.X() > std::max(this->pts[0].X(), this->pts[1].X()) ||
237  _pt.X() < std::min(_line[0].X(), _line[1].X()) ||
238  _pt.X() > std::max(_line[0].X(), _line[1].X()))
239  {
240  return false;
241  }
242 
243  if (_pt.Y() < std::min(this->pts[0].Y(), this->pts[1].Y()) ||
244  _pt.Y() > std::max(this->pts[0].Y(), this->pts[1].Y()) ||
245  _pt.Y() < std::min(_line[0].Y(), _line[1].Y()) ||
246  _pt.Y() > std::max(_line[0].Y(), _line[1].Y()))
247  {
248  return false;
249  }
250 
251  return true;
252  }
253 
256  public: T Length() const
257  {
258  return sqrt((this->pts[0].X() - this->pts[1].X()) *
259  (this->pts[0].X() - this->pts[1].X()) +
260  (this->pts[0].Y() - this->pts[1].Y()) *
261  (this->pts[0].Y() - this->pts[1].Y()));
262  }
263 
266  public: double Slope() const
267  {
268  if (math::equal(this->pts[1].X(), this->pts[0].X()))
269  return NAN_D;
270 
271  return (this->pts[1].Y() - this->pts[0].Y()) /
272  static_cast<double>(this->pts[1].X() - this->pts[0].X());
273  }
274 
278  public: bool operator==(const Line2<T> &_line) const
279  {
280  return this->pts[0] == _line[0] && this->pts[1] == _line[1];
281  }
282 
286  public: bool operator!=(const Line2<T> &_line) const
287  {
288  return !(*this == _line);
289  }
290 
294  public: math::Vector2<T> operator[](size_t _index) const
295  {
296  return this->pts[clamp(_index, IGN_ZERO_SIZE_T, IGN_ONE_SIZE_T)];
297  }
298 
303  public: friend std::ostream &operator<<(
304  std::ostream &_out, const Line2<T> &_line)
305  {
306  _out << _line[0] << " " << _line[1];
307  return _out;
308  }
309 
310  private: math::Vector2<T> pts[2];
311  };
312 
313 
317  }
318  }
319 }
320 #endif
friend std::ostream & operator<<(std::ostream &_out, const Line2< T > &_line)
Stream extraction operator.
Definition: gz/math/Line2.hh:303
Line2< double > Line2d
Definition: gz/math/Line2.hh:315
Two dimensional (x, y) vector.
Definition: gz/math/Vector2.hh:37
Definition: gz/math/AdditivelySeparableScalarField3.hh:27
bool equal(const T &_a, const T &_b, const T &_epsilon=T(1e-6))
check if two values are equal, within a tolerance
Definition: gz/math/Helpers.hh:556
math::Vector2< T > operator[](size_t _index) const
Get the start or end point.
Definition: gz/math/Line2.hh:294
double CrossProduct(const Vector2< T > &_pt) const
Return the cross product of this line and the given point. Given 'a' and 'b' as the start and end poi...
Definition: gz/math/Line2.hh:95
bool Collinear(const math::Vector2< T > &_pt, double _epsilon=1e-6) const
Check if the given point is collinear with this line.
Definition: gz/math/Line2.hh:109
Line2< int > Line2i
Definition: gz/math/Line2.hh:314
bool Collinear(const math::Line2< T > &_line, double _epsilon=1e-6) const
Check if the given line is collinear with this line. This is the AND of Parallel and Intersect.
Definition: gz/math/Line2.hh:137
T X() const
Return the x value.
Definition: gz/math/Vector2.hh:499
bool Within(const math::Vector2< T > &_pt, double _epsilon=1e-6) const
Check if the given point is between the start and end points of the line segment. This does not imply...
Definition: gz/math/Line2.hh:162
A two dimensional line segment. The line is defined by a start and end point.
Definition: gz/math/Line2.hh:35
void Set(const math::Vector2< T > &_ptA, const math::Vector2< T > &_ptB)
Set the start and end point of the line segment.
Definition: gz/math/Line2.hh:58
STL class.
bool Intersect(const Line2< T > &_line, math::Vector2< T > &_pt, double _epsilon=1e-6) const
Check if this line intersects the given line segment. The point of intersection is returned in the _r...
Definition: gz/math/Line2.hh:195
bool OnSegment(const math::Vector2< T > &_pt, double _epsilon=1e-6) const
Return whether the given point is on this line segment.
Definition: gz/math/Line2.hh:149
void Set(double _x1, double _y1, double _x2, double _y2)
Set the start and end point of the line segment.
Definition: gz/math/Line2.hh:70
T clamp(T _v, T _min, T _max)
Simple clamping function.
Definition: gz/math/Helpers.hh:406
Line2< float > Line2f
Definition: gz/math/Line2.hh:316
Line2(double _x1, double _y1, double _x2, double _y2)
Constructor.
Definition: gz/math/Line2.hh:50
T Y() const
Return the y value.
Definition: gz/math/Vector2.hh:506
Line2(const math::Vector2< T > &_ptA, const math::Vector2< T > &_ptB)
Constructor.
Definition: gz/math/Line2.hh:40
T min(T... args)
bool operator!=(const Line2< T > &_line) const
Inequality operator.
Definition: gz/math/Line2.hh:286
double Slope() const
Get the slope of the line.
Definition: gz/math/Line2.hh:266
static const size_t IGN_ZERO_SIZE_T
size_t type with a value of 0
Definition: gz/math/Helpers.hh:227
bool Intersect(const Line2< T > &_line, double _epsilon=1e-6) const
Check if this line intersects the given line segment.
Definition: gz/math/Line2.hh:180
static const size_t IGN_ONE_SIZE_T
size_t type with a value of 1
Definition: gz/math/Helpers.hh:230
T max(T... args)
static const double NAN_D
Returns the representation of a quiet not a number (NAN)
Definition: gz/math/Helpers.hh:269
bool operator==(const Line2< T > &_line) const
Equality operator.
Definition: gz/math/Line2.hh:278
T Length() const
Get the length of the line.
Definition: gz/math/Line2.hh:256
bool Parallel(const math::Line2< T > &_line, double _epsilon=1e-6) const
Check if the given line is parallel with this line.
Definition: gz/math/Line2.hh:123
double CrossProduct(const Line2< T > &_line) const
Return the cross product of this line and the given line. Give 'a' as this line and 'b' as given line...
Definition: gz/math/Line2.hh:82