iLab Neuromorphic Robotics Toolkit  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
LineImpl.H
Go to the documentation of this file.
1 /*! @file
2  @author Unknown
3  @copyright GNU Public License (GPL v3)
4  @section License
5  @verbatim
6  // ////////////////////////////////////////////////////////////////////////
7  // The iLab Neuromorphic Robotics Toolkit (NRT) //
8  // Copyright 2010-2012 by the University of Southern California (USC) //
9  // and the iLab at USC. //
10  // //
11  // iLab - University of Southern California //
12  // Hedco Neurociences Building, Room HNB-10 //
13  // Los Angeles, Ca 90089-2520 - USA //
14  // //
15  // See http://ilab.usc.edu for information about this project. //
16  // ////////////////////////////////////////////////////////////////////////
17  // This file is part of The iLab Neuromorphic Robotics Toolkit. //
18  // //
19  // The iLab Neuromorphic Robotics Toolkit is free software: you can //
20  // redistribute it and/or modify it under the terms of the GNU General //
21  // Public License as published by the Free Software Foundation, either //
22  // version 3 of the License, or (at your option) any later version. //
23  // //
24  // The iLab Neuromorphic Robotics Toolkit is distributed in the hope //
25  // that it will be useful, but WITHOUT ANY WARRANTY; without even the //
26  // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR //
27  // PURPOSE. See the GNU General Public License for more details. //
28  // //
29  // You should have received a copy of the GNU General Public License //
30  // along with The iLab Neuromorphic Robotics Toolkit. If not, see //
31  // <http://www.gnu.org/licenses/>. //
32  // ////////////////////////////////////////////////////////////////////////
33  @endverbatim */
34 
35 // ######################################################################
36 template <class T> inline
38  itsP1(0,0), itsP2(0,0)
39 { }
40 
41 // ######################################################################
42 template <class T> template <class U> inline
44  itsP1(other.p1()), itsP2(other.p2())
45 { }
46 
47 // ######################################################################
48 template <class T> inline
50  itsP1(p1), itsP2(p2)
51 { }
52 
53 // ######################################################################
54 template <class T> inline
55 nrt::Line<T>::Line(T x1, T y1, T x2, T y2) :
56  itsP1(x1, y1), itsP2(x2, y2)
57 { }
58 
59 // ######################################################################
60 template <class T> inline
61 nrt::Line<T>::Line(nrt::Point2D<T> const & center, double const ori, double const len)
62 {
63  T const dx = nrt::clamped_convert<T>(cos(ori) * len * 0.5);
64  T const dy = nrt::clamped_convert<T>(sin(ori) * len * 0.5);
65 
66  itsP1 = nrt::Point2D<T>(center.x() - dx, center.y() + dy);
67  itsP2 = nrt::Point2D<T>(center.x() + dx, center.y() - dy);
68 }
69 
70 // ######################################################################
71 template <class T> inline
73 { return itsP1; }
74 
75 // ######################################################################
76 template <class T> inline
78 { return itsP2; }
79 
80 // ######################################################################
81 template <class T> inline
83 { return nrt::Point2D<T>((itsP1 + itsP2) * 0.5); }
84 
85 // ######################################################################
86 template <class T> inline
87 double nrt::Line<T>::length() const
88 { return itsP1.distanceTo(itsP2); }
89 
90 // ######################################################################
91 template <class T> inline
93 { return itsP1.manhattanDistanceTo(itsP2); }
94 
95 // ######################################################################
96 template <class T> inline
97 double nrt::Line<T>::angle() const
98 { return atan2(double(itsP1.y()-itsP2.y()), double(itsP1.x()-itsP2.x())); }
99 
100 
101 // ######################################################################
102 namespace
103 {
104  template <class T>
105  inline T dot(nrt::Point2D<T> u, nrt::Point2D<T> v)
106  {
107  return u.x()*v.x() + u.y()*v.y();
108  }
109 
110  template <class T>
111  inline T perp(nrt::Point2D<T> u, nrt::Point2D<T> v)
112  {
113  return u.x()*v.y() - u.y()*v.x();
114  }
115 
116  // determine if a point is inside a collinear segment
117  template <class T>
118  int inSegment( nrt::Point2D<T> P, nrt::Line<T> S)
119  {
120  if (S.p1().x() != S.p2().x()) { // S is not vertical
121  if (S.p1().x() <= P.x() && P.x() <= S.p2().x()) return 1;
122  if (S.p1().x() >= P.x() && P.x() >= S.p2().x()) return 1;
123  } else { // S is vertical, so test y() coordinate
124  if (S.p1().y() <= P.y() && P.y() <= S.p2().y()) return 1;
125  if (S.p1().y() >= P.y() && P.y() >= S.p2().y()) return 1;
126  }
127  return 0;
128  }
129 
130  const double epsilon = 0.00000001;
131 }
132 
133 
134 
135 // ######################################################################
136 // Copyright 2001, softSurfer (www.softsurfer.com)
137 // This code may be freely used and modified for any purpose
138 // providing that this copyright notice is included with it.
139 // SoftSurfer makes no warranty for this code, and cannot be held
140 // liable for any real or imagined damage resulting from its use.
141 // Users of this code must verify correctness for their application.
142 template <class T> template <typename promo>
143 std::shared_ptr<nrt::Point2D<typename nrt::promote<T, promo>::type> >
145 {
146  typedef typename nrt::promote<T, promo>::type TP;
147 
148  nrt::Point2D<TP> u(nrt::Point2D<TP>((*this).p2()) - nrt::Point2D<TP>((*this).p1()));
149  nrt::Point2D<TP> v(nrt::Point2D<TP>( other.p2()) - nrt::Point2D<TP>( other.p1()));
150  nrt::Point2D<TP> w(nrt::Point2D<TP>((*this).p1()) - nrt::Point2D<TP>( other.p1()));
151  TP D = perp(u, v);
152 
153  // test if they are parallel (includes either being a point)
154  if (std::abs(D) < epsilon) { // (*this) and other are parallel
155  if (perp(u,w) != 0 || perp(v,w) != 0) return std::shared_ptr<nrt::Point2D<TP> >(); // they are NOT collinear
156 
157  // they are collinear or degenerate
158  // check if they are degenerate points
159  TP du = dot(u,u);
160  TP dv = dot(v,v);
161  if (du==0 && dv==0) { // both segments are points
162  if ((*this).p1() != other.p1()) // they are distinct points
163  return std::shared_ptr<nrt::Point2D<TP> >();
164  // they are the same point
165  return std::shared_ptr<nrt::Point2D<TP> >(new nrt::Point2D<TP>(this->p1()));
166  }
167  if (du==0) { // (*this) is a single point
168  if (inSegment((*this).p1(), other) == 0) // but is not in other
169  return std::shared_ptr<nrt::Point2D<TP> >();
170  return std::shared_ptr<nrt::Point2D<TP> >(new nrt::Point2D<TP>(this->p1()));
171  }
172  if (dv==0) { // other a single point
173  if (inSegment(other.p1(), (*this)) == 0) // but is not in (*this)
174  return std::shared_ptr<nrt::Point2D<TP> >();
175  return std::shared_ptr<nrt::Point2D<TP> >(new nrt::Point2D<TP>(other.p1()));
176  }
177  // they are collinear segments - get overlap (or not)
178  TP t0, t1; // endpoints of (*this) in eqn for other
179  nrt::Point2D<TP> w2(nrt::Point2D<TP>((*this).p2()) - nrt::Point2D<TP>(other.p1()));
180  if (v.x() != 0) { t0 = w.x() / v.x(); t1 = w2.x() / v.x(); }
181  else { t0 = w.y() / v.y(); t1 = w2.y() / v.y(); }
182  if (t0 > t1) { // must have t0 smaller than t1
183  float t=t0; t0=t1; t1=t; // swap if not
184  }
185  if (t0 > 1 || t1 < 0) {
186  return std::shared_ptr<nrt::Point2D<TP> >(); // NO overlap
187  }
188  t0 = t0<0? 0 : t0; // clip to min 0
189  t1 = t1>1? 1 : t1; // clip to max 1
190  if (t0 == t1) // intersect is a point
191  return std::shared_ptr<nrt::Point2D<TP> >(new nrt::Point2D<TP>(nrt::Point2D<TP>(other.p1()) + t0 * v));
192 
193  // they overlap in a valid subsegment
194  return std::shared_ptr<nrt::Point2D<TP> >(new nrt::Point2D<TP>(nrt::Point2D<TP>(other.p1()) + t0 * v));
195  }
196 
197  // the segments are skew and may intersect in a point
198  // get the intersect parameter for (*this)
199  TP sI = perp(v,w) / D;
200  if (sI < 0 || sI > 1) // no intersect with (*this)
201  return std::shared_ptr<nrt::Point2D<TP> >();
202 
203  // get the intersect parameter for other
204  TP tI = perp(u,w) / D;
205  if (tI < 0 || tI > 1) // no intersect with other
206  return std::shared_ptr<nrt::Point2D<TP> >();
207 
208  // compute (*this) intersect point
209  return std::shared_ptr<nrt::Point2D<TP> >(new nrt::Point2D<TP>(nrt::Point2D<TP>(this->p1()) + sI * u));
210 }
211 
212 // ######################################################################
213 // Formula from http://en.wikipedia.org/wiki/Line-line_intersection
214 template <class T> template <typename promo>
215 std::shared_ptr<nrt::Point2D<typename nrt::promote<T, promo>::type> >
217 {
218  typedef typename nrt::promote<T, promo>::type TP;
219 
220  if (angle() == other.angle()) return std::shared_ptr<Point2D<T>>();
221  T const x1 = itsP1.x();
222  T const x2 = itsP2.x();
223  T const x3 = other.itsP1.x();
224  T const x4 = other.itsP2.x();
225  T const y1 = itsP1.y();
226  T const y2 = itsP2.y();
227  T const y3 = other.itsP1.y();
228  T const y4 = other.itsP2.y();
229 
230  TP x = TP((x1*y2-y1*x2)*(x3-x4) - (x1-x2)*(x3*y4-y3*x4)) / TP((x1-x2)*(y3-y4) - (y1-y2)*(x3-x4));
231  TP y = TP((x1*y2-y1*x2)*(y3-y4) - (y1-y2)*(x3*y4-y3*x4)) / TP((x1-x2)*(y3-y4) - (y1-y2)*(x3-x4));
232 
233  return std::make_shared<nrt::Point2D<TP> >(x,y);
234 }
235 
236 // ######################################################################
237 template <class T> inline
239 {
240  const double l2 = itsP1.squaredDistanceTo( itsP2 );
241 
242  if( l2 == 0.0 ) // p1 == p2 case
243  return itsP1.distanceTo( point );
244 
245  // consider the line extending the segment parameterized as p1 + t( p2 - p1 )
246  // find projection of point onto the line
247  // it falls where t = [(point - p1) DOT (p2 - p1)] / |p2-p1|^2
248  const float t = dot( Point2D<T>(point - itsP1), Point2D<T>(itsP2 - itsP1) ) / l2;
249 
250  if( ( t < 0.0 ) || ( t > 1.0 ) ) // point would intersect outside of the line segment
251  return std::numeric_limits<double>::infinity();
252 
253  // projection lands on our segment
254  const Point2D<T> projection = itsP1 + ( t * ( itsP2 - itsP1 ) );
255 
256  return point.distanceTo( projection );
257 }
258 
259 // ######################################################################
260 template <class T> inline
261 std::ostream& nrt::operator<<(std::ostream &out, nrt::Line<T> const& line)
262 {
263  out << "{" << line.p1() << ", " << line.p2() << "}";
264  return out;
265 }
266 
267 // ######################################################################
268 template <class T> inline
269 nrt::Line<T> nrt::scale(nrt::Line<T> const & src, double const factor)
270 { return nrt::Line<T>(src.center(), src.angle(), src.length() * factor); }
271 
272 // ######################################################################
273 template <class T> inline
274 nrt::Line<T> nrt::rotate(nrt::Line<T> const & src, double const angle)
275 { return nrt::Line<T>(src.center(), src.angle() + angle, src.length()); }
276 
277 // ######################################################################
278 template <class T> inline
279 nrt::Line<T> nrt::rotateAbout(nrt::Line<T> const & src, nrt::Point2D<T> const & p, double const angle)
280 { return nrt::Line<T>(nrt::rotateAbout(src.p1(), p, angle), nrt::rotateAbout(src.p2(), p, angle)); }
281 
282 // ######################################################################
283 template <class T> inline
284 bool nrt::operator==(nrt::Line<T> const & lhs, nrt::Line<T> const & rhs)
285 {
286  return ( ((lhs.p1() == rhs.p1()) && (lhs.p2() == rhs.p2())) ||
287  ((lhs.p1() == rhs.p2()) && (lhs.p2() == rhs.p1())) );
288 }
289 
290 // ######################################################################
291 template <class T> inline
292 bool nrt::operator!=(nrt::Line<T> const & lhs, nrt::Line<T> const & rhs)
293 { return ( ! (lhs == rhs) ); }
294 
295 // ######################################################################
296 template <class T> inline
298 { return nrt::Line<T>(lhs.p1() + rhs, lhs.p2() + rhs); }
299 
300 // ######################################################################
301 template <class T> inline
303 { return nrt::Line<T>(lhs + rhs.p1(), lhs + rhs.p2()); }
304 
305 // ######################################################################
306 template <class T> inline
308 { return nrt::Line<T>(lhs.p1() - rhs, lhs.p2() - rhs); }
309 
310 // ######################################################################
311 template <class T> inline
313 { return nrt::Line<T>(lhs - rhs.p1(), lhs - rhs.p2()); }
314 
315 // ######################################################################
316 template <class T> inline
317 nrt::Line<T> nrt::operator*(nrt::Line<T> const & lhs, double const rhs)
318 { return nrt::Line<T>(lhs.p1() * rhs, lhs.p2() * rhs); }
319 
320 // ######################################################################
321 template <class T> inline
322 nrt::Line<T> nrt::operator*(double const lhs, nrt::Line<T> const & rhs)
323 { return nrt::Line<T>(lhs * rhs.p1(), lhs * rhs.p2()); }
324 
325 // ######################################################################
326 template <class T> inline
327 nrt::Line<T> nrt::operator/(nrt::Line<T> const & lhs, double const rhs)
328 { return nrt::Line<T>(lhs.p1() / rhs, lhs.p2() / rhs); }
329 
330 // ######################################################################
331 template <class T> inline
332 nrt::Line<T> & nrt::operator*=(nrt::Line<T> & lhs, double const rhs)
333 {
334  lhs = nrt::Line<T>(lhs.p1() * rhs, lhs.p2() * rhs);
335  return lhs;
336 }
337 
338 // ######################################################################
339 template <class T> inline
340 nrt::Line<T> & nrt::operator/=(nrt::Line<T> & lhs, double const rhs)
341 {
342  lhs = nrt::Line<T>(lhs.p1() / rhs, lhs.p2() / rhs);
343  return lhs;
344 }
345 
346 // ######################################################################
347 template <class T> inline
349 {
350  lhs = nrt::Line<T>(lhs.p1() + rhs, lhs.p2() + rhs);
351  return lhs;
352 }
353 
354 // ######################################################################
355 template <class T> inline
357 {
358  lhs = nrt::Line<T>(lhs.p1() - rhs, lhs.p2() - rhs);
359  return lhs;
360 }
361