Geografic-Information-System/vector/v.generalize/smoothing.c

/****************************************************************
 *
 * MODULE:     v.generalize
 *
 * AUTHOR(S):  Daniel Bundala
 *
 * PURPOSE:    Module for line simplification and smoothing
 *
 * COPYRIGHT:  (C) 2002-2005 by the GRASS Development Team
 *
 *             This program is free software under the
 *             GNU General Public License (>=v2).
 *             Read the file COPYING that comes with GRASS
 *             for details.
 *
 ****************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <grass/gis.h>
#include <grass/vector.h>
#include <grass/glocale.h>
#include "point.h"
#include "matrix.h"

/* TODO: add loop support where possible */

/* boyle's forward looking algorithm
 * return the number of points in the result = Points->n_points
 */
int boyle(struct line_pnts *Points, int look_ahead, int loop_support, int with_z)
{
    POINT last, ppoint;
    POINT *res;
    int next, n, i, p;
    double c1, c2;
    int is_loop, count;

    n = Points->n_points;

    /* if look_ahead is too small or line too short, there's nothing
     * to smooth */
    if (look_ahead < 2 || look_ahead >= n) {
	return n;
    }

    count = n - 2;
    is_loop = 0;

    /* is it loop ? */
    if (Points->x[0] == Points->x[n - 1] &&
        Points->y[0] == Points->y[n - 1] &&
        (Points->z[0] == Points->z[n - 1] || with_z == 0) && loop_support){
        is_loop = 1;
	count = n;
    }

    res = G_malloc(sizeof(POINT) * n);

    point_assign(Points, 0, with_z, &last, 0);
    res[0] = last;
    c1 = (double)1 / (double)(look_ahead - 1);
    c2 = (double)1 - c1;
    next = 1;

    for (i = 0; i < count; i++) {
	p = i + look_ahead;
	if (!is_loop && p >= n)
	    p = n - 1;
	point_assign(Points, p, with_z, &ppoint, is_loop);
	point_scalar(ppoint, c1, &ppoint);
	point_scalar(last, c2, &last);
	point_add(last, ppoint, &res[next]);
	/* original: use smoothed point as new last point */
	/* last = res[next]; */
	next++;
	if (is_loop) {
	    while (next >= n - 1)
		next -= n - 1;
	}
	/* new: smooth with original points */
	point_assign(Points, p, with_z, &last, is_loop);
    }

    for (i = 1; i < n - 1; i++) {
	Points->x[i] = res[i].x;
	Points->y[i] = res[i].y;
	Points->z[i] = res[i].z;
    }
    if (is_loop) {
	Points->x[0] = res[0].x;
	Points->y[0] = res[0].y;
	Points->z[0] = res[0].z;

	Points->x[n - 1] = res[0].x;
	Points->y[n - 1] = res[0].y;
	Points->z[n - 1] = res[0].z;
    }
    
    G_free(res);

    return Points->n_points;
}

/* mcmaster's sliding averaging algorithm. Return the number of points
 * in the output line. This equals to the number of points in the
 * input line */

int sliding_averaging(struct line_pnts *Points, double slide, int look_ahead, 
                      int loop_support, int with_z)
{
    int n, half, i;
    double sc;
    POINT p, tmp, s;
    POINT *res;
    int is_loop, count;

    is_loop = 0;
    n = Points->n_points;
    half = look_ahead / 2;
    
    count = n - half;

    /* is it loop ? */
    if (Points->x[0] == Points->x[n - 1] &&
        Points->y[0] == Points->y[n - 1] && 
        (Points->z[0] == Points->z[n - 1] || with_z == 0) && loop_support){
        is_loop = 1;
        count = n + half;
    }
     
    if (look_ahead % 2 == 0) {
	G_fatal_error(_("Look ahead parameter must be odd"));
	return n;
    }

    if (look_ahead >= n || look_ahead < 2)
	return n;

    res = G_malloc(sizeof(POINT) * (n + half));
    if (!res) {
	G_fatal_error(_("Out of memory"));
	return n;
    }

    sc = (double)1.0 / (double)look_ahead;

    point_assign(Points, 0, with_z, &p, 0);
    for (i = 1; i < look_ahead; i++) {
	point_assign(Points, i, with_z, &tmp, 0);
	point_add(p, tmp, &p);
    }

    /* and calculate the average of remaining points */
    for (i = half; i < count; i++) {
	point_assign(Points, i, with_z, &s, is_loop);
	point_scalar(s, 1.0 - slide, &s);
	point_scalar(p, sc * slide, &tmp);
	point_add(tmp, s, &res[i]);
	if ((i + half + 1 < n) || is_loop) {
	    point_assign(Points, i - half, with_z, &tmp, is_loop);
	    point_subtract(p, tmp, &p);
	    point_assign(Points, i + half + 1, with_z, &tmp, is_loop);
	    point_add(p, tmp, &p);
	}
    }


    if (is_loop) {
        for (i = 0; i < half; i++) {
	    Points->x[i] = res[n + i - 1].x;
	    Points->y[i] = res[n + i - 1].y;
	    Points->z[i] = res[n + i - 1].z;
        }
	for (i = half; i < n; i++) {
	    Points->x[i] = res[i].x;
	    Points->y[i] = res[i].y;
	    Points->z[i] = res[i].z;
	}
    }
    else {
	for (i = half; i < n - half; i++) {
	    Points->x[i] = res[i].x;
	    Points->y[i] = res[i].y;
	    Points->z[i] = res[i].z;
	}
    }

    G_free(res);

    return Points->n_points;
}

