int get_furthest(struct line_pnts *Points, int a, int b, int with_z, double *dist) { int index = a; double d = 0; int i; double x0 = Points->x[a]; double x1 = Points->x[b]; double y0 = Points->y[a]; double y1 = Points->y[b]; double z0 = Points->z[a]; double z1 = Points->z[b]; double px, py, pz, pdist, di; int status; for (i = a + 1; i < b; i++) { di = dig_distance2_point_to_line(Points->x[i], Points->y[i], Points->z[i], x0, y0, z0, x1, y1, z1, with_z, &px, &py, &pz, &pdist, &status); if (di > d) { d = di; index = i; } } *dist = d; return index; }
inline double point_dist_segment_square(POINT a, POINT b, POINT c, int with_z) { double px, py, pz, pdist; int status; return dig_distance2_point_to_line(a.x, a.y, a.z, b.x, b.y, b.z, c.x, c.y, c.z, with_z, &px, &py, &pz, &pdist, &status); }
int lang(struct line_pnts *Points, double thresh, int look_ahead, int with_z) { int count = 1; /* place where the next point will be added. i.e after the last point */ int from = 0; int to = look_ahead; thresh *= thresh; while (from < Points->n_points - 1) { /* repeat until we reach the last point */ int i; int found = 0; /* whether we have found the point outside the threshold */ double x1 = Points->x[from]; double y1 = Points->y[from]; double z1 = Points->z[from]; if (Points->n_points - 1 < to) { /* check that we are always in the line */ to = Points->n_points - 1; } double x2 = Points->x[to]; double y2 = Points->y[to]; double z2 = Points->z[to]; for (i = from + 1; i < to; i++) { double px, py, pz, pdist; int status; if (dig_distance2_point_to_line (Points->x[i], Points->y[i], Points->z[i], x1, y1, z1, x2, y2, z2, with_z, &px, &py, &pz, &pdist, &status) > thresh) { found = 1; break; } } if (found) { to--; } else { Points->x[count] = Points->x[to]; Points->y[count] = Points->y[to]; Points->z[count] = Points->z[to]; count++; from = to; to += look_ahead; } } Points->n_points = count; return Points->n_points; }
double ldist(double x, double y, struct Line *p) { int i; double dist, ndist; i = (p->n_points == 1) ? 0 : 1; dist = dig_distance2_point_to_line(x, y, 0, p->x[0], p->y[0], 0, p->x[i], p->y[i], 0, 0, NULL, NULL, NULL, NULL, NULL); for (i = 1; i < p->n_points - 1; i++) { ndist = dig_distance2_point_to_line(x, y, 0, p->x[i], p->y[i], 0, p->x[i + 1], p->y[i + 1], 0, 0, NULL, NULL, NULL, NULL, NULL); if (ndist < dist) { dist = ndist; } } return (dist); }
/* point_in_buf - test if point px,py is in d buffer of Points ** returns: 1 in buffer ** 0 not in buffer */ static int point_in_buf(struct line_pnts *Points, double px, double py, double d) { int i, np; double sd; np = Points->n_points; d *= d; for (i = 0; i < np - 1; i++) { sd = dig_distance2_point_to_line(px, py, 0, Points->x[i], Points->y[i], 0, Points->x[i + 1], Points->y[i + 1], 0, 0, NULL, NULL, NULL, NULL, NULL); if (sd <= d) { return 1; } } return 0; }
int douglas_peucker(struct line_pnts *Points, double thresh, int with_z) { int *stack = G_malloc(sizeof(int) * Points->n_points * 2); if (!stack) { G_fatal_error(_("Out of memory")); return Points->n_points; } int *index = G_malloc(sizeof(int) * Points->n_points); /* Indices of points in output line */ if (!index) { G_fatal_error(_("Out of memory")); G_free(stack); return Points->n_points; } int top = 2; /* first free slot in the stack */ int icount = 1; /* number of indices stored */ int i; thresh *= thresh; index[0] = 0; /* first point is always in output line */ /* stack contains pairs of elements: (beginning, end) */ stack[0] = 0; stack[1] = Points->n_points - 1; while (top > 0) { /*there are still segments to consider */ /*Pop indices of the segment from the stack */ int last = stack[--top]; int first = stack[--top]; double x1 = Points->x[first]; double y1 = Points->y[first]; double z1 = Points->z[first]; double x2 = Points->x[last]; double y2 = Points->y[last]; double z2 = Points->z[last]; int maxindex = -1; double maxdist = -1; int i; for (i = first + 1; i <= last - 1; i++) { /* Find the furthermost point between first, last */ double px, py, pz, pdist; int status; double dist = dig_distance2_point_to_line(Points->x[i], Points->y[i], Points->z[i], x1, y1, z1, x2, y2, z2, with_z, &px, &py, &pz, &pdist, &status); if (maxindex == -1 || dist > maxdist) { /* update the furthermost point so far seen */ maxindex = i; maxdist = dist; } } if (maxindex == -1 || maxdist <= thresh) { /* no points between or all point are inside the threshold */ index[icount++] = last; } else { /* break line into two parts, the order of pushing is crucial! It gurantees, that we are going to the left */ stack[top++] = maxindex; stack[top++] = last; stack[top++] = first; stack[top++] = maxindex; } } Points->n_points = icount; /* finally, select only points marked in the algorithm */ for (i = 0; i < icount; i++) { Points->x[i] = Points->x[index[i]]; Points->y[i] = Points->y[index[i]]; Points->z[i] = Points->z[index[i]]; } G_free(stack); G_free(index); return (Points->n_points); }
/* point_in_buf - test if point px,py is in d buffer of Points ** dalpha is in degrees ** returns: 1 in buffer ** 0 not in buffer */ static int point_in_buf(struct line_pnts *Points, double px, double py, double da, double db, double dalpha) { int i, np; double cx, cy; double delta, delta_k, k; double vx, vy, wx, wy, mx, my, nx, ny; double len, tx, ty, d, da2; G_debug(3, "point_in_buf()"); dalpha *= PI / 180; /* convert dalpha from degrees to radians */ np = Points->n_points; da2 = da * da; for (i = 0; i < np - 1; i++) { vx = Points->x[i]; vy = Points->y[i]; wx = Points->x[i + 1]; wy = Points->y[i + 1]; if (da != db) { mx = wx - vx; my = wy - vy; len = LENGTH(mx, my); elliptic_tangent(mx / len, my / len, da, db, dalpha, &cx, &cy); delta = mx * cy - my * cx; delta_k = (px - vx) * cy - (py - vy) * cx; k = delta_k / delta; /* G_debug(4, "k = %g, k1 = %g", k, (mx * (px - vx) + my * (py - vy)) / (mx * mx + my * my)); */ if (k <= 0) { nx = vx; ny = vy; } else if (k >= 1) { nx = wx; ny = wy; } else { nx = vx + k * mx; ny = vy + k * my; } /* inverse transform */ elliptic_transform(px - nx, py - ny, 1 / da, 1 / db, dalpha, &tx, &ty); d = dig_distance2_point_to_line(nx + tx, ny + ty, 0, vx, vy, 0, wx, wy, 0, 0, NULL, NULL, NULL, NULL, NULL); /* G_debug(4, "sqrt(d)*da = %g, len' = %g, olen = %g", sqrt(d)*da, da*LENGTH(tx,ty), LENGTH((px-nx),(py-ny))); */ if (d <= 1) { /* G_debug(1, "d=%g", d); */ return 1; } } else { d = dig_distance2_point_to_line(px, py, 0, vx, vy, 0, wx, wy, 0, 0, NULL, NULL, NULL, NULL, NULL); /* G_debug(4, "sqrt(d) = %g", sqrt(d)); */ if (d <= da2) { return 1; } } } return 0; }
/*! \brief Calculate geodesic distance of point to line in meters. Sets (if not null): - px, py - point on line, - dist - distance to line, - spdist - distance to point on line from segment beginning, - sldist - distance to point on line form line beginning along line \param points pointer to line_pnts structure \param ux,uy,uz point coordinates \param with_z flag if to use z coordinate (3D calculation) \param[out] px,py,pz point on line \param[out] dist distance to line, \param[out] spdist distance of point from segment beginning \param[out] lpdist distance of point from line \return nearest segment (first is 1) */ int Vect_line_geodesic_distance(const struct line_pnts *points, double ux, double uy, double uz, int with_z, double *px, double *py, double *pz, double *dist, double *spdist, double *lpdist) { int i; double distance; double new_dist; double tpx, tpy, tpz, ttpx, ttpy, ttpz; double tdist, tspdist, tlpdist = 0, tlpdistseg; double dz; int n_points; int segment; G_begin_distance_calculations(); n_points = points->n_points; if (n_points == 1) { distance = G_distance(ux, uy, points->x[0], points->y[0]); if (with_z) distance = hypot(distance, uz - points->z[0]); tpx = points->x[0]; tpy = points->y[0]; tpz = points->z[0]; tdist = distance; tspdist = 0; tlpdist = 0; segment = 0; } else { distance = dig_distance2_point_to_line(ux, uy, uz, points->x[0], points->y[0], points->z[0], points->x[1], points->y[1], points->z[1], with_z, &tpx, &tpy, &tpz, NULL, NULL); distance = G_distance(ux, uy, tpx, tpy); if (with_z) distance = hypot(distance, uz - tpz); segment = 1; for (i = 1; i < n_points - 1; i++) { new_dist = dig_distance2_point_to_line(ux, uy, uz, points->x[i], points->y[i], points->z[i], points->x[i + 1], points->y[i + 1], points->z[i + 1], with_z, &ttpx, &ttpy, &ttpz, NULL, NULL); new_dist = G_distance(ux, uy, ttpx, ttpy); if (with_z) new_dist = hypot(new_dist, uz - ttpz); if (new_dist < distance) { distance = new_dist; segment = i + 1; tpx = ttpx; tpy = ttpy; tpz = ttpz; } } /* calculate distance from beginning of segment */ tspdist = G_distance(points->x[segment - 1], points->y[segment - 1], tpx, tpy); if (with_z) { dz = points->z[segment - 1] - tpz; tspdist += hypot(tspdist, dz); } /* calculate distance from beginning of line */ if (lpdist) { tlpdist = 0; for (i = 0; i < segment - 1; i++) { tlpdistseg = G_distance(points->x[i], points->y[i], points->x[i + 1], points->y[i + 1]); if (with_z) { dz = points->z[i + 1] - points->z[i]; tlpdistseg += hypot(tlpdistseg, dz); } tlpdist += tlpdistseg; } tlpdist += tspdist; } tdist = distance; } if (px) *px = tpx; if (py) *py = tpy; if (pz && with_z) *pz = tpz; if (dist) *dist = tdist; if (spdist) *spdist = tspdist; if (lpdist) *lpdist = tlpdist; return (segment); }
/*! \brief Calculate distance of point to line. Sets (if not null): - px, py - point on line, - dist - distance to line, - spdist - distance to point on line from segment beginning, - sldist - distance to point on line form line beginning along line \param points pointer to line_pnts structure \param ux,uy,uz point coordinates \param with_z flag if to use z coordinate (3D calculation) \param[out] px,py,pz point on line \param[out] dist distance to line, \param[out] spdist distance of point from segment beginning \param[out] lpdist distance of point from line \return nearest segment (first is 1) */ int Vect_line_distance(const struct line_pnts *points, double ux, double uy, double uz, int with_z, double *px, double *py, double *pz, double *dist, double *spdist, double *lpdist) { int i; double distance; double new_dist; double tpx, tpy, tpz, tdist, tspdist, tlpdist = 0; double dx, dy, dz; int n_points; int segment; n_points = points->n_points; if (n_points == 1) { distance = dig_distance2_point_to_line(ux, uy, uz, points->x[0], points->y[0], points->z[0], points->x[0], points->y[0], points->z[0], with_z, NULL, NULL, NULL, NULL, NULL); tpx = points->x[0]; tpy = points->y[0]; tpz = points->z[0]; tdist = sqrt(distance); tspdist = 0; tlpdist = 0; segment = 0; } else { distance = dig_distance2_point_to_line(ux, uy, uz, points->x[0], points->y[0], points->z[0], points->x[1], points->y[1], points->z[1], with_z, NULL, NULL, NULL, NULL, NULL); segment = 1; for (i = 1; i < n_points - 1; i++) { new_dist = dig_distance2_point_to_line(ux, uy, uz, points->x[i], points->y[i], points->z[i], points->x[i + 1], points->y[i + 1], points->z[i + 1], with_z, NULL, NULL, NULL, NULL, NULL); if (new_dist < distance) { distance = new_dist; segment = i + 1; } } /* we have segment and now we can recalculate other values (speed) */ new_dist = dig_distance2_point_to_line(ux, uy, uz, points->x[segment - 1], points->y[segment - 1], points->z[segment - 1], points->x[segment], points->y[segment], points->z[segment], with_z, &tpx, &tpy, &tpz, &tspdist, NULL); /* calculate distance from beginning of line */ if (lpdist) { tlpdist = 0; for (i = 0; i < segment - 1; i++) { dx = points->x[i + 1] - points->x[i]; dy = points->y[i + 1] - points->y[i]; if (with_z) dz = points->z[i + 1] - points->z[i]; else dz = 0; tlpdist += hypot(hypot(dx, dy), dz); } tlpdist += tspdist; } tdist = sqrt(distance); } if (px) *px = tpx; if (py) *py = tpy; if (pz && with_z) *pz = tpz; if (dist) *dist = tdist; if (spdist) *spdist = tspdist; if (lpdist) *lpdist = tlpdist; return (segment); }