Geografic-Information-System/vector/v.mapcalc/vector.c

#define _ISOC99_SOURCE		/* to get isfinite() */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <grass/gis.h>
#include "list.h"
#include "mapcalc.h"
#include "vector.h"

typedef struct Vecfunc
{
    char *name;
    void *func;
    char *proto;
} VECFUNC;

static VECFUNC vf[] = {
    {"v_copy", v_copy, "p=rp"},
    {"v_add", v_add, "p=rpp"},
    {"pnt_op_func_+", v_add, "p=rpp"},
    {"v_sub", v_sub, "p=rpp"},
    {"pnt_op_func_-", v_sub, "p=rpp"},
    {"v_abs", v_abs, "p=rp"},
    {"v_neg", v_neg, "p=rp"},
    {"pnt_op_func__", v_neg, "p=rp"},
    {"v_mul", v_mul, "p=rpd"},
    {"pnt_op_func_*", v_mul, "p=rpd"},
    {"v_div", v_div, "p=rpd"},
    {"pnt_op_func_/", v_div, "p=rpd"},
    {"v_unit", v_unit, "p=rp"},
    {"v_cross", v_cross, "p=rpp"},
    {"pnt_op_func_^", v_cross, "p=rpp"},
    {"v_val", v_val, "d=p"},
    {"v_dot", v_dot, "d=pp"},
    {"pnt_op_func_%", v_dot, "d=pp"},
    {"v_area", v_area, "d=pp"},
    {"v_eq", v_eq, "d=pp"},
    {"v_eq_epsilon", v_eq_epsilon, "d=ppp"},
    {"v_isortho", v_isortho, "d=pp"},
    {"v_ispara", v_ispara, "d=pp"},
    {"v_isacute", v_isacute, "d=pp"},
    {NULL, NULL, NULL}
};

typedef VECTOR *(*p_func) (void);
typedef VECTOR *(*p_func_p) (void *p0);
typedef VECTOR *(*p_func_pp) (void *p0, void *p1);
typedef VECTOR *(*p_func_ppp) (void *p0, void *p1, void *p2);
typedef VECTOR *(*p_func_ppd) (void *p0, void *p1, double d);

double nanval;
static VECTOR pnt_o = { NULL, 0, 0, 0, 1 };
static VECTOR pnt_i = { NULL, 1, 0, 0, 1 };
static VECTOR pnt_j = { NULL, 0, 1, 0, 1 };
static VECTOR pnt_k = { NULL, 0, 0, 1, 1 };

void init_vec(void);
void printvec(SYMBOL * sym);
void showvec(SYMBOL * sym);
void setpnt(SYMBOL * var, SYMBOL * pnt);
SYMBOL *mkpnt(double x, double y, double z);
SYMBOL *mkpntvar(SYMBOL * var, SYMBOL * pnt);
SYMBOL *pntfunc(SYMBOL * func, SYMBOL * arglist);
SYMBOL *pntop(int op, SYMBOL * pnt1, SYMBOL * pnt2);
SYMBOL *pntapp(SYMBOL * head, SYMBOL * elt);
VECTOR *v_copy(VECTOR * p, VECTOR * p1);
VECTOR *v_add(VECTOR * p, VECTOR * p1, VECTOR * p2);
VECTOR *v_sub(VECTOR * p, VECTOR * p1, VECTOR * p2);
VECTOR *v_abs(VECTOR * p, VECTOR * p1);
VECTOR *v_neg(VECTOR * p, VECTOR * p1);
static inline int _is_zero(double r);
double v_eq(VECTOR * p1, VECTOR * p2);
double v_eq_epsilon(VECTOR * p1, VECTOR * p2, VECTOR * e);
VECTOR *v_mul(VECTOR * p, VECTOR * p1, double d);
VECTOR *v_div(VECTOR * p, VECTOR * p1, double d);
double v_val(VECTOR * p);
VECTOR *v_unit(VECTOR * p, VECTOR * p1);
double v_dot(VECTOR * p1, VECTOR * p2);
VECTOR *v_cross(VECTOR * p, VECTOR * p1, VECTOR * p2);
double v_isortho(VECTOR * p1, VECTOR * p2);
double v_ispara(VECTOR * p1, VECTOR * p2);
double v_isacute(VECTOR * p1, VECTOR * p2);
double v_area(VECTOR * p1, VECTOR * p2);