/* mcmaster's distance weighting algorithm. Return the number
 * of points in the output line which equals to Points->n_points */
int distance_weighting(struct line_pnts *Points, double slide, int look_ahead,
		       int loop_support, int with_z)
{
    POINT p, c, s, tmp;
    int n, i, half, j;
    double dists, d;
    POINT *res;
    int is_loop, count;

    n = Points->n_points;

    if (look_ahead >= n || look_ahead < 1)
	return n;

    half = look_ahead / 2;
    count = n - half;

    /* is it loop ? */
    is_loop = 0;
    if (Points->x[0] == Points->x[n - 1] &&
        Points->y[0] == Points->y[n - 1] &&
        (Points->z[0] == Points->z[n - 1] || with_z == 0) && loop_support){
        is_loop = 1;
        count = n + half - 1;
    }

    if ((look_ahead & 1) == 0) {
	G_fatal_error(_("Look ahead parameter must be odd"));
	return n;
    }

    res = (POINT *) G_malloc(sizeof(POINT) * (n + half));
    if (!res) {
	G_fatal_error(_("Out of memory"));
	return n;
    }

    point_assign(Points, 0, with_z, &res[0], 0);

    for (i = half; i < count; i++) {
	point_assign(Points, i, with_z, &c, is_loop);
	s.x = s.y = s.z = 0;
	dists = 0;

	for (j = i - half; j <= i + half; j++) {
	    if (j == i)
		continue;
	    point_assign(Points, j, with_z, &p, is_loop);
	    d = point_dist(p, c);
	    if (d < GRASS_EPSILON)
		continue;
	    d = (double)1.0 / d;
	    dists += d;
	    point_scalar(p, d, &tmp);
	    s.x += tmp.x;
	    s.y += tmp.y;
	    s.z += tmp.z;
	}
	if (dists < GRASS_EPSILON) {
	    point_add(c, s, &res[i]);
	}
	else {
	    point_scalar(s, slide / dists, &tmp);
	    point_scalar(c, (double)1.0 - slide, &s);
	    point_add(s, tmp, &res[i]);
	}
    }

    if (is_loop) {
        for (i = 0; i < half; i++) {
	    Points->x[i] = res[n + i - 1].x;
	    Points->y[i] = res[n + i - 1].y;
	    Points->z[i] = res[n + i - 1].z;
        }
	for (i = half; i < n; i++) {
	    Points->x[i] = res[i].x;
	    Points->y[i] = res[i].y;
	    Points->z[i] = res[i].z;
	}
    }
    else {
	for (i = half; i < n - half; i++) {
	    Points->x[i] = res[i].x;
	    Points->y[i] = res[i].y;
	    Points->z[i] = res[i].z;
	}
    }

    G_free(res);

    return Points->n_points;
}


/* Chaiken's algorithm. Return the number of points in smoothed line 
 */
