spatialite-sys 0.2.0

/*

 grass_crs3d.c -- strictly derived from Grass GIS code: vector/v.rectify/crs3d.c
    
 version 4.3, 2015 May 5

 Author: Sandro Furieri a.furieri@lqt.it

 ------------------------------------------------------------------------------
 DISCLAIMER: this source is strictly derived from Grass GIS code and simply
             contains very trivial adjustments required in order to compile
			 smoothly on libspatialite.
			 NOTE: accordingly to the initial license this file is released
			 under GPL2+ terms
 ------------------------------------------------------------------------------
 
 This program is free software; you can redistribute it and/or
 modify it under the terms of the GNU General Public License
 as published by the Free Software Foundation; either version 2
 of the License, or (at your option) any later version.

 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with this program; if not, write to the Free Software
 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 
*/

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <limits.h>

#if defined(_WIN32) && !defined(__MINGW32__)
#include "config-msvc.h"
#else
#include "config.h"
#endif

#ifdef ENABLE_GCP		/* only if ControlPoints enabled */

/* original code from Grass GIS starts here */

/***********************************************************************

   crs3d.c

   written by: Markus Metz

   based on crs.c - Center for Remote Sensing rectification routines

************************************************************************/


#include "grass_crs.h"

/***********************************************************************

  FUNCTION PROTOTYPES FOR STATIC (INTERNAL) FUNCTIONS

************************************************************************/

static int calccoef (struct Control_Points_3D *, double *, double *, double *,
		     int);
static int calcls (struct Control_Points_3D *, struct MATRIX *, double *,
		   double *, double *, double *, double *, double *);
static int exactdet (struct Control_Points_3D *, struct MATRIX *, double *,
		     double *, double *, double *, double *, double *);
static int solvemat (struct MATRIX *, double *, double *, double *, double *,
		     double *, double *);
static double term (int, double, double, double);

/***********************************************************************

  TRANSFORM A SINGLE COORDINATE PAIR.

************************************************************************/

