shepard.cpp

这是一个GPS相关的程序

    }
    u = bb + bb;
    v = aa / u;

    /* STORE R IN A. */

    *a = sqrt(v * v + .25f) * u;
    *s = bb / *a;
    *c__ = v * (*s + *s);

    /* NOTE THAT R HAS THE SIGN OF B, S .GT. 0, AND C HAS */
    /*   SIGN(A)*SIGN(B). */

    *b = 1.f;
    if (*c__ != 0.f)
    {
        *b = 1.f / *c__;
    }
    return 0;

    /* A = B = 0. */

L2:
    *c__ = 1.f;
    *s = 0.f;
    return 0;
} /* givens_ */

int rotate_(int *n,   double *c__,   double *s,   double *x,   double *y)
{
    int i__1;

    static int i__;
    static double xi, yi;


    /****************************************************************/
	/*																*/
    /* 						ROBERT RENKA							*/
    /* 					UNIV. OF NORTH TEXAS						*/
    /*(817) 565 - 2767												*/
	/*																*/
    /*(C  S)														*/
    /*   THIS ROUTINE APPLIES THE GIVENS ROTATION() TO THE			*/
    /*(-S  C)														*/
    /*(X(1) ... X(N))												*/
    /* 2 BY N MATRIX().												*/
    /*(Y(1) ... Y(N))												*/
	/*																*/
    /* ON INPUT --													*/
	/*																*/
    /* 	N = NUMBER OF COLUMNS TO BE ROTATED.						*/
	/*																*/
    /* 	C, S = ELEMENTS OF THE GIVENS ROTATION.	THESE MAY BE		*/
    /* 	      DETERMINED BY SUBROUTINE GIVENS.						*/
	/*																*/
    /* 	X, Y = ARRAYS OF LENGTH .GE. N CONTAINING THE VECTORS		*/
    /* 	      TO BE ROTATED.										*/
	/*																*/
    /* PARAMETERS N, C, AND S ARE NOT ALTERED BY THIS ROUTINE.		*/
	/*																*/
    /* ON OUTPUT --													*/
	/*																*/
    /* 	X, Y = ROTATED VECTORS.										*/
	/*																*/
    /* MODULES REQUIRED BY ROTATE -- NONE							*/
	/*																*/
    /****************************************************************/


    /* LOCAL PARAMETERS -- */

    /* I =	  DO - LOOP INDEX */
    /* XI, YI = X(I), Y(I) */

    /* Parameter adjustments */
    --y;
    --x;

    /* Function Body */
    if (*n <= 0 || *c__ == 1.f && *s == 0.f)
    {
        return 0;
    }
    i__1 = *n;
    for (i__ = 1; i__ <= i__1; ++i__)
    {
        xi = x[i__];
        yi = y[i__];
        x[i__] = *c__ * xi + *s * yi;
        y[i__] = -(*s) * xi + *c__ * yi;
        /* L1: */
    }
    return 0;
} /* rotate_ */