void init_vec(void)
{
    SYMBOL *sym;
    int i;

    for (i = 0; vf[i].name; i++) {
	sym = putsym(vf[i].name);
	switch (vf[i].proto[0]) {
	case 'p':
	    sym->type = st_pfunc;
	    sym->rettype = st_pnt;
	    break;
	case 'd':
	    sym->type = st_nfunc;
	    sym->rettype = st_num;
	    break;
	}
	sym->itype = sym->type;
	sym->v.p = vf[i].func;
	sym->proto = vf[i].proto + 2;
    }

    /* add some handy constants */
    sym = putsym("pnt_o");
    sym->type = sym->itype = st_pnt;
    sym->v.p = &pnt_o;

    sym = putsym("pnt_i");
    sym->type = sym->itype = st_pnt;
    sym->v.p = &pnt_i;

    sym = putsym("pnt_j");
    sym->type = sym->itype = st_pnt;
    sym->v.p = &pnt_j;

    sym = putsym("pnt_k");
    sym->type = sym->itype = st_pnt;
    sym->v.p = &pnt_k;

    /* initialize NaN */
    nanval = sqrt(-1);
}

void printvec(SYMBOL * sym)
{
    VECTOR *v;

    v = (VECTOR *) sym->v.p;

    fprintf(stdout, "\t(");
    if (!isfinite(v->x))
	fprintf(stdout, "??.??");
    else if (v->x == (int)v->x)
	fprintf(stdout, "%d", (int)v->x);
    else
	fprintf(stdout, "%g", v->x);
    fprintf(stdout, ", ");
    if (!isfinite(v->y))
	fprintf(stdout, "??.??");
    else if (v->y == (int)v->y)
	fprintf(stdout, "%d", (int)v->y);
    else
	fprintf(stdout, "%g", v->y);
    if (isfinite(v->z)) {
	fprintf(stdout, ", ");
	if (v->z == (int)v->z)
	    fprintf(stdout, "%d", (int)v->z);
	else
	    fprintf(stdout, "%g", v->z);
    }
    fprintf(stdout, ")\n");
}

void showvec(SYMBOL * sym)
{

    VECTOR *v;

    v = (VECTOR *) sym->v.p;
    printvec(sym);

    if (v && --v->refcnt > 0)
	sym->v.p = NULL;
    freesym(sym);
}

void setpnt(SYMBOL * var, SYMBOL * pnt)
{
    SYMBOL *sym;

    if (var->name) {
	sym = getsym(var->name);
	if (sym) {
	    if (--((VECTOR *) sym->v.p)->refcnt < 1)
		G_free(sym->v.p);
	    /*
	     * If refcnt(pnt) == 1, this was anonymous, else it's used
	     * somewhere else. Must we dup then?
	     */
	    sym->v.p = pnt->v.p;
	}
    }

    if (--((VECTOR *) var->v.p)->refcnt < 1)
	G_free(var->v.p);
    var->v.p = NULL;
    freesym(var);

    printvec(pnt);
    pnt->v.p = NULL;
    freesym(pnt);
}

SYMBOL *mkpnt(double x, double y, double z)
{
    SYMBOL *pnt;
    VECTOR *vec;

    vec = (VECTOR *) listitem(sizeof(VECTOR));
    vec->x = x;
    vec->y = y;
    vec->z = z;
    vec->refcnt = 1;

    pnt = (SYMBOL *) listitem(sizeof(SYMBOL));
    pnt->type = pnt->itype = st_pnt;
    pnt->v.p = vec;

    return pnt;
}

SYMBOL *mkpntvar(SYMBOL * var, SYMBOL * pnt)
{
    var->type = var->itype = st_pnt;
    var->name = var->v.p;
    var->v.p = pnt->v.p;
    pnt->v.p = NULL;
    freesym(pnt);

    symtab = (SYMBOL *) listadd((LIST *) symtab, (LIST *) var, cmpsymsym);

    printvec(var);

    return var;
}