GCP_PRIVATE int
gcp_CRS_georef_3d (double e1,	/* EASTING TO BE TRANSFORMED */
		   double n1,	/* NORTHING TO BE TRANSFORMED */
		   double z1,	/* HEIGHT TO BE TRANSFORMED */
		   double *e,	/* EASTING, TRANSFORMED */
		   double *n,	/* NORTHING, TRANSFORMED */
		   double *z,	/* HEIGHT, TRANSFORMED */
		   double E[],	/* EASTING COEFFICIENTS */
		   double N[],	/* NORTHING COEFFICIENTS */
		   double Z[],	/* HEIGHT COEFFICIENTS */
		   int order	/* ORDER OF TRANSFORMATION TO BE PERFORMED, MUST MATCH THE
				   ORDER USED TO CALCULATE THE COEFFICIENTS */
    )
{
    double e2, n2, z2, en, ez, nz,
	e3, n3, z3, e2n, e2z, en2, ez2, n2z, nz2, enz;

    switch (order)
      {
      case 1:
	  *e = E[0] + E[1] * e1 + E[2] * n1 + E[3] * z1;
	  *n = N[0] + N[1] * e1 + N[2] * n1 + N[3] * z1;
	  *z = Z[0] + Z[1] * e1 + Z[2] * n1 + Z[3] * z1;
	  break;

      case 2:
	  e2 = e1 * e1;
	  en = e1 * n1;
	  ez = e1 * z1;
	  n2 = n1 * n1;
	  nz = n1 * z1;
	  z2 = z1 * z1;

	  *e = E[0] + E[1] * e1 + E[2] * n1 + E[3] * z1 +
	      E[4] * e2 + E[5] * en + E[6] * ez + E[7] * n2 + E[8] * nz +
	      E[9] * z2;
	  *n = N[0] + N[1] * e1 + N[2] * n1 + N[3] * z1 + N[4] * e2 +
	      N[5] * en + N[6] * ez + N[7] * n2 + N[8] * nz + N[9] * z2;
	  *z = Z[0] + Z[1] * e1 + Z[2] * n1 + Z[3] * z1 + Z[4] * e2 +
	      Z[5] * en + Z[6] * ez + Z[7] * n2 + Z[8] * nz + Z[9] * z2;
	  break;

      case 3:
	  e2 = e1 * e1;
	  en = e1 * n1;
	  ez = e1 * z1;
	  n2 = n1 * n1;
	  nz = n1 * z1;
	  z2 = z1 * z1;

	  e3 = e1 * e2;
	  e2n = e2 * n1;
	  e2z = e2 * z1;
	  en2 = e1 * n2;
	  enz = e1 * n1 * z1;
	  ez2 = e1 * z2;
	  n3 = n1 * n2;
	  n2z = n2 * z1;
	  nz2 = n1 * z2;
	  z3 = z1 * z2;

	  *e = E[0] + E[1] * e1 + E[2] * n1 + E[3] * z1 +
	      E[4] * e2 + E[5] * en + E[6] * ez + E[7] * n2 + E[8] * nz +
	      E[9] * z2 + E[10] * e3 + E[11] * e2n + E[12] * e2z + E[13] * en2 +
	      E[14] * enz + E[15] * ez2 + E[16] * n3 + E[17] * n2z +
	      E[18] * nz2 + E[19] * z3;
	  *n = N[0] + N[1] * e1 + N[2] * n1 + N[3] * z1 + N[4] * e2 +
	      N[5] * en + N[6] * ez + N[7] * n2 + N[8] * nz + N[9] * z2 +
	      N[10] * e3 + N[11] * e2n + N[12] * e2z + N[13] * en2 +
	      N[14] * enz + N[15] * ez2 + N[16] * n3 + N[17] * n2z +
	      N[18] * nz2 + N[19] * z3;
	  *z = Z[0] + Z[1] * e1 + Z[2] * n1 + Z[3] * z1 + Z[4] * e2 +
	      Z[5] * en + Z[6] * ez + Z[7] * n2 + Z[8] * nz + Z[9] * z2 +
	      Z[10] * e3 + Z[11] * e2n + Z[12] * e2z + Z[13] * en2 +
	      Z[14] * enz + Z[15] * ez2 + Z[16] * n3 + Z[17] * n2z +
	      Z[18] * nz2 + Z[19] * z3;
	  break;

      default:
	  return MPARMERR;
      }

    return MSUCCESS;
}

/***********************************************************************

  COMPUTE THE FORWARD AND BACKWARD GEOREFFERENCING COEFFICIENTS
  BASED ON A SET OF CONTROL POINTS

************************************************************************/

GCP_PRIVATE int
gcp_CRS_compute_georef_equations_3d (struct Control_Points_3D *cp,
				     double E12[], double N12[], double Z12[],
				     double E21[], double N21[], double Z21[],
				     int order)
{
    double *tempptr;
    int status;

    if (order < 1 || order > MAXORDER)
	return MPARMERR;

    /* CALCULATE THE FORWARD TRANSFORMATION COEFFICIENTS */
    status = calccoef (cp, E12, N12, Z12, order);

    if (status != MSUCCESS)
	return status;

    /* SWITCH THE 1 AND 2 EASTING, NORTHING, AND HEIGHT ARRAYS */

    tempptr = cp->e1;
    cp->e1 = cp->e2;
    cp->e2 = tempptr;
    tempptr = cp->n1;
    cp->n1 = cp->n2;
    cp->n2 = tempptr;
    tempptr = cp->z1;
    cp->z1 = cp->z2;
    cp->z2 = tempptr;

    /* CALCULATE THE BACKWARD TRANSFORMATION COEFFICIENTS */

    status = calccoef (cp, E21, N21, Z21, order);

    /* SWITCH THE 1 AND 2 EASTING, NORTHING, AND HEIGHT ARRAYS BACK */

    tempptr = cp->e1;
    cp->e1 = cp->e2;
    cp->e2 = tempptr;
    tempptr = cp->n1;
    cp->n1 = cp->n2;
    cp->n2 = tempptr;
    tempptr = cp->z1;
    cp->z1 = cp->z2;
    cp->z2 = tempptr;

    return status;
}

