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 
23 namespace ignition
24 {
25  namespace math
26  {
30  template<typename T>
31  class Line2
32  {
36  public: Line2(const math::Vector2<T> &_ptA, const math::Vector2<T> &_ptB)
37  {
38  this->Set(_ptA, _ptB);
39  }
40 
46  public: Line2(double _x1, double _y1, double _x2, double _y2)
47  {
48  this->Set(_x1, _y1, _x2, _y2);
49  }
50 
54  public: void Set(const math::Vector2<T> &_ptA,
55  const math::Vector2<T> &_ptB)
56  {
57  this->pts[0] = _ptA;
58  this->pts[1] = _ptB;
59  }
60 
66  public: void Set(double _x1, double _y1, double _x2, double _y2)
67  {
68  this->pts[0].Set(_x1, _y1);
69  this->pts[1].Set(_x2, _y2);
70  }
71 
78  public: double CrossProduct(const Line2<T> &_line) const
79  {
80  return (this->pts[0].X() - this->pts[1].X()) *
81  (_line[0].Y() -_line[1].Y()) -
82  (this->pts[0].Y() - this->pts[1].Y()) *
83  (_line[0].X() - _line[1].X());
84  }
85 
88  // (_pt.y - a.y) * (b.x - a.x) - (_pt.x - a.x) * (b.y - a.y)
91  public: double CrossProduct(const Vector2<T> &_pt) const
92  {
93  return (_pt.Y() - this->pts[0].Y()) *
94  (this->pts[1].X() - this->pts[0].X()) -
95  (_pt.X() - this->pts[0].X()) *
96  (this->pts[1].Y() - this->pts[0].Y());
97  }
98 
105  public: bool Collinear(const math::Vector2<T> &_pt,
106  double _epsilon = 1e-6) const
107  {
108  return math::equal(this->CrossProduct(_pt),
109  static_cast<T>(0), _epsilon);
110  }
111 
119  public: bool Parallel(const math::Line2<T> &_line,
120  double _epsilon = 1e-6) const
121  {
122  return math::equal(this->CrossProduct(_line),
123  static_cast<T>(0), _epsilon);
124  }
125 
133  public: bool Collinear(const math::Line2<T> &_line,
134  double _epsilon = 1e-6) const
135  {
136  return this->Parallel(_line, _epsilon) &&
137  this->Intersect(_line, _epsilon);
138  }
139 
145  public: bool OnSegment(const math::Vector2<T> &_pt,
146  double _epsilon = 1e-6) const
147  {
148  return this->Collinear(_pt, _epsilon) && this->Within(_pt, _epsilon);
149  }
150 
158  public: bool Within(const math::Vector2<T> &_pt,
159  double _epsilon = 1e-6) const
160  {
161  return _pt.X() <= std::max(this->pts[0].X(),
162  this->pts[1].X()) + _epsilon &&
163  _pt.X() >= std::min(this->pts[0].X(),
164  this->pts[1].X()) - _epsilon &&
165  _pt.Y() <= std::max(this->pts[0].Y(),
166  this->pts[1].Y()) + _epsilon &&
167  _pt.Y() >= std::min(this->pts[0].Y(),
168  this->pts[1].Y()) - _epsilon;
169  }
170 
176  public: bool Intersect(const Line2<T> &_line,
177  double _epsilon = 1e-6) const
178  {
179  static math::Vector2<T> ignore;
180  return this->Intersect(_line, ignore, _epsilon);
181  }
182 
191  public: bool Intersect(const Line2<T> &_line, math::Vector2<T> &_pt,
192  double _epsilon = 1e-6) const
193  {
194  double d = this->CrossProduct(_line);
195 
196  // d is zero if the two line are collinear. Must check special
197  // cases.
198  if (math::equal(d, 0.0, _epsilon))
199  {
200  // Check if _line's starting point is on the line.
201  if (this->Within(_line[0], _epsilon))
202  {
203  _pt = _line[0];
204  return true;
205  }
206  // Check if _line's ending point is on the line.
207  else if (this->Within(_line[1], _epsilon))
208  {
209  _pt = _line[1];
210  return true;
211  }
212  // Other wise return false.
213  else
214  return false;
215  }
216 
217  _pt.X((_line[0].X() - _line[1].X()) *
218  (this->pts[0].X() * this->pts[1].Y() -
219  this->pts[0].Y() * this->pts[1].X()) -
220  (this->pts[0].X() - this->pts[1].X()) *
221  (_line[0].X() * _line[1].Y() - _line[0].Y() * _line[1].X()));
222 
223  _pt.Y((_line[0].Y() - _line[1].Y()) *
224  (this->pts[0].X() * this->pts[1].Y() -
225  this->pts[0].Y() * this->pts[1].X()) -
226  (this->pts[0].Y() - this->pts[1].Y()) *
227  (_line[0].X() * _line[1].Y() - _line[0].Y() * _line[1].X()));
228 
229  _pt /= d;
230 
231  if (_pt.X() < std::min(this->pts[0].X(), this->pts[1].X()) ||
232  _pt.X() > std::max(this->pts[0].X(), this->pts[1].X()) ||
233  _pt.X() < std::min(_line[0].X(), _line[1].X()) ||
234  _pt.X() > std::max(_line[0].X(), _line[1].X()))
235  {
236  return false;
237  }
238 
239  if (_pt.Y() < std::min(this->pts[0].Y(), this->pts[1].Y()) ||
240  _pt.Y() > std::max(this->pts[0].Y(), this->pts[1].Y()) ||
241  _pt.Y() < std::min(_line[0].Y(), _line[1].Y()) ||
242  _pt.Y() > std::max(_line[0].Y(), _line[1].Y()))
243  {
244  return false;
245  }
246 
247  return true;
248  }
249 
252  public: T Length() const
253  {
254  return sqrt((this->pts[0].X() - this->pts[1].X()) *
255  (this->pts[0].X() - this->pts[1].X()) +
256  (this->pts[0].Y() - this->pts[1].Y()) *
257  (this->pts[0].Y() - this->pts[1].Y()));
258  }
259 
262  public: double Slope() const
263  {
264  if (math::equal(this->pts[1].X(), this->pts[0].X()))
265  return NAN_D;
266 
267  return (this->pts[1].Y() - this->pts[0].Y()) /
268  static_cast<double>(this->pts[1].X() - this->pts[0].X());
269  }
270 
274  public: bool operator==(const Line2<T> &_line) const
275  {
276  return this->pts[0] == _line[0] && this->pts[1] == _line[1];
277  }
278 
282  public: bool operator!=(const Line2<T> &_line) const
283  {
284  return !(*this == _line);
285  }
286 
290  public: math::Vector2<T> operator[](size_t _index) const
291  {
292  return this->pts[clamp(_index, IGN_ZERO_SIZE_T, IGN_ONE_SIZE_T)];
293  }
294 
299  public: friend std::ostream &operator<<(
300  std::ostream &_out, const Line2<T> &_line)
301  {
302  _out << _line[0] << " " << _line[1];
303  return _out;
304  }
305 
306  private: math::Vector2<T> pts[2];
307  };
308 
309 
313  }
314 }
315 #endif
bool operator==(const Line2< T > &_line) const
Equality operator.
Definition: Line2.hh:274
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:105
T Length() const
Get the length of the line.
Definition: Line2.hh:252
static const size_t IGN_ONE_SIZE_T
size_t type with a value of 1
Definition: Helpers.hh:216
Line2< float > Line2f
Definition: Line2.hh:312
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:133
static const double NAN_D
Returns the representation of a quiet not a number (NAN)
Definition: Helpers.hh:255
void Set(double _x1, double _y1, double _x2, double _y2)
Set the start and end point of the line segment.
Definition: Line2.hh:66
bool operator!=(const Line2< T > &_line) const
Inequality operator.
Definition: Line2.hh:282
double Slope() const
Get the slope of the line.
Definition: Line2.hh:262
static const size_t IGN_ZERO_SIZE_T
size_t type with a value of 0
Definition: Helpers.hh:213
Two dimensional (x, y) vector.
Definition: Vector2.hh:29
T X() const
Return the x value.
Definition: Vector2.hh:380
T max(const std::vector< T > &_values)
get the maximum value of vector of values
Definition: Helpers.hh:515
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:158
A two dimensional line segment.
Definition: Line2.hh:31
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:542
Line2(const math::Vector2< T > &_ptA, const math::Vector2< T > &_ptB)
Constructor.
Definition: Line2.hh:36
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:119
Line2< double > Line2d
Definition: Line2.hh:311
math::Vector2< T > operator[](size_t _index) const
Get the start or end point.
Definition: Line2.hh:290
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:145
friend std::ostream & operator<<(std::ostream &_out, const Line2< T > &_line)
Stream extraction operator.
Definition: Line2.hh:299
double CrossProduct(const Vector2< T > &_pt) const
Return the cross product of this line and the given point.
Definition: Line2.hh:91
bool Intersect(const Line2< T > &_line, double _epsilon=1e-6) const
Check if this line intersects the given line segment.
Definition: Line2.hh:176
Line2(double _x1, double _y1, double _x2, double _y2)
Constructor.
Definition: Line2.hh:46
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:191
Definition: Angle.hh:38
double CrossProduct(const Line2< T > &_line) const
Return the cross product of this line and the given line.
Definition: Line2.hh:78
T Y() const
Return the y value.
Definition: Vector2.hh:387
T min(const std::vector< T > &_values)
get the minimum value of vector of values
Definition: Helpers.hh:528
T clamp(T _v, T _min, T _max)
Simple clamping function.
Definition: Helpers.hh:392
Line2< int > Line2i
Definition: Line2.hh:310
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:54