SYMBOL *pntfunc(SYMBOL * func, SYMBOL * arglist)
{
    SYMBOL *sym, *sptr;
    VECTOR *res = NULL;
    int argc = -1, i;

    sym = (SYMBOL *) listitem(sizeof(SYMBOL));
    sym->type = sym->itype = st_pnt;

    if (!func || !func->v.p || func->type != st_pfunc) {
	parseerror = 1;
	G_warning(_("Can't call bad function"));
    }
    else
	argc = listcnt((LIST *) arglist);

    for (i = 0, sptr = arglist; sptr; sptr = sptr->next, i++) {
	if (func->proto[i] == 'r')
	    i++;
	if (func->proto[i] == 'p') {
	    if (sptr->itype != st_pnt || !sptr->v.p)
		argc = -1;
	}
	else if (func->proto[i] == 'd' && sptr->itype != st_num)
	    argc = -1;
    }

    res = (VECTOR *) listitem(sizeof(VECTOR));

    if (argc == 0 && (!func->proto || !*func->proto))
	res = (*(p_func) func->v.p) ();
    else if (argc == 1 && !strcmp(func->proto, "p"))
	res = (*(p_func_p) func->v.p) (arglist->v.p);
    else if (argc == 1 && !strcmp(func->proto, "rp"))
	res = (*(p_func_pp) func->v.p) (res, arglist->v.p);
    else if (argc == 2 && !strcmp(func->proto, "rpd"))
	res = (*(p_func_ppd) func->v.p) (res, arglist->v.p,
					 arglist->next->v.d);
    else if (argc == 2 && !strcmp(func->proto, "pp"))
	res = (*(p_func_pp) func->v.p) (arglist->v.p, arglist->next->v.p);
    else if (argc == 2 && !strcmp(func->proto, "rpp"))
	res = (*(p_func_ppp) func->v.p) (res, arglist->v.p,
					 arglist->next->v.p);
    else if (argc == 3 && !strcmp(func->proto, "ppp"))
	res = (*(p_func_ppp) func->v.p) (arglist->v.p,
					 arglist->next->v.p,
					 arglist->next->next->v.p);
    else {
	G_warning(_("Bad arguments to pointfunc %s"), func->name);
	parseerror = 1;
	sym = (SYMBOL *) listdelall((LIST *) sym, freesym);
	if (res)
	    G_free(res);
	return NULL;
    }
    sym->v.p = res;

    listdelall((LIST *) arglist, freesym);

    return sym;
}

SYMBOL *pntop(int op, SYMBOL * pnt1, SYMBOL * pnt2)
{
    SYMBOL *func, *arglist, *res = NULL;
    char buf[32];

    sprintf(buf, "pnt_op_func_%c", op);

    func = getsym(buf);
    if (!func) {
	G_warning(_("No function defined to perform ``point %c point''"), op);
	parseerror = 1;
    }
    else if (func->rettype == st_pnt) {
	res = (SYMBOL *) listitem(sizeof(SYMBOL));
	symcpy(res, func);
	res->next = NULL;
	func = res;
	arglist = (SYMBOL *) listapp(NULL, (LIST *) pnt1);
	arglist = (SYMBOL *) listapp((LIST *) arglist, (LIST *) pnt2);

	res = pntfunc(func, arglist);
    }

    return res;
}

SYMBOL *pntapp(SYMBOL * head, SYMBOL * elt)
{
    return (SYMBOL *) listapp((LIST *) head, (LIST *) elt);
}

/*
 * Utility function to copy a point: p = p1;
 * The dimension (2D/3D) depends on p1. Note, that copying a constant
 * will always yield 3D.
 */
VECTOR *v_copy(VECTOR * p, VECTOR * p1)
{
    p->x = p1->x;
    p->y = p1->y;
    p->z = p1->z;

    return p;
}

/*
 * Vector addition
 * Result is 2D if at least one of p1 or p2 is 2D.
 */
VECTOR *v_add(VECTOR * p, VECTOR * p1, VECTOR * p2)
{
    p->x = p1->x + p2->x;
    p->y = p1->y + p2->y;
    /*
     * resist the tentation setting p->z to nanval and then testing for
     * dimension, as p might be the same as p1 or p2
     */
    if (!isnan(p1->z) && !isnan(p2->z))
	p->z = p1->z + p2->z;
    else
	p->z = nanval;

    return p;
}

/*
 * Vector substraction
 * Result is 2D if at least one of p1 or p2 is 2D.
 */
VECTOR *v_sub(VECTOR * p, VECTOR * p1, VECTOR * p2)
{
    p->x = p1->x - p2->x;
    p->y = p1->y - p2->y;
    if (!isnan(p1->z) && !isnan(p2->z))
	p->z = p1->z + p2->z;
    else
	p->z = nanval;

    return p;
}

/*
 * Utility function to make all coordinates positive
 */
VECTOR *v_abs(VECTOR * p, VECTOR * p1)
{
    p->x = fabs(p1->x);
    p->y = fabs(p1->y);
    if (!isnan(p1->z))
	p->z = fabs(p1->z);
    else
	p->z = nanval;

    return p;
}

/*
 * Utility function to negate all coordinates
 */
VECTOR *v_neg(VECTOR * p, VECTOR * p1)
{
    p->x = -p1->x;
    p->y = -p1->y;
    if (!isnan(p1->z))
	p->z = -p1->z;
    else
	p->z = nanval;

    return p;
}

/*
 * Utility function to compare two doubles for equality without epsilon
 * This is not really true, as we consider NaN to be zero.
 */
static inline int _is_zero(double r)
{
    if ((isfinite(r) && r == 0.0) || isnan(r))
	return 1;
    return 0;
}