/***********************************************************************

  COMPUTE THE GEOREFFERENCING COEFFICIENTS
  BASED ON A SET OF CONTROL POINTS

************************************************************************/

static int
calccoef (struct Control_Points_3D *cp,
	  double E[], double N[], double Z[], int order)
{
    struct MATRIX m;
    double *a;
    double *b;
    double *c;
    int numactive;		/* NUMBER OF ACTIVE CONTROL POINTS */
    int status, i;

    /* CALCULATE THE NUMBER OF VALID CONTROL POINTS */

    for (i = numactive = 0; i < cp->count; i++)
      {
	  if (cp->status[i] > 0)
	      numactive++;
      }

    /* CALCULATE THE MINIMUM NUMBER OF CONTROL POINTS NEEDED TO DETERMINE
       A TRANSFORMATION OF THIS ORDER */

    /*
       2D    3D
       1st order:    3     4
       2nd order:    6    10
       3rd order:   10    20
     */

    m.n = numactive + 1;

    if (order == 1)
	m.n = 4;
    else if (order == 2)
	m.n = 10;
    else if (order == 3)
	m.n = 20;

    if (numactive < m.n)
	return MNPTERR;

    /* INITIALIZE MATRIX */

    m.v = calloc (m.n * m.n, sizeof (double));
    a = calloc (m.n, sizeof (double));
    b = calloc (m.n, sizeof (double));
    c = calloc (m.n, sizeof (double));

    if (numactive == m.n)
	status = exactdet (cp, &m, a, b, c, E, N, Z);
    else
	status = calcls (cp, &m, a, b, c, E, N, Z);

    free (m.v);
    free (a);
    free (b);
    free (c);

    return status;
}

/***********************************************************************

  CALCULATE THE TRANSFORMATION COEFFICIENTS WITH EXACTLY THE MINIMUM
  NUMBER OF CONTROL POINTS REQUIRED FOR THIS TRANSFORMATION.

************************************************************************/

static int
exactdet (struct Control_Points_3D *cp, struct MATRIX *m, double a[], double b[], double c[], double E[],	/* EASTING COEFFICIENTS */
	  double N[],		/* NORTHING COEFFICIENTS */
	  double Z[]		/* HEIGHT COEFFICIENTS */
    )
{
    int pntnow, currow, j;

    currow = 1;
    for (pntnow = 0; pntnow < cp->count; pntnow++)
      {
	  if (cp->status[pntnow] > 0)
	    {
		/* POPULATE MATRIX M */

		for (j = 1; j <= m->n; j++)
		    M (currow, j) =
			term (j, cp->e1[pntnow], cp->n1[pntnow],
			      cp->z1[pntnow]);

		/* POPULATE MATRIX A AND B */

		a[currow - 1] = cp->e2[pntnow];
		b[currow - 1] = cp->n2[pntnow];
		c[currow - 1] = cp->z2[pntnow];

		currow++;
	    }
      }

    if (currow - 1 != m->n)
	return MINTERR;

    return solvemat (m, a, b, c, E, N, Z);
}

/***********************************************************************

  CALCULATE THE TRANSFORMATION COEFFICIENTS WITH MORE THAN THE MINIMUM
  NUMBER OF CONTROL POINTS REQUIRED FOR THIS TRANSFORMATION.  THIS
  ROUTINE USES THE LEAST SQUARES METHOD TO COMPUTE THE COEFFICIENTS.

************************************************************************/

