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);
}