int chaiken(struct line_pnts *Points, double thresh, int loop_support, int with_z)
{

    int n, i;
    POINT_LIST head, *cur;
    POINT p0, p1, p2, m1, tmp;
    int is_loop;

    n = Points->n_points;

    /* line is too short */
    if (n < 3)
	return n;

    is_loop = 0;
    /* is it loop ? */
    if (Points->x[0] == Points->x[n - 1] &&
        Points->y[0] == Points->y[n - 1] && 
        (Points->z[0] == Points->z[n - 1] || with_z == 0) && loop_support){
        is_loop = 1;
    }

    thresh *= thresh;

    head.next = NULL;
    cur = &head;
    if (!is_loop) {
	/* always keep first point */
	point_assign(Points, 0, with_z, &p0, 0);
    }
    else {
	point_assign(Points, 0, with_z, &p1, 0);
	point_assign(Points, 1, with_z, &p2, 0);
	point_add(p1, p2, &tmp);
	point_scalar(tmp, 0.5, &p0);
    }

    point_list_add(cur, p0); 
    cur = cur->next;

    for (i = 2; i <= n; i++) {
	if (!is_loop) {
	    if (i == n)
		point_assign(Points, i - 1, with_z, &p2, 0);
	    else
		point_assign(Points, i, with_z, &p2, 0);
	}
	else
	    point_assign(Points, i, with_z, &p2, 1);
	point_assign(Points, i - 1, with_z, &p1, 0);

	while (1) {
	    point_add(p1, p2, &tmp);
	    point_scalar(tmp, 0.5, &m1);

	    point_list_add(cur, m1);

	    if (point_dist_square(p0, m1) > thresh) {
		point_add(p1, m1, &tmp);	/* need to refine the partition */
		point_scalar(tmp, 0.5, &p2);
		point_add(p1, p0, &tmp);
		point_scalar(tmp, 0.5, &p1);
	    }
	    else {
		break;		/* good approximation */
	    }
	}

	while (cur->next != NULL)
	    cur = cur->next;

	p0 = cur->p;
    }

    if (!is_loop) {
	point_assign(Points, n - 1, with_z, &p0, 0);
	point_list_add(cur, p0); /* always keep last point */
    }

    if (point_list_copy_to_line_pnts(head, Points) == -1) {
	G_fatal_error(_("Out of memory"));
    }
    point_list_free(head);

    return Points->n_points;
}


/* use for refining tangent in hermite interpolation */
void refine_tangent(POINT * p)
{
    double l = point_dist2(*p);

    if (l < GRASS_EPSILON) {
	point_scalar(*p, 0.0, p);
    }
    else {
	point_scalar(*p, (double)1.0 / sqrt(sqrt(sqrt(l))), p);
    }
    return;
}

/* approximates given line using hermite cubic spline
 * interpolates by steps of length step
 * returns the number of point in result
 */