static int
calcls (struct Control_Points_3D *cp, struct MATRIX *m, double a[], double b[], double c[], double E[],	/* EASTING COEFFICIENTS */
	double N[],		/* NORTHING COEFFICIENTS */
	double Z[]		/* HEIGHT COEFFICIENTS */
    )
{
    int i, j, n, numactive = 0;

    /* INITIALIZE THE UPPER HALF OF THE MATRIX AND THE TWO COLUMN VECTORS */

    for (i = 1; i <= m->n; i++)
      {
	  for (j = i; j <= m->n; j++)
	      M (i, j) = 0.0;
	  a[i - 1] = b[i - 1] = c[i - 1] = 0.0;
      }

    /* SUM THE UPPER HALF OF THE MATRIX AND THE COLUMN VECTORS ACCORDING TO
       THE LEAST SQUARES METHOD OF SOLVING OVER DETERMINED SYSTEMS */

    for (n = 0; n < cp->count; n++)
      {
	  if (cp->status[n] > 0)
	    {
		numactive++;
		for (i = 1; i <= m->n; i++)
		  {
		      for (j = i; j <= m->n; j++)
			  M (i, j) +=
			      term (i, cp->e1[n], cp->n1[n],
				    cp->z1[n]) * term (j, cp->e1[n], cp->n1[n],
						       cp->z1[n]);

		      a[i - 1] +=
			  cp->e2[n] * term (i, cp->e1[n], cp->n1[n], cp->z1[n]);
		      b[i - 1] +=
			  cp->n2[n] * term (i, cp->e1[n], cp->n1[n], cp->z1[n]);
		      c[i - 1] +=
			  cp->z2[n] * term (i, cp->e1[n], cp->n1[n], cp->z1[n]);
		  }
	    }
      }

    if (numactive <= m->n)
	return MINTERR;

    /* TRANSPOSE VALUES IN UPPER HALF OF M TO OTHER HALF */

    for (i = 2; i <= m->n; i++)
	for (j = 1; j < i; j++)
	    M (i, j) = M (j, i);

    return solvemat (m, a, b, c, E, N, Z);
}

/***********************************************************************

  CALCULATE THE X/Y TERM BASED ON THE TERM NUMBER

  ORDER\TERM   1      2      3      4      5      6      7      8      9      10
  1            e0n0z0 e1n0z0 e0n1z0 e0n0z1
  2            e0n0z0 e1n0z0 e0n1z0 e0n0z1 e2n0z0 e1n1z0 e1n0z1 e0n2z0 e0n1z1 e0n0z2
  3            e0n0z0 e1n0z0 e0n1z0 e0n0z1 e2n0z0 e1n1z0 e1n0z1 e0n2z0 e0n1z1 e0n0z2

  ORDER\TERM   11      12     13     14     15     16     17     18     19     20
  3            e3n0z0  e2n1z0 e2n0z1 e1n2z0 e1n1z1 e1n0z2 e0n3z0 e0n2z1 e0n1z2 e0n0z3

************************************************************************/

static double
term (int term, double e, double n, double z)
{
    switch (term)
      {
	  /* 1st order */
      case 1:
	  return 1.0;
      case 2:
	  return e;
      case 3:
	  return n;
      case 4:
	  return z;
	  /* 2nd order */
      case 5:
	  return e * e;
      case 6:
	  return e * n;
      case 7:
	  return e * z;
      case 8:
	  return n * n;
      case 9:
	  return n * z;
      case 10:
	  return z * z;
	  /* 3rd order */
      case 11:
	  return e * e * e;
      case 12:
	  return e * e * n;
      case 13:
	  return e * e * z;
      case 14:
	  return e * n * n;
      case 15:
	  return e * n * z;
      case 16:
	  return e * z * z;
      case 17:
	  return n * n * n;
      case 18:
	  return n * n * z;
      case 19:
	  return n * z * z;
      case 20:
	  return z * z * z;
      }

    return 0.0;
}

