Line2D.cpp
Go to the documentation of this file.
1 //
2 // Line2D.cpp
3 //
4 
5 #include <cmath>
6 
7 #include "Line2D.hpp"
8 
9 #include "Point2D.hpp"
10 #include "Polygon2D.hpp"
11 
12 
13 namespace datatypes
14 {
15 
16 
18 {
19  return Polygon2D(first, second);
20 }
21 
22 Polygon2D Line2D::toPolygon2D(unsigned samplePoints) const
23 {
24  assert(samplePoints > 1);
25  if (samplePoints <= 1)
26  return Polygon2D(); // sanity check for when NDEBUG disables the assert() check
27 
28  Polygon2D result;
29  result.resize(samplePoints);
30  result.front() = first;
31  result.back() = second;
32 
33  Point2D step = getDiff() / double(samplePoints - 1);
34  for (unsigned k = 1; k < samplePoints - 1; ++k)
35  {
36  result[k] = first + step * double(k);
37  }
38 
39  return result;
40 }
41 
42 double Line2D::getInclination() const
43 {
44  Point2D diff = getDiff();
45  if (fuzzyCompare(diff.getX(), 0.0))
46  {
47  // vertical line
48  if (first.getY() < second.getY())
49  return M_PI_2; // pi/2
50  else
51  return -M_PI_2; // -pi/2
52  }
53  return atan2(diff.getY(), diff.getX());
54 }
55 
57 {
58  Point2D diff = getDiff();
59  double thislength = getLength();
60 
61  return Line2D(first,
62  Point2D(first.getX() + diff.getX() / thislength,
63  first.getY() + diff.getY() / thislength));
64 }
65 
66 std::string Line2D::toString()
67 {
68  std::string text;
69 
70  text = "[" + getP1().toString() + " -> " +getP2().toString() + "]";
71 
72  return text;
73 }
74 
75 bool Line2D::containsPoint(const Point2D& point) const
76 {
77  double d = distanceFromLineSegment(point);
78  return fuzzyCompare(d, 0.0);
79 }
80 
81 double Line2D::distanceToPoint(const Point2D& point) const
82 {
83  Point2D diff = point - projectOntoLine(point);
84  return hypot(diff.getX(), diff.getY());
85 }
86 
87 double dot_product(const Point2D& p1, const Point2D& p2)
88 {
89  return p1.getX() * p2.getX() + p1.getY() * p2.getY();
90 }
92 {
93  Point2D line_vec = getDiff();
94  double line_length = length();
95  if (!fuzzyCompare(line_length, 0.0))
96  {
97  Point2D line_unitvec = line_vec / line_length;
98  Point2D vec_to_point = point - first;
99  double length_of_projection =
100  dot_product(line_unitvec, vec_to_point);
101  return first + line_unitvec * length_of_projection;
102  }
103  else
104  {
105  // Zero-length line; what should we do?
106  return point;
107  }
108 }
109 
110 double Line2D::distanceFromLineSegment(const Point2D& point) const
111 {
112 // How do I find the distance from a point to a line?
113 //
114 // Let the point be C (Cx,Cy) and the line be AB (Ax,Ay) to (Bx,By).
115 // Let P be the point of perpendicular projection of C on AB. The parameter
116 // r, which indicates P's position along AB, is computed by the dot product
117 // of AC and AB divided by the square of the length of AB:
118 //
119 // (1) AC dot AB
120 // r = ---------
121 // ||AB||^2
122 //
123 // r has the following meaning:
124 //
125 // r=0 P = A
126 // r=1 P = B
127 // r<0 P is on the backward extension of AB
128 // r>1 P is on the forward extension of AB
129 // 0<r<1 P is interior to AB
130 //
131 // The length of a line segment in d dimensions, AB is computed by:
132 //
133 // L = sqrt( (Bx-Ax)^2 + (By-Ay)^2 + ... + (Bd-Ad)^2)
134 //
135 // so in 2D:
136 //
137 // L = sqrt( (Bx-Ax)^2 + (By-Ay)^2 )
138 //
139 // and the dot product of two vectors in d dimensions, U dot V is computed:
140 //
141 // D = (Ux * Vx) + (Uy * Vy) + ... + (Ud * Vd)
142 //
143 // so in 2D:
144 //
145 // D = (Ux * Vx) + (Uy * Vy)
146 //
147 // So (1) expands to:
148 //
149 // (Cx-Ax)(Bx-Ax) + (Cy-Ay)(By-Ay)
150 // r = -------------------------------
151 // L^2
152 //
153 // The point P can then be found:
154 //
155 // Px = Ax + r(Bx-Ax)
156 // Py = Ay + r(By-Ay)
157 //
158 // And the distance from A to P = r*L.
159 //
160 // Use another parameter s to indicate the location along PC, with the
161 // following meaning:
162 // s<0 C is left of AB
163 // s>0 C is right of AB
164 // s=0 C is on AB
165 //
166 // Compute s as follows:
167 //
168 // (Ay-Cy)(Bx-Ax)-(Ax-Cx)(By-Ay)
169 // s = -----------------------------
170 // L^2
171 //
172 //
173 // Then the distance from C to P = |s|*L.
174 
175  const double cx = point.getX();
176  const double cy = point.getY();
177 
178  const double ax = getP1().getX();
179  const double ay = getP1().getY();
180  const double bx = getP2().getX();
181  const double by = getP2().getY();
182 
183  const double r_numerator = (cx - ax) * (bx - ax) + (cy - ay) * (by - ay);
184  const double r_denomenator = (bx - ax) * (bx - ax) + (by - ay) * (by - ay);
185  const double r = r_numerator / r_denomenator;
186 
187 // const double px = ax + r * (bx - ax);
188 // const double py = ay + r * (by - ay);
189 
190  const double s = ((ay - cy) * (bx - ax) - (ax - cx) * (by - ay) ) / r_denomenator;
191 
192  const double distanceLine = std::abs(s) * sqrt(r_denomenator);
193  double distanceSegment = -1.0;
194 
195  // (xx,yy) is the point on the lineSegment closest to (cx,cy)
196 
197 // double xx = px;
198 // double yy = py;
199 
200  if ( (r >= 0) && (r <= 1) )
201  {
202  distanceSegment = distanceLine;
203  }
204  else
205  {
206  double dist1 = (cx - ax) * (cx - ax) + (cy - ay) * (cy - ay);
207  double dist2 = (cx - bx) * (cx - bx) + (cy - by) * (cy - by);
208  if (dist1 < dist2)
209  {
210 // xx = ax;
211 // yy = ay;
212  distanceSegment = sqrt(dist1);
213  }
214  else
215  {
216 // xx = bx;
217 // yy = by;
218  distanceSegment = sqrt(dist2);
219  }
220  }
221 
222  return distanceSegment;
223 }
224 
225 
226 // Line intersection algorithm, copied from Graphics Gems II
227 #define SAME_SIGNS(a, b) ((a) * (b) >= 0)
228 template<typename FloatT>
229 static bool line2d_intersect(FloatT x1, FloatT y1, FloatT x2, FloatT y2,
230  FloatT x3, FloatT y3, FloatT x4, FloatT y4)
231 {
232  FloatT a1, a2, b1, b2, c1, c2; /* Coefficients of line eqns. */
233  FloatT r1, r2, r3, r4; /* 'Sign' values */
234 
235  a1 = y2 - y1;
236  b1 = x1 - x2;
237  c1 = x2 * y1 - x1 * y2;
238 
239  r3 = a1 * x3 + b1 * y3 + c1;
240  r4 = a1 * x4 + b1 * y4 + c1;
241 
242  if ( r3 != 0 && r4 != 0 && SAME_SIGNS( r3, r4 ))
243  return false;
244 
245  a2 = y4 - y3;
246  b2 = x3 - x4;
247  c2 = x4 * y3 - x3 * y4;
248 
249  r1 = a2 * x1 + b2 * y1 + c2;
250  r2 = a2 * x2 + b2 * y2 + c2;
251 
252  if ( r1 != 0 && r2 != 0 && SAME_SIGNS( r1, r2 ))
253  return false;
254 
255  return true;
256 }
257 
259 {
260  if (isZero() || other.isZero())
261  return NotIntersecting;
262 
263  bool has_inside_intersection =
265  second.getX(), second.getY(),
266  other.first.getX(), other.first.getY(),
267  other.second.getX(), other.second.getY());
268 
269  bool is_vertical = fuzzyCompare(getDiff().getX(), 0.0);
270  bool other_is_vertical = fuzzyCompare(other.getDiff().getX(), 0.0);
271  IntersectingType result =
272  has_inside_intersection ? LineIntersecting : OutsideIntersecting;
273  Point2D intersectPoint;
274 
275  if (is_vertical && other_is_vertical)
276  result = NotIntersecting;
277  else if (is_vertical)
278  {
279  Point2D otherdiff = other.getDiff();
280  double oa = otherdiff.getY() / otherdiff.getX();
281  intersectPoint.setXY(first.getX(),
282  oa * first.getX() + other.first.getY() - oa * other.first.getX());
283  }
284  else if (other_is_vertical)
285  {
286  Point2D diff = getDiff();
287  double a = diff.getY() / diff.getX();
288  intersectPoint.setXY(other.first.getX(),
289  a * other.first.getX() + first.getY() - a * first.getX());
290  }
291  else
292  {
293  Point2D otherdiff = other.getDiff();
294  double oa = otherdiff.getY() / otherdiff.getX();
295  Point2D diff = getDiff();
296  double ta = diff.getY() / diff.getX();
297  if (oa == ta) // parallel lines
298  return NotIntersecting;
299  double x = ( - other.first.getY() + oa * other.first.getX()
300  + first.getY() - ta * first.getX() ) / (oa - ta);
301  intersectPoint.setXY(x, ta * (x - first.getX()) + first.getY());
302  }
303 
304  if (point)
305  *point = intersectPoint;
306  return result;
307 }
308 
310 {
311  if (points.size() >= 2)
312  {
313  // y = a + b*x
314 
315  const Point2D mean(points.getCenterOfGravity());
316 
317  double SSxy = 0;
318  double SSxx = 0;
319  double minX = points.front().getX();
320  double maxX = minX;
321 
322  for (Polygon2D::const_iterator point = points.begin(); point != points.end(); ++point)
323  {
324  minX = std::min(minX, double(point->getX()));
325  maxX = std::max(maxX, double(point->getX()));
326  SSxy += (point->getX() - mean.getX()) * (point->getY() - mean.getY());
327  SSxx += (point->getX() - mean.getX()) * (point->getX() - mean.getX());
328  }
329 
330  const double b(SSxy / SSxx);
331  const double a(mean.getY() - b * mean.getX());
332 
333  return Line2D(Point2D(minX, a + b * minX), Point2D(maxX, a + b * maxX));
334  }
335 
336  return Line2D();
337 }
338 
340 {
341  return getP1() + (0.5 * getDiff());
342 }
343 
344 } // namespace datatypes
static bool line2d_intersect(FloatT x1, FloatT y1, FloatT x2, FloatT y2, FloatT x3, FloatT y3, FloatT x4, FloatT y4)
Definition: Line2D.cpp:229
std::string toString(UINT16 digits=2) const
Text output for debugging.
Definition: Point2D.cpp:75
double dot_product(const Point2D &p1, const Point2D &p2)
Definition: Line2D.cpp:87
IntersectingType
Describes how two lines can be intersecting.
Definition: Line2D.hpp:35
value_type getX() const
Definition: Point2D.hpp:70
Point2D getCenterOfGravity() const
Returns the center of gravity of this polygon.
Definition: Polygon2D.cpp:108
double getLength() const
Definition: Line2D.hpp:93
#define SAME_SIGNS(a, b)
Definition: Line2D.cpp:227
double hypot(double x, double y, double z)
Definition: MathToolbox.cpp:19
const Point2D & getP1() const
Returns the first point.
Definition: Line2D.hpp:83
Line2D()
Empty constructor.
Definition: Line2D.hpp:44
A polygon of 2D-points.
Definition: Polygon2D.hpp:43
double distanceToPoint(const Point2D &point) const
Returns the distance of the given point to its orthogonal projection onto this line.
Definition: Line2D.cpp:81
Point2D second
The second point of this line.
Definition: Line2D.hpp:195
value_type getY() const
Definition: Point2D.hpp:73
bool containsPoint(const Point2D &point) const
Returns true if this line "contains" the given point.
Definition: Line2D.cpp:75
Point2D getDiff() const
Returns the difference between line end and line start as a Point2D.
Definition: Line2D.hpp:109
void setXY(value_type x, value_type y)
Definition: Point2D.hpp:147
The lines are not intersecting, i.e. they are parallel or zero.
Definition: Line2D.hpp:37
std::string toString()
Definition: Line2D.cpp:66
Polygon2D toPolygon2D() const
Conversion to Polygon2D.
Definition: Line2D.cpp:17
Point2D projectOntoLine(const Point2D &point) const
Definition: Line2D.cpp:91
A line in the two-dimensional plane, composed out of two points.
Definition: Line2D.hpp:24
Point2D first
The first point of this line.
Definition: Line2D.hpp:193
static Line2D fromLinearRegression(const Polygon2D &points)
Returns a Line2D from several points using linear regression.
Definition: Line2D.cpp:309
double distanceFromLineSegment(const Point2D &point) const
Returns the distance of a point to this line segment.
Definition: Line2D.cpp:110
bool fuzzyCompare(double a, double b)
Tests if two double values are nearly equal.
Definition: MathToolbox.hpp:28
bool isZero() const
Returns true if both points are zero.
Definition: Line2D.hpp:80
The lines are intersecting, but outside of their line segments.
Definition: Line2D.hpp:39
IntersectingType isIntersecting(const Line2D &other, Point2D *intersectingPoint=NULL) const
Calculates the intersection point between two lines.
Definition: Line2D.cpp:258
The lines are intersecting within their line segments.
Definition: Line2D.hpp:38
Point2D getCenterPoint() const
Returns the point in the middle between first and second point.
Definition: Line2D.cpp:339
double length() const
Definition: Line2D.hpp:97
Line2D getUnitVector() const
Returns a unit vector for this line.
Definition: Line2D.cpp:56
const Point2D & getP2() const
Returns the second point.
Definition: Line2D.hpp:86
double getInclination() const
Definition: Line2D.cpp:42


libsick_ldmrs
Author(s): SICK AG , Martin Günther , Jochen Sprickerhof
autogenerated on Sat Jun 8 2019 17:57:33