/* Subroutine */ int setup2_(double *xk,   double *yk,   double *fk,   double *xi,   double *yi,
                             double *fi, double *s1, double *s2, double *r__, double *row)
{
//    double sqrt(double);

    static double d__;
    static int i__;
    static double w, w1, w2, dx, dy, dxsq, dysq;


    /****************************************************************/
	/*																*/
    /* 						ROBERT RENKA							*/
    /* 					UNIV. OF NORTH TEXAS						*/
    /*(817) 565 - 2767												*/
	/*																*/
    /*   THIS ROUTINE SETS UP THE I - TH ROW OF AN AUGMENTED RE-	*/
    /* GRESSION MATRIX FOR A WEIGHTED LEAST - SQUARES FIT OF A		*/
    /* QUADRATIC FUNCTION Q(X, Y) TO A SET OF DATA VALUES F, WHERE	*/
    /* Q(XK, YK) = FK.  THE FIRST 3 COLUMNS(QUADRATIC TERMS) ARE	*/
    /* SCALED BY 1/S2 AND THE FOURTH AND FIFTH COLUMNS(LINEAR		*/
    /* TERMS) ARE SCALED BY 1/S1.  THE WEIGHT IS(R - D)/(R*D) IF	*/
    /* R .GT. D AND 0 IF R .LE. D, WHERE D IS THE DISTANCE			*/
    /* BETWEEN NODES I AND K.										*/
	/*																*/
    /* ON INPUT --													*/
	/*																*/
    /* 	XK, YK, FK = COORDINATES AND DATA VALUE AT NODE K --		*/
    /* 		   INTERPOLATED BY Q.									*/
	/*																*/
    /* 	XI, YI, FI = COORDINATES AND DATA VALUE AT NODE I.			*/
	/*																*/
    /* 	S1, S2 = RECIPROCALS OF THE SCALE FACTORS.					*/
	/*																*/
    /* 	R = RADIUS OF INFLUENCE ABOUT NODE K DEFINING THE			*/
    /* 	    WEIGHT.													*/
	/*																*/
    /* 	ROW = ARRAY OF LENGTH 6.									*/
	/*																*/
    /* INPUT PARAMETERS ARE NOT ALTERED BY THIS ROUTINE.			*/
	/*																*/
    /* ON OUTPUT --													*/
	/*																*/
    /* 	ROW = VECTOR CONTAINING A ROW OF THE AUGMENTED				*/
    /* 	      REGRESSION MATRIX.									*/
	/*																*/
    /* MODULES REQUIRED BY SETUP2 -- NONE							*/
	/*																*/
    /* INTRINSIC FUNCTION CALLED BY SETUP2 -- SQRT					*/
	/*																*/
    /****************************************************************/


    /* LOCAL PARAMETERS - */

    /* I =	 DO - LOOP INDEX */
    /* DX =	 XI - XK */
    /* DY =	 YI - YK */
    /* DXSQ = DX*DX */
    /* DYSQ = DY*DY */
    /* D =	 DISTANCE BETWEEN NODES K AND I */
    /* W =	 WEIGHT ASSOCIATED WITH THE ROW */
    /* W1 =	 W/S1 */
    /* W2 =	 W/S2 */

    --row;

    dx = *xi - *xk;
    dy = *yi - *yk;
    dxsq = dx * dx;
    dysq = dy * dy;
    d__ = sqrt(dxsq + dysq);
    if (d__ <= 0.f || d__ >= *r__)
    {
        goto L1;
    }
    w = (*r__ - d__) / *r__ / d__;
    w1 = w / *s1;
    w2 = w / *s2;
    row[1] = dxsq * w2;
    row[2] = dx * dy * w2;
    row[3] = dysq * w2;
    row[4] = dx * w1;
    row[5] = dy * w1;
    row[6] = (*fi - *fk) * w;
    return 0;

    /* NODES K AND I COINCIDE OR NODE I IS OUTSIDE OF THE RADIUS */
    /*   OF INFLUENCE.  SET ROW TO THE ZERO VECTOR. */

L1:
    for (i__ = 1; i__ <= 6; ++i__)
    {
        /* L2: */
        row[i__] = 0.f;
    }
    return 0;
} /* setup2_ */