/***********************************************************************

  SOLVE FOR THE 'E', 'N' AND 'Z' COEFFICIENTS BY USING A
  SOMEWHAT MODIFIED GAUSSIAN ELIMINATION METHOD.

  | M11 M12 ... M1n | | E0   |   | a0   |
  | M21 M22 ... M2n | | E1   | = | a1   |
  |  .   .   .   .  | | .    |   | .    |
  | Mn1 Mn2 ... Mnn | | En-1 |   | an-1 |

  ,

  | M11 M12 ... M1n | | N0   |   | b0   |
  | M21 M22 ... M2n | | N1   | = | b1   |
  |  .   .   .   .  | | .    |   | .    |
  | Mn1 Mn2 ... Mnn | | Nn-1 |   | bn-1 |

  and

  | M11 M12 ... M1n | | Z0   |   | c0   |
  | M21 M22 ... M2n | | Z1   | = | c1   |
  |  .   .   .   .  | | .    |   | .    |
  | Mn1 Mn2 ... Mnn | | Zn-1 |   | cn-1 |

************************************************************************/

static int
solvemat (struct MATRIX *m, double a[], double b[], double c[],
	  double E[], double N[], double Z[])
{
    int i, j, i2, j2, imark;
    double factor, temp;
    double pivot;		/* ACTUAL VALUE OF THE LARGEST PIVOT CANDIDATE */

    for (i = 1; i <= m->n; i++)
      {
	  j = i;

	  /* find row with largest magnitude value for pivot value */

	  pivot = M (i, j);
	  imark = i;
	  for (i2 = i + 1; i2 <= m->n; i2++)
	    {
		temp = fabs (M (i2, j));
		if (temp > fabs (pivot))
		  {
		      pivot = M (i2, j);
		      imark = i2;
		  }
	    }

	  /* if the pivot is very small then the points are nearly co-linear */
	  /* co-linear points result in an undefined matrix, and nearly */
	  /* co-linear points results in a solution with rounding error */

	  if (fabs (pivot) < GRASS_EPSILON)
	      return MUNSOLVABLE;

	  /* if row with highest pivot is not the current row, switch them */

	  if (imark != i)
	    {
		for (j2 = 1; j2 <= m->n; j2++)
		  {
		      temp = M (imark, j2);
		      M (imark, j2) = M (i, j2);
		      M (i, j2) = temp;
		  }

		temp = a[imark - 1];
		a[imark - 1] = a[i - 1];
		a[i - 1] = temp;

		temp = b[imark - 1];
		b[imark - 1] = b[i - 1];
		b[i - 1] = temp;

		temp = c[imark - 1];
		c[imark - 1] = c[i - 1];
		c[i - 1] = temp;
	    }

	  /* compute zeros above and below the pivot, and compute
	     values for the rest of the row as well */

	  for (i2 = 1; i2 <= m->n; i2++)
	    {
		if (i2 != i)
		  {
		      factor = M (i2, j) / pivot;
		      for (j2 = j; j2 <= m->n; j2++)
			  M (i2, j2) -= factor * M (i, j2);
		      a[i2 - 1] -= factor * a[i - 1];
		      b[i2 - 1] -= factor * b[i - 1];
		      c[i2 - 1] -= factor * c[i - 1];
		  }
	    }
      }

    /* SINCE ALL OTHER VALUES IN THE MATRIX ARE ZERO NOW, CALCULATE THE
       COEFFICIENTS BY DIVIDING THE COLUMN VECTORS BY THE DIAGONAL VALUES. */

    for (i = 1; i <= m->n; i++)
      {
	  E[i - 1] = a[i - 1] / M (i, i);
	  N[i - 1] = b[i - 1] / M (i, i);
	  Z[i - 1] = c[i - 1] / M (i, i);
      }

    return MSUCCESS;
}

#endif /* end including GCP */