shepard.cpp

这是一个GPS相关的程序

    /* INVALID INPUT PARAMETER. */

L5:
    *ier = 1;
    return 0;

    /* NO CELLS CONTAIN A POINT WITHIN RMAX OF P, OR */
    /*   SW = 0 AND THUS DS .GE. RSQ(K) FOR ALL K. */

L6:
    *q = 0.f;
    *qx = 0.f;
    *qy = 0.f;
    *ier = 2;
    return 0;
} /* qs2grd_ */

/* Subroutine */ int getnp2_(double *px,   double *py,   double *x,   double *y,   int *
                             nr, int *lcell, int *lnext, double *xmin, double *ymin, double *dx,
                             double *dy, int *np, double *dsq)
{
    int lcell_dim1, lcell_offset, i__1, i__2;
    double r__1, r__2;

//    double sqrt(double);

    static int i__, j, l;
    static double r__;
    static int i0, j0, i1, i2, j1, j2, ln;
    static double xp, yp, rsq;
    static int imin, jmin, imax, jmax;
    static double delx, dely;
    static int lmin;
    static bool first;
    static double rsmin;


    /****************************************************************/
	/*																*/
    /* 						ROBERT RENKA							*/
    /* 					UNIV. OF NORTH TEXAS						*/
    /*(817) 565 - 2767												*/
	/*																*/
    /*   GIVEN A SET OF N NODES AND THE DATA STRUCTURE DEFINED IN	*/
    /* SUBROUTINE STORE2, THIS SUBROUTINE USES THE CELL METHOD TO	*/
    /* FIND THE CLOSEST UNMARKED NODE NP TO A SPECIFIED POINT P.	*/
    /* NP IS THEN MARKED BY SETTING LNEXT(NP) TO -LNEXT(NP).(A		*/
    /* NODE IS MARKED IF AND ONLY IF THE CORRESPONDING LNEXT ELE-	*/
    /* MENT IS NEGATIVE.  THE ABSOLUTE VALUES OF LNEXT ELEMENTS,	*/
    /* HOWEVER, MUST BE PRESERVED.)	THUS, THE CLOSEST M NODES TO	*/
    /* P MAY BE DETERMINED BY A SEQUENCE OF M CALLS TO THIS ROU-	*/
    /* TINE.  NOTE THAT IF THE NEAREST NEIGHBOR TO NODE K IS TO		*/
    /* BE DETERMINED(PX = X(K) AND PY = Y(K)), THEN K SHOULD BE		*/
    /* MARKED BEFORE THE CALL TO THIS ROUTINE.						*/
    /*   THE SEARCH IS BEGUN IN THE CELL CONTAINING(OR CLOSEST		*/
    /* TO) P AND PROCEEDS OUTWARD IN RECTANGULAR LAYERS UNTIL ALL	*/
    /* CELLS WHICH CONTAIN POINTS WITHIN DISTANCE R OF P HAVE		*/
    /* BEEN SEARCHED, WHERE R IS THE DISTANCE FROM P TO THE FIRST	*/
    /* UNMARKED NODE ENCOUNTERED(INFINITE IF NO UNMARKED NODES		*/
    /* ARE PRESENT).												*/
	/*																*/
    /* ON INPUT --													*/
	/*																*/
    /* 	PX, PY = CARTESIAN COORDINATES OF THE POINT P WHOSE			*/
    /* 		NEAREST UNMARKED NEIGHBOR IS TO BE FOUND.				*/
	/*																*/
    /* 	X, Y = ARRAYS OF LENGTH N, FOR N .GE. 2, CONTAINING			*/
    /* 	      THE CARTESIAN COORDINATES OF THE NODES.				*/
	/*																*/
    /* 	NR = NUMBER OF ROWS AND COLUMNS IN THE CELL GRID.			*/
    /* 	     NR .GE. 1.												*/
	/*																*/
    /* 	LCELL = NR BY NR ARRAY OF NODAL INDICES ASSOCIATED			*/
    /* 		WITH CELLS.												*/
	/*																*/
    /* 	LNEXT = ARRAY OF LENGTH N CONTAINING NEXT - NODE INDI-		*/
    /* 		CES(OR THEIR NEGATIVES).								*/
	/*																*/
    /* 	XMIN, YMIN, DX, DY = MINIMUM NODAL COORDINATES AND CELL		*/
    /* 			  DIMENSIONS.  DX AND DY MUST BE					*/
    /* 			  POSITIVE.											*/
	/*																*/
    /*   INPUT PARAMETERS OTHER THAN LNEXT ARE NOT ALTERED BY		*/
    /* THIS ROUTINE.  WITH THE EXCEPTION OF(PX, PY) AND THE SIGNS	*/
    /* OF LNEXT ELEMENTS, THESE PARAMETERS SHOULD BE UNALTERED		*/
    /* FROM THEIR VALUES ON OUTPUT FROM SUBROUTINE STORE2.			*/
	/*																*/
    /* ON OUTPUT --													*/
	/*																*/
    /* 	NP = INDEX(FOR X AND Y) OF THE NEAREST UNMARKED				*/
    /* 	     NODE TO P, OR 0 IF ALL NODES ARE MARKED OR NR			*/
    /* 	     .LT. 1 OR DX .LE. 0 OR DY .LE. 0.	LNEXT(NP)			*/
    /* 	     .LT. 0 IF NP .NE. 0.									*/
	/*																*/
    /* 	DSQ = SQUARED EUCLIDEAN DISTANCE BETWEEN P AND NODE			*/
    /* 	      NP, OR 0 IF NP = 0.									*/
	/*																*/
    /* MODULES REQUIRED BY GETNP2 -- NONE							*/
	/*																*/
    /* INTRINSIC FUNCTIONS CALLED BY GETNP2 -- IABS, IFIX, SQRT		*/
	/*																*/
    /****************************************************************/

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

    xp = *px;
    yp = *py;

    /* TEST FOR INVALID INPUT PARAMETERS. */

    if (*nr < 1 || *dx <= 0.f || *dy <= 0.f)
    {
        goto L9;
    }

    /* INITIALIZE PARAMETERS -- */

    /*   FIRST = TRUE IFF THE FIRST UNMARKED NODE HAS YET TO BE */
    /* 	    ENCOUNTERED, */
    /*   IMIN, IMAX, JMIN, JMAX = CELL INDICES DEFINING THE RANGE OF */
    /* 			  THE SEARCH, */
    /*   DELX, DELY = PX - XMIN AND PY - YMIN, */
    /*   I0, J0 = CELL CONTAINING OR CLOSEST TO P, */
    /*   I1, I2, J1, J2 = CELL INDICES OF THE LAYER WHOSE INTERSEC- */
    /* 		  TION WITH THE RANGE DEFINED BY IMIN,..., */
    /* 		  JMAX IS CURRENTLY BEING SEARCHED. */

    first = true;
    imin = 1;
    imax = *nr;
    jmin = 1;
    jmax = *nr;
    delx = xp - *xmin;
    dely = yp - *ymin;
    i0 = (int)(delx / *dx) + 1;
    if (i0 < 1)
    {
        i0 = 1;
    }
    if (i0 > *nr)
    {
        i0 = *nr;
    }
    j0 = (int)(dely / *dy) + 1;
    if (j0 < 1)
    {
        j0 = 1;
    }
    if (j0 > *nr)
    {
        j0 = *nr;
    }
    i1 = i0;
    i2 = i0;
    j1 = j0;
    j2 = j0;

    /* OUTER LOOP ON LAYERS, INNER LOOP ON LAYER CELLS, EXCLUDING */
    /*   THOSE OUTSIDE THE RANGE(IMIN, IMAX) X(JMIN, JMAX). */

L1:
    i__1 = j2;
    for (j = j1; j <= i__1; ++j)
    {
        if (j > jmax)
        {
            goto L7;
        }
        if (j < jmin)
        {
            goto L6;
        }
        i__2 = i2;
        for (i__ = i1; i__ <= i__2; ++i__)
        {
            if (i__ > imax)
            {
                goto L6;
            }
            if (i__ < imin)
            {
                goto L5;
            }
            if (j != j1 && j != j2 && i__ != i1 && i__ != i2)
            {
                goto L5;
            }

            /* SEARCH CELL(I, J) FOR UNMARKED NODES L. */

            l = lcell[i__ + j * lcell_dim1];
            if (l == 0)
            {
                goto L5;
            }

            /*   LOOP ON NODES IN CELL(I, J). */

        L2:
            ln = lnext[l];
            if (ln < 0)
            {
                goto L4;
            }

            /*   NODE L IS NOT MARKED. */

            /* Computing 2nd power */
            r__1 = x[l] - xp;
            /* Computing 2nd power */
            r__2 = y[l] - yp;
            rsq = r__1 * r__1 + r__2 * r__2;
            if (! first)
            {
                goto L3;
            }

            /*   NODE L IS THE FIRST UNMARKED NEIGHBOR OF P ENCOUNTERED. */
            /*     INITIALIZE LMIN TO THE CURRENT CANDIDATE FOR NP, AND */
            /*     RSMIN TO THE SQUARED DISTANCE FROM P TO LMIN.  IMIN, */
            /*     IMAX, JMIN, AND JMAX ARE UPDATED TO DEFINE THE SMAL- */
            /*     LEST RECTANGLE CONTAINING A CIRCLE OF RADIUS R = */
            /*     SQRT(RSMIN) CENTERED AT P, AND CONTAINED IN(1, NR) X */
            /*(1, NR)(EXCEPT THAT, IF P IS OUTSIDE THE RECTANGLE */
            /*     DEFINED BY THE NODES, IT IS POSSIBLE THAT IMIN .GT. */
            /*     NR, IMAX .LT. 1, JMIN .GT. NR, OR JMAX .LT. 1).  FIRST */
            /*     IS RESET TO FALSE. */

            lmin = l;
            rsmin = rsq;
            r__ = sqrt(rsmin);
            imin = (int)((delx - r__) / *dx) + 1;
            if (imin < 1)
            {
                imin = 1;
            }
            imax = (int)((delx + r__) / *dx) + 1;
            if (imax > *nr)
            {
                imax = *nr;
            }
            jmin = (int)((dely - r__) / *dy) + 1;
            if (jmin < 1)
            {
                jmin = 1;
            }
            jmax = (int)((dely + r__) / *dy) + 1;
            if (jmax > *nr)
            {
                jmax = *nr;
            }
            first = false;
            goto L4;

            /*   TEST FOR NODE L CLOSER THAN LMIN TO P. */

        L3:
            if (rsq >= rsmin)
            {
                goto L4;
            }

            /*   UPDATE LMIN AND RSMIN. */

            lmin = l;
            rsmin = rsq;

            /*   TEST FOR TERMINATION OF LOOP ON NODES IN CELL(I, J). */

        L4:
            if (abs(ln) == l)
            {
                goto L5;
            }
            l = abs(ln);
            goto L2;
        L5:
            ;
        }
    L6:
        ;
    }

    /* TEST FOR TERMINATION OF LOOP ON CELL LAYERS. */

L7:
    if (i1 <= imin && i2 >= imax && j1 <= jmin && j2 >= jmax)
    {
        goto L8;
    }
    --i1;
    ++i2;
    --j1;
    ++j2;
    goto L1;

    /* UNLESS NO UNMARKED NODES WERE ENCOUNTERED, LMIN IS THE */
    /*   CLOSEST UNMARKED NODE TO P. */

L8:
    if (first)
    {
        goto L9;
    }
    *np = lmin;
    *dsq = rsmin;
    lnext[lmin] = -lnext[lmin];
    return 0;

    /* ERROR -- NR, DX, OR DY IS INVALID OR ALL NODES ARE MARKED. */

L9:
    *np = 0;
    *dsq = 0.f;
    return 0;
} /* getnp2_ */