/*
 * Test for equality of two points. No epsion applied
 */
double v_eq(VECTOR * p1, VECTOR * p2)
{
    VECTOR p;
    int dim = 2;

    if (!isnan(p1->z) && !isnan(p2->z))
	dim = 3;

    v_sub(&p, p1, p2);
    v_abs(&p, &p);
    if (_is_zero(p.x) && _is_zero(p.y) && (dim == 2 || _is_zero(p.z)))
	return 1;
    return 0;
}

/*
 * Test for equality of two points by a given epsilon
 * epsilon is supposed to have positive values only.
 */
double v_eq_epsilon(VECTOR * p1, VECTOR * p2, VECTOR * e)
{
    VECTOR p;
    int dim = 2;

    if (!isnan(p1->z) && !isnan(p2->z))
	dim = 3;

    v_sub(&p, p1, p2);
    v_abs(&p, &p);
    if (p.x < e->x && p.y < e->y && (dim == 2 || (p.z < e->z)))
	return 1;
    return 0;
}

/*
 * Multiply a vector by a scalar
 */
VECTOR *v_mul(VECTOR * p, VECTOR * p1, double d)
{
    p->x = d * p1->x;
    p->y = d * p1->y;
    if (!isnan(p1->z))
	p->z = d * p1->z;
    else
	p->z = nanval;

    return p;
}

/*
 * Divide a vector by a scalar
 */
VECTOR *v_div(VECTOR * p, VECTOR * p1, double d)
{
    if (!isfinite(d) || d == 0.0) {
	parseerror = 1;
	return p;
    }
    p->x = p1->x / d;
    p->y = p1->y / d;
    if (!isnan(p1->z))
	p->z = p1->z / d;
    else
	p->z = nanval;

    return p;
}

/*
 * Compute the value of a vector
 */
double v_val(VECTOR * p)
{
    return sqrt(p->x * p->x + p->y * p->y +
		((isnan(p->z)) ? 0 : p->z * p->z));
}

/*
 * The only way to get a value of zero is that P1 is the origin.
 * The unit vector of the origin doesn't exist, but we return the
 * origin.
 */
VECTOR *v_unit(VECTOR * p, VECTOR * p1)
{
    double val = v_val(p1);

    if (_is_zero(val))
	return v_copy(p, &pnt_o);

    p->x = p1->x / val;
    p->y = p1->y / val;
    if (!isnan(p1->z))
	p->z = p1->z / val;
    else
	p->z = nanval;

    return p;
}

/*
 * Compute the dot product of P1 and P2
 */
double v_dot(VECTOR * p1, VECTOR * p2)
{
    int dim = 2;

    if (!isnan(p1->z) && !isnan(p2->z))
	dim = 3;
    return p1->x * p2->x + p1->y * p2->y + ((dim == 2) ? 0 : p1->z * p2->z);
}

/*
 * Compute the cross product of P1 and P2
 * Return (0,0) for 2D
 */

VECTOR *v_cross(VECTOR * p, VECTOR * p1, VECTOR * p2)
{
    VECTOR p0;

    if (!isnan(p1->z) && !isnan(p2->z)) {
	p0.x = p1->y * p2->z + p1->z * p2->y;
	p0.y = p1->z * p2->x + p1->x * p2->z;
	p0.z = p1->x * p2->y + p1->y * p2->x;
	v_copy(p, &p0);
    }
    else {
	p->x = p->y = 0;
	p->z = nanval;
    }

    return p;
}

/*
 * Decide if vector P1 is ortogonal to vector P2
 * Should test if either P1 or P2 are (0,0,0);
 */
double v_isortho(VECTOR * p1, VECTOR * p2)
{
    return v_dot(p1, p2) == 0;
}

/*
 * Decide if P1 and P2 are parallel. If they are but have a diferent
 * direction, -1 is returned.
 */
double v_ispara(VECTOR * p1, VECTOR * p2)
{
    double dot, val, dif;

    dot = v_dot(p1, p2);
    val = v_val(p1);
    val *= v_val(p2);

    dif = fabs(dot - val);
    if (_is_zero(dif))
	return 1;

    dif = fabs(dot + val);
    if (_is_zero(dif))
	return -1;

    return 0;
}

/*
 * Decide if P1 and P2 have and angle alpha which: 0 < alpha < 90.0
 */
double v_isacute(VECTOR * p1, VECTOR * p2)
{
    return v_dot(p1, p2) > 0;
}

/*
 * Return the area spanned by the two vectors P1 and P2
 * Works only in 3D.
 */
double v_area(VECTOR * p1, VECTOR * p2)
{
    VECTOR p;

    return 0.5 * v_val(v_cross(&p, p1, p2));
}