/*Reumann-Witkam algorithm
* Returns number of points in the output line
*/
int reumann_witkam(struct line_pnts *Points, double thresh, int with_z)
{
int seg1, seg2;
int i, count;
POINT x0, x1, x2, sub, diff;
double subd, diffd, sp, dist;
int n;
n = Points->n_points;
if (n < 3)
return n;
thresh *= thresh;
seg1 = 0;
seg2 = 1;
count = 1;
point_assign(Points, 0, with_z, &x1);
point_assign(Points, 1, with_z, &x2);
point_subtract(x2, x1, &sub);
subd = point_dist2(sub);
for (i = 2; i < n; i++) {
point_assign(Points, i, with_z, &x0);
point_subtract(x1, x0, &diff);
diffd = point_dist2(diff);
sp = point_dot(diff, sub);
dist = (diffd * subd - sp * sp) / subd;
/* if the point is out of the threshlod-sausage, store it and calculate
* all variables which do not change for each line-point calculation */
if (dist > thresh) {
point_assign(Points, i - 1, with_z, &x1);
point_assign(Points, i, with_z, &x2);
point_subtract(x2, x1, &sub);
subd = point_dist2(sub);
Points->x[count] = x0.x;
Points->y[count] = x0.y;
Points->z[count] = x0.z;
count++;
}
}
Points->x[count] = Points->x[n - 1];
Points->y[count] = Points->y[n - 1];
Points->z[count] = Points->z[n - 1];
Points->n_points = count + 1;
return Points->n_points;
}
inline double point_angle_between(POINT a, POINT b, POINT c)
{
point_subtract(b, a, &a);
point_subtract(c, b, &b);
return acos(point_dot(a, b) / sqrt(point_dist2(a) * point_dist2(b)));
}