int hermite(struct line_pnts *Points, double step, double angle_thresh,
	    int loop_support, int with_z)
{
    POINT_LIST head, *last, *point;
    POINT p0, p1, t0, t1, tmp, res;
    double length, next, length_begin, l;
    double s;
    double h1, h2, h3, h4;
    int n, i;
    int ni;
    int is_loop;

    n = Points->n_points;

    /* line is too short */
    if (n <= 2) {
	return n;
    }

    is_loop = 0;

    /* is it loop ? */
    if (Points->x[0] == Points->x[n - 1] &&
        Points->y[0] == Points->y[n - 1] && 
        (Points->z[0] == Points->z[n - 1] || with_z == 0) && loop_support){
        is_loop = 1;
    }

    /* convert degrees=>radians */
    angle_thresh *= M_PI / 180.0;

    head.next = NULL;
    point = last = &head;

    if (!is_loop) {
	point_assign(Points, 0, with_z, &p0, 0);
	point_assign(Points, 1, with_z, &p1, 0);
	/* length of line from point 0 to i+1 */
	length = point_dist(p0, p1);
	/* tangent at p0, p1 */
	point_subtract(p1, p0, &t0);
	refine_tangent(&t0);
	point_assign(Points, 2, with_z, &tmp, 0);
	point_subtract(tmp, p0, &t1);
	refine_tangent(&t1);
    }
    else {
	point_assign(Points, n - 2, with_z, &p0, 0);
	point_assign(Points, 0, with_z, &p1, 0);
	/* length of line from point n - 2 to 0 = n - 1 */
	length = point_dist(p0, p1);
	/* tangent at p0, p1 */
	point_assign(Points, 1, with_z, &tmp, 0);
	point_subtract(tmp, p0, &t0);
	refine_tangent(&t0);

	point_assign(Points, 0, with_z, &p0, 0);
	point_assign(Points, 1, with_z, &p1, 0);
	/* length of line from point 0 to 1 */
	length = point_dist(p0, p1);
	/* tangent at p0, p2 */
	point_assign(Points, 2, with_z, &tmp, 0);
	point_subtract(tmp, p0, &t1);
	refine_tangent(&t1);
    }

    /* length of line 0..i */
    length_begin = 0;
    next = 0;
    
    /* we always operate on the segment point[i]..point[i+1] */
    i = 0;
    while (i < n - 1) {
	if (next > length || (length - length_begin < GRASS_EPSILON)) {	/* segmet i..i+1 is finished or too short */
	    i++;
	    if (i >= n - 1)
		break;		/* we are already out of line */
	    point_assign(Points, i, with_z, &p0, is_loop);
	    point_assign(Points, i + 1, with_z, &p1, is_loop);
	    length_begin = length;
	    length += point_dist(p0, p1);
	    ni = i + 2;
	    if (!is_loop && ni > n - 1)
		ni = n - 1;	/* ensure that we are in the line */
	    t0 = t1;
	    point_assign(Points, ni, with_z, &tmp, is_loop);
	    point_subtract(tmp, p0, &t1);
	    refine_tangent(&t1);
	}
	else {
	    l = length - length_begin;	/* length of actual segment */
	    s = (next - length_begin) / l;	/* 0<=s<=1 where we want to add new point on the line */

	    /* then we need to calculate 4 control polynomials */
	    h1 = 2 * s * s * s - 3 * s * s + 1;
	    h2 = -2 * s * s * s + 3 * s * s;
	    h3 = s * s * s - 2 * s * s + s;
	    h4 = s * s * s - s * s;

	    point_scalar(p0, h1, &res);
	    point_scalar(p1, h2, &tmp);
	    point_add(res, tmp, &res);
	    point_scalar(t0, h3, &tmp);
	    point_add(res, tmp, &res);
	    point_scalar(t1, h4, &tmp);
	    point_add(res, tmp, &res);
	    point_list_add(last, res);
	    last = last->next;

	    next += step;
	}
	/* if the angle between 2 vectors is less then eps, remove the
	 * middle point */
	if (point->next && point->next->next && point->next->next->next) {
	    if (point_angle_between
		(point->next->p, point->next->next->p,
		 point->next->next->next->p) < angle_thresh) {
		point_list_delete_next(point->next);
	    }
	    else
		point = point->next;
	}
    }

    point_assign(Points, n - 1, with_z, &p0, 0);
    point_list_add(last, p0);

    if (point_list_copy_to_line_pnts(head, Points) == -1)
	G_fatal_error(_("Out of memory"));

    point_list_free(head);

    return Points->n_points;
}

/* snakes algorithm for line simplification/generalization 
 * returns the number of points in the output line. This is
 * always equal to the number of points in the original line
 * 
 * alpha, beta are 2 parameters which change the behaviour of the algorithm
 * 
 * TODO: Add parameter iterations, so the runnining time is O(N^3 * log iterations)
 * instead of O(N^3 * itearations). Probably not needed, for many iterations,
 * the result is almost straight line
 */