/* Subroutine */ int givens_(double *a,   double *b,   double *c__,   double *s)
{
    /* Builtin functions */
//    double sqrt(double);

    /* Local variables */
    static double r__, u, v, aa, bb;


    /****************************************************************/
	/*																*/
    /* 						ROBERT RENKA							*/
    /* 					UNIV. OF NORTH TEXAS						*/
    /*(817) 565 - 2767												*/
	/*																*/
    /*   THIS ROUTINE CONSTRUCTS THE GIVENS PLANE ROTATION --		*/
    /*(C  S)														*/
    /* G =() WHERE C*C + S*S = 1 -- WHICH ZEROS THE SECOND			*/
    /*(-S  C)														*/
    /* ENTRY OF THE 2 - VECTOR(A B) - TRANSPOSE.  A CALL TO GIVENS	*/
    /* IS NORMALLY FOLLOWED BY A CALL TO ROTATE WHICH APPLIES		*/
    /* THE TRANSFORMATION TO A 2 BY N MATRIX.  THIS ROUTINE WAS		*/
    /* TAKEN FROM LINPACK.											*/
	/*																*/
    /* ON INPUT --													*/
	/*																*/
    /* 	A, B = COMPONENTS OF THE 2 - VECTOR TO BE ROTATED.			*/
	/*																*/
    /* ON OUTPUT --													*/
	/*																*/
    /* 	A = VALUE OVERWRITTEN BY R = +/-SQRT(A*A + B*B)				*/
	/*																*/
    /* 	B = VALUE OVERWRITTEN BY A VALUE Z WHICH ALLOWS C			*/
    /* 	    AND S TO BE RECOVERED AS FOLLOWS --						*/
    /* 	      C = SQRT(1 - Z*Z), S = Z     IF ABS(Z) .LE. 1.		*/
    /* 	      C = 1/Z, S = SQRT(1 - C*C) IF ABS(Z) .GT. 1.			*/
	/*																*/
    /* 	C = +/-(A/R)												*/
	/*																*/
    /* 	S = +/-(B/R)												*/
	/*																*/
    /* MODULES REQUIRED BY GIVENS -- NONE							*/
	/*																*/
    /* INTRINSIC FUNCTIONS CALLED BY GIVENS - ABS, SQRT				*/
	/*																*/
    /****************************************************************/


    /* LOCAL PARAMETERS -- */

    /* AA, BB = LOCAL COPIES OF A AND B */
    /* R =	  C*A + S*B = +/-SQRT(A*A + B*B) */
    /* U, V =   VARIABLES USED TO SCALE A AND B FOR COMPUTING R */

    aa = *a;
    bb = *b;
    if (dabs(aa) <= dabs(bb))
    {
        goto L1;
    }

    /* ABS(A) .GT. ABS(B) */

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

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

    *b = *s;
    *a = r__;
    return 0;

    /* ABS(A) .LE. ABS(B) */

L1:
    if (bb == 0.f)
    {
        goto L2;