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 IGNITION_MATH_LINE2_HH_
18 #define IGNITION_MATH_LINE2_HH_
19 
20 #include <algorithm>
21 #include <ignition/math/Vector2.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 Line2
35  {
39  public: Line2(const math::Vector2<T> &_ptA, const math::Vector2<T> &_ptB)
40  {
41  this->Set(_ptA, _ptB);
42  }
43 
49  public: Line2(double _x1, double _y1, double _x2, double _y2)
50  {
51  this->Set(_x1, _y1, _x2, _y2);
52  }
53 
57  public: void Set(const math::Vector2<T> &_ptA,
58  const math::Vector2<T> &_ptB)
59  {
60  this->pts[0] = _ptA;
61  this->pts[1] = _ptB;
62  }
63 
69  public: void Set(double _x1, double _y1, double _x2, double _y2)
70  {
71  this->pts[0].Set(_x1, _y1);
72  this->pts[1].Set(_x2, _y2);
73  }
74 
81  public: double CrossProduct(const Line2<T> &_line) const
82  {
83  return (this->pts[0].X() - this->pts[1].X()) *
84  (_line[0].Y() -_line[1].Y()) -
85  (this->pts[0].Y() - this->pts[1].Y()) *
86  (_line[0].X() - _line[1].X());
87  }
88 
91  // (_pt.y - a.y) * (b.x - a.x) - (_pt.x - a.x) * (b.y - a.y)
94  public: double CrossProduct(const Vector2<T> &_pt) const
95  {
96  return (_pt.Y() - this->pts[0].Y()) *
97  (this->pts[1].X() - this->pts[0].X()) -
98  (_pt.X() - this->pts[0].X()) *
99  (this->pts[1].Y() - this->pts[0].Y());
100  }
101 
108  public: bool Collinear(const math::Vector2<T> &_pt,
109  double _epsilon = 1e-6) const
110  {
111  return math::equal(this->CrossProduct(_pt),
112  static_cast<T>(0), _epsilon);
113  }
114 
122  public: bool Parallel(const math::Line2<T> &_line,
123  double _epsilon = 1e-6) const
124  {
125  return math::equal(this->CrossProduct(_line),
126  static_cast<T>(0), _epsilon);
127  }
128 
136  public: bool Collinear(const math::Line2<T> &_line,
137  double _epsilon = 1e-6) const
138  {
139  return this->Parallel(_line, _epsilon) &&
140  this->Intersect(_line, _epsilon);
141  }
142 
148  public: bool OnSegment(const math::Vector2<T> &_pt,
149  double _epsilon = 1e-6) const
150  {
151  return this->Collinear(_pt, _epsilon) && this->Within(_pt, _epsilon);
152  }
153 
161  public: bool Within(const math::Vector2<T> &_pt,
162  double _epsilon = 1e-6) const
163  {
164  return _pt.X() <= std::max(this->pts[0].X(),
165  this->pts[1].X()) + _epsilon &&
166  _pt.X() >= std::min(this->pts[0].X(),
167  this->pts[1].X()) - _epsilon &&
168  _pt.Y() <= std::max(this->pts[0].Y(),
169  this->pts[1].Y()) + _epsilon &&
170  _pt.Y() >= std::min(this->pts[0].Y(),
171  this->pts[1].Y()) - _epsilon;
172  }
173 
179  public: bool Intersect(const Line2<T> &_line,
180  double _epsilon = 1e-6) const
181  {
182  static math::Vector2<T> ignore;
183  return this->Intersect(_line, ignore, _epsilon);
184  }
185 
194  public: bool Intersect(const Line2<T> &_line, math::Vector2<T> &_pt,
195  double _epsilon = 1e-6) const
196  {
197  double d = this->CrossProduct(_line);
198 
199  // d is zero if the two line are collinear. Must check special
200  // cases.
201  if (math::equal(d, 0.0, _epsilon))
202  {
203  // Check if _line's starting point is on the line.
204  if (this->Within(_line[0], _epsilon))
205  {
206  _pt = _line[0];
207  return true;
208  }
209  // Check if _line's ending point is on the line.
210  else if (this->Within(_line[1], _epsilon))
211  {
212  _pt = _line[1];
213  return true;
214  }
215  // Other wise return false.
216  else
217  return false;
218  }
219 
220  _pt.X((_line[0].X() - _line[1].X()) *
221  (this->pts[0].X() * this->pts[1].Y() -
222  this->pts[0].Y() * this->pts[1].X()) -
223  (this->pts[0].X() - this->pts[1].X()) *
224  (_line[0].X() * _line[1].Y() - _line[0].Y() * _line[1].X()));
225 
226  _pt.Y((_line[0].Y() - _line[1].Y()) *
227  (this->pts[0].X() * this->pts[1].Y() -
228  this->pts[0].Y() * this->pts[1].X()) -
229  (this->pts[0].Y() - this->pts[1].Y()) *
230  (_line[0].X() * _line[1].Y() - _line[0].Y() * _line[1].X()));
231 
232  _pt /= d;
233 
234  if (_pt.X() < std::min(this->pts[0].X(), this->pts[1].X()) ||
235  _pt.X() > std::max(this->pts[0].X(), this->pts[1].X()) ||
236  _pt.X() < std::min(_line[0].X(), _line[1].X()) ||
237  _pt.X() > std::max(_line[0].X(), _line[1].X()))
238  {
239  return false;
240  }
241 
242  if (_pt.Y() < std::min(this->pts[0].Y(), this->pts[1].Y()) ||
243  _pt.Y() > std::max(this->pts[0].Y(), this->pts[1].Y()) ||
244  _pt.Y() < std::min(_line[0].Y(), _line[1].Y()) ||
245  _pt.Y() > std::max(_line[0].Y(), _line[1].Y()))
246  {
247  return false;
248  }
249 
250  return true;
251  }
252 
255  public: T Length() const
256  {
257  return sqrt((this->pts[0].X() - this->pts[1].X()) *
258  (this->pts[0].X() - this->pts[1].X()) +
259  (this->pts[0].Y() - this->pts[1].Y()) *
260  (this->pts[0].Y() - this->pts[1].Y()));
261  }
262 
265  public: double Slope() const
266  {
267  if (math::equal(this->pts[1].X(), this->pts[0].X()))
268  return NAN_D;
269 
270  return (this->pts[1].Y() - this->pts[0].Y()) /
271  static_cast<double>(this->pts[1].X() - this->pts[0].X());
272  }
273 
277  public: bool operator==(const Line2<T> &_line) const
278  {
279  return this->pts[0] == _line[0] && this->pts[1] == _line[1];
280  }
281 
285  public: bool operator!=(const Line2<T> &_line) const
286  {
287  return !(*this == _line);
288  }
289 
293  public: math::Vector2<T> operator[](size_t _index) const
294  {
295  return this->pts[clamp(_index, IGN_ZERO_SIZE_T, IGN_ONE_SIZE_T)];
296  }
297 
302  public: friend std::ostream &operator<<(
303  std::ostream &_out, const Line2<T> &_line)
304  {
305  _out << _line[0] << " " << _line[1];
306  return _out;
307  }
308 
309  private: math::Vector2<T> pts[2];
310  };
311 
312 
316  }
317  }
318 }
319 #endif
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Line2::Line2
Line2(double _x1, double _y1, double _x2, double _y2)
Constructor.
Definition: Line2.hh:49
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Line2::Length
T Length() const
Get the length of the line.
Definition: Line2.hh:255
ignition
Definition: Angle.hh:40
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Line2::operator[]
math::Vector2< T > operator[](size_t _index) const
Get the start or end point.
Definition: Line2.hh:293
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Line2::CrossProduct
double CrossProduct(const Line2< T > &_line) const
Return the cross product of this line and the given line.
Definition: Line2.hh:81
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::IGN_ONE_SIZE_T
static const size_t IGN_ONE_SIZE_T
size_t type with a value of 1
Definition: Helpers.hh:219
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Line2::Set
void Set(double _x1, double _y1, double _x2, double _y2)
Set the start and end point of the line segment.
Definition: Line2.hh:69
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Line2::Within
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.
Definition: Line2.hh:161
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Line2::OnSegment
bool OnSegment(const math::Vector2< T > &_pt, double _epsilon=1e-6) const
Return whether the given point is on this line segment.
Definition: Line2.hh:148
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Line2::Intersect
bool Intersect(const Line2< T > &_line, double _epsilon=1e-6) const
Check if this line intersects the given line segment.
Definition: Line2.hh:179
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Line2::Collinear
bool Collinear(const math::Line2< T > &_line, double _epsilon=1e-6) const
Check if the given line is collinear with this line.
Definition: Line2.hh:136
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Line2::operator!=
bool operator!=(const Line2< T > &_line) const
Inequality operator.
Definition: Line2.hh:285
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::min
T min(const std::vector< T > &_values)
get the minimum value of vector of values
Definition: Helpers.hh:531
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Line2::Intersect
bool Intersect(const Line2< T > &_line, math::Vector2< T > &_pt, double _epsilon=1e-6) const
Check if this line intersects the given line segment.
Definition: Line2.hh:194
Vector2.hh
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::equal
bool equal(const T &_a, const T &_b, const T &_epsilon=T(1e-6))
check if two values are equal, within a tolerance
Definition: Helpers.hh:545
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Line2::Slope
double Slope() const
Get the slope of the line.
Definition: Line2.hh:265
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Line2::Collinear
bool Collinear(const math::Vector2< T > &_pt, double _epsilon=1e-6) const
Check if the given point is collinear with this line.
Definition: Line2.hh:108
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Line2f
Line2< float > Line2f
Definition: Line2.hh:315
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector2::Y
T Y() const
Return the y value.
Definition: Vector2.hh:398
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::IGN_ZERO_SIZE_T
static const size_t IGN_ZERO_SIZE_T
size_t type with a value of 0
Definition: Helpers.hh:216
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Line2::CrossProduct
double CrossProduct(const Vector2< T > &_pt) const
Return the cross product of this line and the given point.
Definition: Line2.hh:94
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::max
T max(const std::vector< T > &_values)
get the maximum value of vector of values
Definition: Helpers.hh:518
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Line2::operator==
bool operator==(const Line2< T > &_line) const
Equality operator.
Definition: Line2.hh:277
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Line2d
Line2< double > Line2d
Definition: Line2.hh:314
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::clamp
T clamp(T _v, T _min, T _max)
Simple clamping function.
Definition: Helpers.hh:395
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector2
Two dimensional (x, y) vector.
Definition: Vector2.hh:33
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Line2::operator<<
friend std::ostream & operator<<(std::ostream &_out, const Line2< T > &_line)
Stream extraction operator.
Definition: Line2.hh:302
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::NAN_D
static const double NAN_D
Returns the representation of a quiet not a number (NAN)
Definition: Helpers.hh:258
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Line2::Line2
Line2(const math::Vector2< T > &_ptA, const math::Vector2< T > &_ptB)
Constructor.
Definition: Line2.hh:39
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Line2i
Line2< int > Line2i
Definition: Line2.hh:313
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector2::X
T X() const
Return the x value.
Definition: Vector2.hh:391
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Line2
A two dimensional line segment. The line is defined by a start and end point.
Definition: Line2.hh:35
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Line2::Parallel
bool Parallel(const math::Line2< T > &_line, double _epsilon=1e-6) const
Check if the given line is parallel with this line.
Definition: Line2.hh:122
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Line2::Set
void Set(const math::Vector2< T > &_ptA, const math::Vector2< T > &_ptB)
Set the start and end point of the line segment.
Definition: Line2.hh:57