/* Subroutine */ int store2_(int *n,   double *x,   double *y,   int *nr,
                             int *lcell, int *lnext, double *xmin, double *ymin, double *dx,
                             double *dy, int *ier)
{
    int lcell_dim1, lcell_offset, i__1, i__2;

    static int i__, j, k, l, kb, nn, np1, nnr;
    static double xmn, ymn, xmx, ymx, delx, dely;


    /****************************************************************/
	/*																*/	
    /* 						ROBERT RENKA							*/
    /* 					UNIV. OF NORTH TEXAS						*/
    /*(817) 565 - 2767												*/
	/*																*/
    /*   GIVEN A SET OF N ARBITRARILY DISTRIBUTED NODES IN THE		*/
    /* PLANE, THIS SUBROUTINE CREATES A DATA STRUCTURE FOR A		*/
    /* CELL - BASED METHOD OF SOLVING CLOSEST - POINT PROBLEMS.	THE */
    /* SMALLEST RECTANGLE CONTAINING THE NODES IS PARTITIONED		*/
    /* INTO AN NR BY NR UNIFORM GRID OF CELLS, AND NODES ARE AS-	*/
    /* SOCIATED WITH CELLS.	IN PARTICULAR, THE DATA STRUCTURE		*/
    /* STORES THE INDICES OF THE NODES CONTAINED IN EACH CELL.		*/
    /* FOR A UNIFORM RANDOM DISTRIBUTION OF NODES, THE NEAREST		*/
    /* NODE TO AN ARBITRARY POINT CAN BE DETERMINED IN CONSTANT		*/
    /* EXPECTED TIME.												*/
	/*																*/
    /* ON INPUT --													*/
	/*																*/
    /* 	N = NUMBER OF NODES.  N .GE. 2.								*/
	/*																*/
    /* 	X, Y = ARRAYS OF LENGTH N CONTAINING THE CARTESIAN			*/
    /* 	      COORDINATES OF THE NODES.								*/
	/*																*/
    /* 	NR = NUMBER OF ROWS AND COLUMNS IN THE GRID.  THE			*/
    /* 	     CELL DENSITY(AVERAGE NUMBER OF NODES PER CELL)			*/
    /* 	     IS D = N/(NR**2).	A RECOMMENDED VALUE, BASED			*/
    /* 	     ON EMPIRICAL EVIDENCE, IS D = 3 -- NR =				*/
    /* 	     SQRT(N/3).  NR .GE. 1.									*/
	/*																*/
    /* THE ABOVE PARAMETERS ARE NOT ALTERED BY THIS ROUTINE.		*/
	/*																*/
    /* 	LCELL = ARRAY OF LENGTH .GE. NR**2.							*/
	/*																*/
    /* 	LNEXT = ARRAY OF LENGTH .GE. N.								*/
	/*																*/
    /* ON OUTPUT --													*/
	/*																*/
    /* 	LCELL = NR BY NR CELL ARRAY SUCH THAT LCELL(I, J)			*/
    /* 		CONTAINS THE INDEX(FOR X AND Y) OF THE					*/
    /* 		FIRST NODE(NODE WITH SMALLEST INDEX) IN					*/
    /* 		CELL(I, J), OR LCELL(I, J) = 0 IF NO NODES				*/
    /* 		ARE CONTAINED IN THE CELL.  THE UPPER RIGHT				*/
    /* 		CORNER OF CELL(I, J) HAS COORDINATES(XMIN+				*/
    /* 		I*DX, YMIN + J*DY).  LCELL IS NOT DEFINED IF			*/
    /* 		IER .NE. 0.												*/
	/*																*/
    /* 	LNEXT = ARRAY OF NEXT - NODE INDICES SUCH THAT				*/
    /* 		LNEXT(K) CONTAINS THE INDEX OF THE NEXT NODE			*/
    /* 		IN THE CELL WHICH CONTAINS NODE K, OR					*/
    /* 		LNEXT(K) = K IF K IS THE LAST NODE IN THE				*/
    /* 		CELL FOR K = 1,..., N.(THE NODES CONTAINED				*/
    /* 		IN A CELL ARE ORDERED BY THEIR INDICES.)				*/
    /* 		IF, FOR EXAMPLE, CELL(I, J) CONTAINS NODES				*/
    /* 		2, 3, AND 5(AND NO OTHERS), THEN LCELL(I, J)			*/
    /* 		= 2, LNEXT(2) = 3, LNEXT(3) = 5, AND					*/
    /* 		LNEXT(5) = 5.  LNEXT IS NOT DEFINED IF					*/
    /* 		IER .NE. 0.												*/
	/*																*/
    /* 	XMIN, YMIN = CARTESIAN COORDINATES OF THE LOWER LEFT		*/
    /* 		    CORNER OF THE RECTANGLE DEFINED BY THE				*/
    /* 		    NODES(SMALLEST NODAL COORDINATES) UN-				*/
    /* 		    LESS IER = 1.  THE UPPER RIGHT CORNER IS			*/
    /*(XMAX, YMAX) FOR XMAX = XMIN + NR*DX AND						*/
    /* 		    YMAX = YMIN + NR*DY.								*/
	/*																*/
    /* 	DX, DY = DIMENSIONS OF THE CELLS UNLESS IER = 1.  DX		*/
    /* 		=(XMAX - XMIN)/NR AND DY =(YMAX - YMIN)/NR				*/
    /* 		WHERE XMIN, XMAX, YMIN, AND YMAX ARE THE				*/
    /* 		EXTREMA OF X AND Y.										*/
	/*																*/
    /* 	IER = ERROR INDICATOR --									*/
    /* 	      IER = 0 IF NO ERRORS WERE ENCOUNTERED.				*/
    /* 	      IER = 1 IF N .LT. 2 OR NR .LT. 1.						*/
    /* 	      IER = 2 IF DX = 0 OR DY = 0.							*/
	/*																*/
    /* MODULES REQUIRED BY STORE2 -- NONE							*/
	/*																*/
    /* INTRINSIC FUNCTIONS CALLED BY STORE2 -- FLOAT, IFIX			*/
	/*																*/
    /****************************************************************/

    /* Parameter adjustments */
    --lnext;
    --y;
    --x;
    lcell_dim1 = *nr;
    lcell_offset = 1 + lcell_dim1;
    lcell -= lcell_offset;

    /* Function Body */
    nn = *n;
    nnr = *nr;
    if (nn < 2 || nnr < 1)
    {
        goto L4;
    }

    /* COMPUTE THE DIMENSIONS OF THE RECTANGLE CONTAINING THE */
    /*   NODES. */

    xmn = x[1];
    xmx = xmn;
    ymn = y[1];
    ymx = ymn;
    i__1 = nn;
    for (k = 2; k <= i__1; ++k)
    {
        if (x[k] < xmn)
        {
            xmn = x[k];
        }
        if (x[k] > xmx)
        {
            xmx = x[k];
        }
        if (y[k] < ymn)
        {
            ymn = y[k];
        }
        /* L1: */
        if (y[k] > ymx)
        {
            ymx = y[k];
        }
    }
    *xmin = xmn;
    *ymin = ymn;

    /* COMPUTE CELL DIMENSIONS AND TEST FOR ZERO AREA. */

    delx = (xmx - xmn) / (double) nnr;
    dely = (ymx - ymn) / (double) nnr;
    *dx = delx;
    *dy = dely;
    if (delx == 0.f || dely == 0.f)
    {
        goto L5;
    }

    /* INITIALIZE LCELL. */

    i__1 = nnr;
    for (j = 1; j <= i__1; ++j)
    {
        i__2 = nnr;
        for (i__ = 1; i__ <= i__2; ++i__)
        {
            /* L2: */
            lcell[i__ + j * lcell_dim1] = 0;
        }
    }

    /* LOOP ON NODES, STORING INDICES IN LCELL AND LNEXT. */

    np1 = nn + 1;
    i__2 = nn;
    for (k = 1; k <= i__2; ++k)
    {
        kb = np1 - k;
        i__ = (int)((x[kb] - xmn) / delx) + 1;
        if (i__ > nnr)
        {
            i__ = nnr;
        }
        j = (int)((y[kb] - ymn) / dely) + 1;
        if (j > nnr)
        {
            j = nnr;
        }
        l = lcell[i__ + j * lcell_dim1];
        lnext[kb] = l;
        if (l == 0)
        {
            lnext[kb] = kb;
        }
        /* L3: */
        lcell[i__ + j * lcell_dim1] = kb;
    }

    /* NO ERRORS ENCOUNTERED */

    *ier = 0;
    return 0;

    /* INVALID INPUT PARAMETER */

L4:
    *ier = 1;
    return 0;

    /* DX = 0 OR DY = 0 */

L5:
    *ier = 2;
    return 0;
}