int snakes(struct line_pnts *Points, double alpha, double beta,
           int loop_support, int with_z)
{
    MATRIX g, ginv, xcoord, ycoord, zcoord, xout, yout, zout;

    int n = Points->n_points;
    int i, j;
    double x0, y0, z0;
    double a = 2.0 * alpha + 6.0 * beta;
    double b = -alpha - 4.0 * beta;
    double c = beta;
    double val[5] = { c, b, a, b, c };
    int plus = 4;
    int is_loop = 0;

    if (n < plus)
	return n;

    /* is it loop ? */
    if (Points->x[0] == Points->x[n - 1] &&
        Points->y[0] == Points->y[n - 1] && 
        (Points->z[0] == Points->z[n - 1] || with_z == 0) && loop_support){
        is_loop = 1;
	
	if (n < plus + 2)
	    return n;
    }

    if (!matrix_init(n + 2 * plus, n + 2 * plus, &g)) {
	G_fatal_error(_("Out of memory"));
	return n;
    }
    if (!matrix_init(n + 2 * plus, 1, &xcoord)) {
	G_fatal_error(_("Out of memory"));
	return n;
    }
    if (!matrix_init(n + 2 * plus, 1, &ycoord)) {
	G_fatal_error(_("Out of memory"));
	return n;
    }
    if (!matrix_init(n + 2 * plus, 1, &zcoord)) {
	G_fatal_error(_("Out of memory"));
	return n;
    }
    if (!matrix_init(n + 2 * plus, 1, &xout)) {
	G_fatal_error(_("Out of memory"));
	return n;
    }
    if (!matrix_init(n + 2 * plus, 1, &yout)) {
	G_fatal_error(_("Out of memory"));
	return n;
    }
    if (!matrix_init(n + 2 * plus, 1, &zout)) {
	G_fatal_error(_("Out of memory"));
	return n;
    }

    x0 = Points->x[0];
    y0 = Points->y[0];
    z0 = Points->z[0];

    /* store the coordinates in the column vectors */
    for (i = 0; i < n; i++) {
	xcoord.a[i + plus][0] = Points->x[i] - x0;
	ycoord.a[i + plus][0] = Points->y[i] - y0;
	zcoord.a[i + plus][0] = Points->z[i] - z0;
    }

    if (!is_loop) {
	/* repeat first and last point at the beginning and end
	 * of each vector respectively */
	for (i = 0; i < plus; i++) {
	    xcoord.a[i][0] = 0;
	    ycoord.a[i][0] = 0;
	    zcoord.a[i][0] = 0;
	}

	for (i = n + plus; i < n + 2 * plus; i++) {
	    xcoord.a[i][0] = Points->x[n - 1] - x0;
	    ycoord.a[i][0] = Points->y[n - 1] - y0;
	    zcoord.a[i][0] = Points->z[n - 1] - z0;
	}
    }
    else {
	/* loop: point 0 and point n - 1 are identical */

	/* repeat last and first points at the beginning and
	 * end of each vector respectively */

	/* add points from n - plus - 1 to n - 2 */
	for (i = 0, j = n - plus - 1; i < plus; i++, j++) {
	    xcoord.a[i][0] = Points->x[j] - x0;;
	    ycoord.a[i][0] = Points->y[j] - y0;;
	    zcoord.a[i][0] = Points->z[j] - z0;;
	}
	/* add points from 1 to plus + 1 */
	for (i = n + plus, j = 1; i < n + 2 * plus; i++, j++) {
	    xcoord.a[i][0] = Points->x[j] - x0;
	    ycoord.a[i][0] = Points->y[j] - y0;
	    zcoord.a[i][0] = Points->z[j] - z0;
	}
    }

    /* calculate the matrix */

    for (i = 0; i < n + 2 * plus; i++)
	for (j = 0; j < n + 2 * plus; j++) {
	    int index = j - i + 2;

	    if (index >= 0 && index <= 4)
		g.a[i][j] = val[index];
	    else
		g.a[i][j] = 0;
	}

    matrix_add_identity((double)1.0, &g);

    /* find its inverse */
    if (!matrix_inverse(&g, &ginv, 0)) {
	G_fatal_error(_("Unable to find the inverse matrix"));
	return n;
    }

    if (!matrix_mult(&ginv, &xcoord, &xout)
	|| !matrix_mult(&ginv, &ycoord, &yout)
	|| !matrix_mult(&ginv, &zcoord, &zout)) {
	G_fatal_error(_("Unable to calculate the output vectors"));
	return n;
    }

    if (!is_loop) {
	/* copy the new values of coordinates, but
	 * never move the last and first point */
	for (i = 1; i < n - 1; i++) {
	    Points->x[i] = xout.a[i + plus][0] + x0;
	    Points->y[i] = yout.a[i + plus][0] + y0;
	    if (with_z)
		Points->z[i] = zout.a[i + plus][0] + z0;
	}
    }
    else {
	/* copy the new values of coordinates,
	 * also move the last and first point */
	for (i = 0; i < n; i++) {
	    Points->x[i] = xout.a[i + plus][0] + x0;
	    Points->y[i] = yout.a[i + plus][0] + y0;
	    if (with_z)
		Points->z[i] = zout.a[i + plus][0] + z0;
	}

	Points->x[n - 1] = Points->x[0];
	Points->y[n - 1] = Points->y[0];
	Points->z[n - 1] = Points->z[0];
    }

    matrix_free(&g);
    matrix_free(&ginv);
    matrix_free(&xcoord);
    matrix_free(&ycoord);
    matrix_free(&zcoord);
    matrix_free(&xout);
    matrix_free(&yout);
    matrix_free(&zout);

    return Points->n_points;
}