CGR Localization
 All Classes Namespaces Files Functions Variables Macros Pages
line.h
Go to the documentation of this file.
1 //========================================================================
2 // This software is free: you can redistribute it and/or modify
3 // it under the terms of the GNU Lesser General Public License Version 3,
4 // as published by the Free Software Foundation.
5 //
6 // This software is distributed in the hope that it will be useful,
7 // but WITHOUT ANY WARRANTY; without even the implied warranty of
8 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
9 // GNU Lesser General Public License for more details.
10 //
11 // You should have received a copy of the GNU Lesser General Public License
12 // Version 3 in the file COPYING that came with this distribution.
13 // If not, see <http://www.gnu.org/licenses/>.
14 //========================================================================
20 //========================================================================
21 
22 #include <float.h>
23 #include "geometry.h"
24 
25 #ifndef LINE_H
26 #define LINE_H
27 
28 #define LINE_TEM template <class num> inline
29 #define LINE_FUN Line2d<num>
30 
31 #define LazyCaching
32 //using namespace GVector;
33 template <class num> class Line2d;
34 
35 template <class num>
36 class Line2d{
37 private:
39  GVector::vector2d<num> p0,p1;
41  mutable GVector::vector2d<num> dir;
43  mutable GVector::vector2d<num> perp;
45  mutable num length;
47  mutable bool updateRequired;
49  static const num eps = 1e-5;
50 
51 public:
52  //Constructors, Setters
53  Line2d();
54  Line2d(GVector::vector2d<num> _p1);
55  Line2d(GVector::vector2d<num> _p0, GVector::vector2d<num> _p1);
56  Line2d(num p0x, num p0y, num p1x, num p1y);
57  void set(GVector::vector2d<num> _p0, GVector::vector2d<num> _p1);
58  void set(num p0x, num p0y, num p1x, num p1y);
60  void calcValues() const;
61 
62  //Common Math Functions related to lines
64  num closestDistFromLine(GVector::vector2d<num> p, bool endcaps);
66  num distFromLine(GVector::vector2d< num > p, GVector::vector2d< num > rayDir, bool extendLine);
68  num distFromLine1(GVector::vector2d<num> p, num rayDir, bool extendLine);
70  GVector::vector2d<num> perpFromLine(GVector::vector2d<num> p, bool endcaps);
72  bool liesAlongside(GVector::vector2d<num> p, num margin = 0.0);
74  bool intersects(Line2d<num> l1, bool extendL0, bool extendL1, bool touch);
76  bool intersects(GVector::vector2d< num > p2, GVector::vector2d< num > p3, bool extendL0, bool extendL1, bool touch);
78  bool intersects(GVector::vector2d<num> v, bool extendL0, bool extendL1){return intersects(Line2d<num>(v), extendL0, extendL1);}
80  bool intersects(GVector::vector2d<num> p, GVector::vector2d<num> rayDir, bool touching);
82  bool intersects(GVector::vector2d<num> p, num rayDir);
84  GVector::vector2d<num> intersectTest(GVector::vector2d< num > p2, GVector::vector2d< num > p3, bool &intersects, bool extendL0, bool extendL1);
86  GVector::vector2d<num> intersection(Line2d<num> l1, bool extendL0, bool extendL1);
88  GVector::vector2d<num> intersection(GVector::vector2d< num > p2, GVector::vector2d< num > p3, bool extendL0, bool extendL1);
90  Line2d<num> rotate(GVector::vector2d<num> p, num angle);
92  GVector::vector2d<num> project_in(GVector::vector2d<num> p);
94  GVector::vector2d<num> project_out(GVector::vector2d<num> p);
96  char* ToString(char* str);
97 
98  //Getters
99  inline const GVector::vector2d<num>& P0() const {return p0;}
100  inline const GVector::vector2d<num>& P1() const {return p1;}
101 #ifdef LazyCaching
102  inline const GVector::vector2d<num>& Dir() const {if(updateRequired) calcValues(); return dir;}
103  inline const GVector::vector2d<num>& Perp() const {if(updateRequired) calcValues(); return perp;}
104  inline const num Length() const {if(updateRequired) calcValues(); return length;}
105  inline const num Angle() const {if(updateRequired) calcValues(); return dir.angle();}
106 #else
107  inline GVector::vector2d<num> Dir() const {return (p1-p0).norm();}
108  inline GVector::vector2d<num> Perp() const {return (p1-p0).perp().norm();}
109  inline num Length() const {return (p1-p0).length();}
110  inline num Angle() const {return (p1-p0).angle();}
111 #endif
112 
113  //Operator overloads
115  Line2d<num> operator*(num f) const;
117  Line2d<num> operator/(num f) const;
119  Line2d<num> &operator*=(num f);
121  Line2d<num> &operator/=(num f);
123  Line2d<num> operator+(GVector::vector2d<num> v) const;
124  Line2d<num> operator-(GVector::vector2d<num> v) const;
126  Line2d<num> &operator+=(GVector::vector2d<num> v);
127  Line2d<num> &operator-=(GVector::vector2d<num> v);
128 };
129 
130 typedef Line2d<double> line2d;
131 typedef Line2d<float> line2f;
132 typedef Line2d<int> line2i;
133 
134 LINE_TEM
135 LINE_FUN::Line2d()
136 {
137  p0.zero();
138  p1.zero();
139 #ifdef LazyCaching
140  //calcValues();
141  updateRequired = true;
142 #endif
143 }
144 
145 LINE_TEM
146 LINE_FUN::Line2d(GVector::vector2d< num > _p1)
147 {
148  p0.zero();
149  p1 = _p1;
150 #ifdef LazyCaching
151  //calcValues();
152  updateRequired = true;
153 #endif
154 }
155 
156 LINE_TEM
157 LINE_FUN::Line2d(GVector::vector2d< num > _p0, GVector::vector2d< num > _p1)
158 {
159  p0 = _p0;
160  p1 = _p1;
161 #ifdef LazyCaching
162  //calcValues();
163  updateRequired = true;
164 #endif
165 }
166 
167 LINE_TEM
168 LINE_FUN::Line2d(num p0x, num p0y, num p1x, num p1y)
169 {
170  p0.set(p0x, p0y);
171  p1.set(p1x, p1y);
172 #ifdef LazyCaching
173  //calcValues();
174  updateRequired = true;
175 #endif
176 }
177 
178 LINE_TEM
179 void LINE_FUN::set(GVector::vector2d<num> _p0, GVector::vector2d<num> _p1)
180 {
181  p0 = _p0;
182  p1 = _p1;
183 #ifdef LazyCaching
184  //calcValues();
185  updateRequired = true;
186 #endif
187 }
188 
189 LINE_TEM
190 void LINE_FUN::set(num p0x, num p0y, num p1x, num p1y)
191 {
192  p0.set(p0x, p0y);
193  p1.set(p1x, p1y);
194 #ifdef LazyCaching
195  //calcValues();
196  updateRequired = true;
197 #endif
198 }
199 
200 LINE_TEM
201 void LINE_FUN::calcValues() const
202 {
203  length = (p1-p0).length();
204  dir = (p1-p0)/length;
205  perp = dir.perp();
206  updateRequired = false;
207 }
208 
209 LINE_TEM
210 num LINE_FUN::closestDistFromLine(GVector::vector2d<num> p, bool endcaps)
211 {
212 #ifdef LazyCaching
213  if(updateRequired) calcValues();
214 #else
215  calcValues();
216 #endif
217  num dist = fabs(perp.dot(p-p0));
218  num locationOnLine = dir.dot(p-p0);
219 
220  if(locationOnLine<0.0 && endcaps){
221  return (p-p0).length();
222  }else if(locationOnLine>length && endcaps){
223  return (p-p1).length();
224  }
225  return dist;
226 }
227 
228 LINE_TEM
229 num LINE_FUN::distFromLine(GVector::vector2d<num> p, GVector::vector2d<num> rayDir, bool extendLine)
230 {
231  #ifdef LazyCaching
232  if(updateRequired) calcValues();
233  #else
234  calcValues();
235  #endif
236  num result = 0.0;
237  if(intersects(p,rayDir) || extendLine){
238  num sinTheta = dir.cross(rayDir.norm());
239  result = fabs(perp.dot(p-p0)/sinTheta);
240  }else{
241  result = NAN;
242  }
243  if(result<0.0){
244  printf("Fail2! %d %f %f %f\n",(intersects(p,rayDir) || extendLine)?1:0,dir.cross(rayDir.norm()), perp.dot(p-p0), result);
245  fflush(stdout);
246  }
247  return result;
248 }
249 
250 LINE_TEM
251 num LINE_FUN::distFromLine1(GVector::vector2d<num> p, num rayDir, bool extendLine)
252 {
253  #ifdef LazyCaching
254  if(updateRequired) calcValues();
255  #else
256  calcValues();
257  #endif
258  calcValues();
259  GVector::vector2d<num> heading;
260  heading.heading(rayDir);
261  num result = 0.0;
262  if(intersects(p,heading,false) || extendLine){
263  num sinTheta = dir.cross(heading.norm());
264  result = fabs(perp.dot(p-p0)/sinTheta);
265  }else{
266  result = NAN;
267  }
268  if(result<0.0){
269  printf("Fail1! %d %f %f %f\n",(intersects(p,heading,false) || extendLine)?1:0,dir.cross(heading.norm()), perp.dot(p-p0), result);
270  fflush(stdout);
271  }
272  return result;
273 }
274 
275 LINE_TEM
276 GVector::vector2d<num> LINE_FUN::perpFromLine(GVector::vector2d<num> p, bool endcaps)
277 {
278  #ifdef LazyCaching
279  if(updateRequired) calcValues();
280  #else
281  calcValues();
282  #endif
283  num d = dir.dot(p-p0);
284  if((d>=0.0 && d<=length) || !endcaps){
285  // Lies (alongside line), or (beyond line but using extension of line)
286  return ( (p-p0) - dir*d );
287  }else{
288  // Beyond line and using endcaps
289  if(d<0.0)
290  return (p-p0);
291  else
292  return (p-p1);
293  }
294 }
295 
296 LINE_TEM
297 bool LINE_FUN::liesAlongside(GVector::vector2d<num> p, num margin)
298 {
299  #ifdef LazyCaching
300  if(updateRequired) calcValues();
301  #else
302  calcValues();
303  #endif
304  num location = (p-p0).dot(dir);
305  return (location>-margin && location<length+margin);
306 }
307 
308 LINE_TEM
309 bool LINE_FUN::intersects(GVector::vector2d< num > p2, GVector::vector2d< num > p3, bool extendL0, bool extendL1, bool touch)
310 {
311  #ifdef LazyCaching
312  if(updateRequired) calcValues();
313  #else
314  calcValues();
315  #endif
316  //GVector::vector2d<num> perp1 = perp;//(p1-p0).perp();
317  //Check to see if lines intersect within their extents, or if it is okay to extend lines
318  if(touch)
319  return ( (extendL1 || (perp.dot(p2-p0)*perp.dot(p3-p0)<=eps)) && (extendL0 || ((p3-p2).perpdot(p0-p2)*(p3-p2).perpdot(p1-p2)<=eps)) );
320  else
321  return ( (extendL1 || (perp.dot(p2-p0)*perp.dot(p3-p0)<0.0)) && (extendL0 || ((p3-p2).perpdot(p0-p2)*(p3-p2).perpdot(p1-p2)<0.0)) );
322 }
323 
324 LINE_TEM
325 GVector::vector2d<num> LINE_FUN::intersectTest(GVector::vector2d< num > p2, GVector::vector2d< num > p3, bool &intersects, bool extendL0, bool extendL1)
326 {
327  #ifdef LazyCaching
328  if(updateRequired) calcValues();
329  #else
330  calcValues();
331  #endif
332  GVector::vector2d<num> dir2 = (p3-p2).norm();
333 
334  num den = dir.y*dir2.x - dir.x*dir2.y;
335  num ua = (dir.y*(p0.x-p2.x) - dir.x*(p0.y-p2.y))/den;
336 
337  GVector::vector2d<num> p = p2 + ua*dir2;
338  num position0 = dir.dot(p-p0);
339  num position1 = dir2.dot(p-p2);
340 
341  intersects = true;
342  if( (!extendL0 && (position0<0.0 || position0>length)) || (!extendL1 && (sq(position1)>(p3-p2).sqlength())) )
343  intersects = false;
344 
345  return p;
346 }
347 
348 LINE_TEM
349 bool LINE_FUN::intersects(Line2d<num> l2, bool extendL0, bool extendL1, bool touch)
350 {
351  #ifdef LazyCaching
352  if(updateRequired) calcValues();
353  #else
354  calcValues();
355  #endif
356  //GVector::vector2d<num> perp1 = perp;//(p1-p0).perp();
357  //Check to see if lines intersect within their extents, or if it is okay to extend lines
358  if(touch)
359  return ( (extendL1 || (perp.dot(l2.P0()-p0)*perp.dot(l2.P1()-p0)<=eps)) && (extendL0 || (l2.Perp().dot(p0-l2.P0())*l2.Perp().dot(p1-l2.P0())<=eps)) );
360  else
361  return ( (extendL1 || (perp.dot(l2.P0()-p0)*perp.dot(l2.P1()-p0)<-eps)) && (extendL0 || (l2.Perp().dot(p0-l2.P0())*l2.Perp().dot(p1-l2.P0())<-eps)) );
362 }
363 
364 LINE_TEM
365 bool LINE_FUN::intersects(GVector::vector2d< num > p, GVector::vector2d< num > rayDir, bool touching)
366 {
367  #ifdef LazyCaching
368  if(updateRequired) calcValues();
369  #else
370  calcValues();
371  #endif
372  GVector::vector2d<num> v0 = p0-p;
373  GVector::vector2d<num> v1 = p1-p;
374  if(v0.cross(v1)<0.0)
375  swap(v0,v1);
376 
377  if(touching)
378  return ( v0.cross(rayDir)>=0.0 && v1.cross(rayDir) <= 0.0 ); //Check if rayDir lies within the angle reqion between v0 and v1
379  else
380  return ( v0.cross(rayDir)>eps && v1.cross(rayDir) < -eps ); //Check if rayDir lies within the angle reqion between v0 and v1
381 }
382 
383 LINE_TEM
384 bool LINE_FUN::intersects(GVector::vector2d<num> p, num rayDir)
385 {
386  #ifdef LazyCaching
387  if(updateRequired) calcValues();
388  #else
389  calcValues();
390  #endif
391  GVector::vector2d<num> heading;
392  heading.heading(rayDir);
393 
394  return intersects(p, heading,false);
395 }
396 
397 LINE_TEM
398 GVector::vector2d<num> LINE_FUN::intersection(GVector::vector2d< num > p2, GVector::vector2d< num > p3, bool extendL0, bool extendL1)
399 {
400  #ifdef LazyCaching
401  if(updateRequired) calcValues();
402  #else
403  calcValues();
404  #endif
405  GVector::vector2d<num> dir2 = (p3-p2).norm();
406 
407  num den = dir.y*dir2.x - dir.x*dir2.y;
408  num ua = (dir.y*(p0.x-p2.x) - dir.x*(p0.y-p2.y))/den;
409 
410  GVector::vector2d<num> p = p2 + ua*dir2;
411  num position0 = dir.dot(p-p0);
412  num position1 = dir2.dot(p-p2);
413 
414  if( (!extendL0 && (position0<0.0 || position0>length)) || (!extendL1 && (sq(position1)>(p3-p2).sqlength())) )
415  return GVector::vector2d<num>(NAN,NAN);
416 
417  return p;
418 }
419 
420 LINE_TEM
421 GVector::vector2d<num> LINE_FUN::intersection(Line2d< num > l1, bool extendL0, bool extendL1)
422 {
423  #ifdef LazyCaching
424  if(updateRequired) calcValues();
425  #else
426  calcValues();
427  #endif
428  GVector::vector2d<num> p02 =l1.P0();
429  GVector::vector2d<num> dir2 = l1.Dir();
430 
431  num den = dir.y*dir2.x - dir.x*dir2.y;
432  num ua = (dir.y*(p0.x-p02.x) - dir.x*(p0.y-p02.y))/den;
433 
434  GVector::vector2d<num> p = p02 + ua*dir2;
435  num position0 = dir.dot(p-p0);
436  num position1 = dir2.dot(p-p02);
437 
438  if( (!extendL0 && (position0<0.0 || position0>length)) || (!extendL1 && (position1<0.0 || position1>l1.Length())) )
439  return GVector::vector2d<num>(NAN,NAN);
440 
441  return p;
442 }
443 
444 LINE_TEM
445 Line2d<num> LINE_FUN::rotate(GVector::vector2d<num> p, num angle)
446 {
447  #ifdef LazyCaching
448  if(updateRequired) calcValues();
449  #else
450  calcValues();
451  #endif
452  GVector::vector2d<num> _p0 = p + (p0-p).rotate(angle);
453  GVector::vector2d<num> _p1 = p + (p1-p).rotate(angle);
454  return Line2d<num>(_p0,_p1);
455 }
456 
457 LINE_TEM
458 GVector::vector2d<num> LINE_FUN::project_in(GVector::vector2d<num> p)
459 {
460  #ifdef LazyCaching
461  if(updateRequired) calcValues();
462  #else
463  calcValues();
464  #endif
465  return GVector::vector2d<num>(dir.dot(p-p0),perp.dot(p-p0));
466 }
467 
468 LINE_TEM
469 GVector::vector2d<num> LINE_FUN::project_out(GVector::vector2d<num> p)
470 {
471  #ifdef LazyCaching
472  if(updateRequired) calcValues();
473  #else
474  calcValues();
475  #endif
476  return (dir*p.x + perp*p.y);
477 }
478 
479 LINE_TEM
480 char* LINE_FUN::ToString(char* str)
481 {
482  snprintf(str,64, "(%8.3f,%8.3f) - (%8.3f,%8.3f)",V2COMP(p0),V2COMP(p1));
483  return str;
484 }
485 
486 #define LINE_UNARY_OPERATOR_SCALAR(op) \
487  LINE_TEM \
488  Line2d<num>& LINE_FUN::operator op (num f) \
489  { \
490  p0 = p0 op f; \
491  p1 = p1 op f; \
492  updateRequired = true; \
493  return(*this); \
494  }
495 
496 LINE_UNARY_OPERATOR_SCALAR(*=)
497 LINE_UNARY_OPERATOR_SCALAR(/=)
498 
499 #define LINE_BINARY_OPERATOR_SCALAR(op) \
500  LINE_TEM \
501  Line2d<num> LINE_FUN::operator op (num f) const\
502  { \
503  GVector::vector2d<num> p0_ = p0 op f; \
504  GVector::vector2d<num> p1_ = p1 op f; \
505  return(Line2d<num>(p0_,p1_)); \
506  }
507 
508 LINE_BINARY_OPERATOR_SCALAR(*)
509 LINE_BINARY_OPERATOR_SCALAR(/)
510 
511 #define LINE_BINARY_OPERATOR_VECTOR(op) \
512  LINE_TEM \
513  Line2d<num> LINE_FUN::operator op (GVector::vector2d<num> v) const\
514  { \
515  GVector::vector2d<num> p0_ = p0 op v; \
516  GVector::vector2d<num> p1_ = p1 op v; \
517  return(Line2d<num>(p0_,p1_)); \
518  }
519 
520 LINE_BINARY_OPERATOR_VECTOR(+)
521 LINE_BINARY_OPERATOR_VECTOR(-)
522 
523 #define LINE_UNARY_OPERATOR_VECTOR(op) \
524  LINE_TEM \
525  Line2d<num>& LINE_FUN::operator op (GVector::vector2d<num> v) \
526  { \
527  p0 = p0 op v; \
528  p1 = p1 op v; \
529  updateRequired = true; \
530  return(*this); \
531  }
532 
533 LINE_UNARY_OPERATOR_VECTOR(+=)
534 LINE_UNARY_OPERATOR_VECTOR(-=)
535 
536 #endif //LINE_H
Line2d::operator+
Line2d< num > operator+(GVector::vector2d< num > v) const
returns this line translated by vector v
Line2d::operator/
Line2d< num > operator/(num f) const
returns this line scaled by 1/f
Line2d::distFromLine1
num distFromLine1(GVector::vector2d< num > p, num rayDir, bool extendLine)
Calculate distance of p along the direction angle rayDir from this line.
Line2d::operator*
Line2d< num > operator*(num f) const
returns this line scaled by f
Line2d
Definition: line.h:33
Line2d::ToString
char * ToString(char *str)
Returns a string formated representation of the line.
Line2d::intersectTest
GVector::vector2d< num > intersectTest(GVector::vector2d< num > p2, GVector::vector2d< num > p3, bool &intersects, bool extendL0, bool extendL1)
Checks if the current line intersects the line joining p2 and p3, and if so, returns the point of int...
Line2d::operator/=
Line2d< num > & operator/=(num f)
scales this line by 1/f
Line2d::rotate
Line2d< num > rotate(GVector::vector2d< num > p, num angle)
Rotates the line about point p by given angle.
Line2d::project_out
GVector::vector2d< num > project_out(GVector::vector2d< num > p)
Projects point p from the coordinate frame attached to the line segment to global coordinates...
Line2d::operator+=
Line2d< num > & operator+=(GVector::vector2d< num > v)
translates this line by vector v
Line2d::distFromLine
num distFromLine(GVector::vector2d< num > p, GVector::vector2d< num > rayDir, bool extendLine)
Calculate distance of p along the direction rayDir from this line.
Line2d::operator*=
Line2d< num > & operator*=(num f)
scales this line by f
Line2d::intersection
GVector::vector2d< num > intersection(Line2d< num > l1, bool extendL0, bool extendL1)
Return the point of intersection of two lines.
Line2d::intersects
bool intersects(GVector::vector2d< num > v, bool extendL0, bool extendL1)
Return true if the ray v from origin intersects this line.
Definition: line.h:78
GVector::vector2d::cross
num cross(const vector2d< num > p) const MustUseResult
return z component of 3D cross product on 2D vectors. right handed.
Definition: gvector.h:968
Line2d::calcValues
void calcValues() const
Calculate derived values (dir, perp, angle)
GVector::vector2d::norm
vector2d< num > norm() const MustUseResult
return a unit length vector in the same direction
Definition: gvector.h:892
Line2d::perpFromLine
GVector::vector2d< num > perpFromLine(GVector::vector2d< num > p, bool endcaps)
Returns a vector perpendicular to the line, starting from the line, terminating at p...
Line2d::intersects
bool intersects(Line2d< num > l1, bool extendL0, bool extendL1, bool touch)
Return true if the line l1 intersects this line. If touch==true then touching counts as intersection...
GVector::vector2d::perp
vector2d< num > perp() const MustUseResult
return the perpendicular of a vector (i.e. rotated 90 deg counterclockwise)
Definition: gvector.h:825
GVector::vector2d::heading
void heading(num angle)
make a unit vector at given angle
Definition: gvector.h:802
GVector::vector2d::dot
num dot(const vec p) const MustUseResult
return dot product of vector with p
Definition: gvector.h:942
Line2d::project_in
GVector::vector2d< num > project_in(GVector::vector2d< num > p)
Projects point p into the coordinate frame attached to the line segment, where the x axis is in the d...
Line2d::closestDistFromLine
num closestDistFromLine(GVector::vector2d< num > p, bool endcaps)
Calculate closest distance of p from this line.
Line2d::liesAlongside
bool liesAlongside(GVector::vector2d< num > p, num margin=0.0)
Returns true if p lies along side this line.