mathsubs.c

GPS导航定位程序

#include "includes.h"

/****************************************************************************    
* Function: long SignExtend(unsigned long Argumnet, int BitNumber)
*
* Sign extend a 32-bit integer beginning at a specified bit number.    
* This routine is used to promote a two's complement number to a 32-bit
* long integer, where the original number occupies less than 32 bits. 
*
* Input: Argument - 32-bit unsigned integer.
*        BitNumber - the start bit number (0-31).
*
* Output: None.
*
* Return Value: The signed extended number.
****************************************************************************/
long SignExtend(unsigned long Argumnet, int BitNumber)
{
    long result;                    /* The resultant sign extended number. */

    if(BitNumber>=31)                               /* No change required. */
	result = Argumnet;
    else
    {
	result = Argumnet<<(31-BitNumber);  /* Get bitnum in the sign bit. */
	result = result>>(31-BitNumber);                /* Then do an ASR. */
    }
    return(result);
}

/****************************************************************************    
* Function: void GreatCircle(double PointA_latitude, double PointA_longitude,
*                            double PointB_latitude, double PointB_longitude,
*                            double *GreatCircleDistance,
*                            double *GreatCircleBearing)
*
* Compute great circle range (radians) and bearing(radians E of N) between 2
* points, A and B. The algorithm is given in "Map Projections Used by the
* U. S. Geological Survey", John P. Snyder, 1982 (Geological Survey Bulletin
* 1532).  Much more sophisticated (and time-consuming) algorithms exist for
* high accuracy over long baselines, see for example, "Spheroidal Geodesics,
* Reference Systems, & Local Geometry", U. S. Naval Ocean-ographic Office,
* publication SP-138.
*
* Input: PointA_Latitude - Point A WGS-84 latitude, radians.
*        PointA_Longitude - Point A WGS-84 longitude, radians.
*        PointB_Latitude - Point B WGS-84 latitude, radians.
*        PointB_Longitude - Point B WGS-84 longitude, radians.
*
* Output: GreatCircleDistance - Distance between A and B, radians.
*         GreatCircleBearing - Bearing from A to B, radians east of north.
*
* Return Value: None.
****************************************************************************/
void GreatCircle(double PointA_Latitude, double PointA_Longitude,
		 double PointB_Latitude, double PointB_Longitude,
		 double *GreatCircleDistance, double *GreatCircleBearing)
{
    double Cos_PointA_Latitude;             /* Cosine of point A latitude. */
    double Cos_PointB_Latitude;             /* Cosine of point B latitude. */
    double Sin_PointA_Latitude;               /* Sine of point A latitude. */
    double Sin_PointB_Latitude;               /* Sine of point B latitude. */
    double Delta_Longitude;        /* Longitude differance B - A, radians. */
    double Cos_Delta_Longitude;              /* Cosine of delta longitude. */
    double Sin_Delta_Longitude;                /* Sine of delta longitude. */
    double Sin_Distance;                 /* Sine of great circle distance. */
    double Cos_Distance;               /* Cosine of great circle distance. */
    double Sin_Bearing;                   /* Sine of great circle bearing. */
    double Cos_Bearing;                 /* Cosine of great circle bearing. */
    double intermeadiate;                 /* An intermeadiate calculation. */

    /* Calculate some sines and cosines now to save time later. */
    
    Cos_PointA_Latitude = cos(PointA_Latitude);
    Cos_PointB_Latitude = cos(PointB_Latitude);
    Sin_PointA_Latitude = sin(PointA_Latitude);
    Sin_PointB_Latitude = sin(PointB_Latitude);
    Delta_Longitude = PointB_Longitude - PointA_Longitude;
    Cos_Delta_Longitude = cos(Delta_Longitude);
    Sin_Delta_Longitude = sin(Delta_Longitude);

    /* Get the cosine of the great circle distance. */

    Cos_Distance = Sin_PointA_Latitude*Sin_PointB_Latitude
	       + Cos_PointA_Latitude*Cos_PointB_Latitude*Cos_Delta_Longitude;

    /* Get the sine of the great circle distance. */

    intermeadiate = DSquare(Cos_PointB_Latitude*Sin_Delta_Longitude)
    + DSquare(Cos_PointA_Latitude*Sin_PointB_Latitude
	      - Sin_PointA_Latitude*Cos_PointB_Latitude*Cos_Delta_Longitude);

    Sin_Distance = sqrt(fabs(intermeadiate));

    /* Get the the great circle distance. */

    if(fabs(Sin_Distance)>.0000001 || fabs(Cos_Distance)>.0000001)
	*GreatCircleDistance = atan2(Sin_Distance,Cos_Distance);
    else
	*GreatCircleDistance = 0.0;                         /* Same point. */

    /* Get the sine of the great circle bearing. */

    Sin_Bearing = Sin_Delta_Longitude*Cos_PointB_Latitude;

    /* Get the cosine of the great circle bearing. */

    Cos_Bearing = Cos_PointA_Latitude*Sin_PointB_Latitude
	       - Sin_PointA_Latitude*Cos_PointB_Latitude*Cos_Delta_Longitude;

    /* Get the great circle bearing. */

    if(fabs(Sin_Bearing)>.0000001 || fabs(Cos_Bearing)>.0000001)
    {
	intermeadiate = atan2(Sin_Bearing,Cos_Bearing);
	while(intermeadiate<0.0)
	    intermeadiate += 2.0*PI;
	*GreatCircleBearing = intermeadiate;
    }
    else
	*GreatCircleBearing = 0.0;            /* Due north or co-incident. */
}

/****************************************************************************
 * The following array contains atan(x) in radians scaled by 2^13.
 * Entries 0 to 128 correspond to resultant angles 0 to pi/2 in steps of
 * 1/128*pi.
 ***************************************************************************/

static int atan2array[129] = 
{0,64,128,192,256,320,384,448,511,575,639,702,766,829,892,956,1019,1082,1144,
 1207,1270,1332,1394,1456,1518,1580,1642,1703,1764,1825,1886,1947,2007,2067,
 2127,2187,2246,2305,2364,2423,2481,2539,2597,2655,2712,2769,2826,2883,2939,
 2995,3051,3106,3161,3216,3270,3325,3378,3432,3485,3538,3591,3643,3695,3747,
 3798,3849,3900,3950,4000,4050,4100,4149,4197,4246,4294,4342,4389,4437,4483,
 4530,4576,4622,4667,4713,4758,4802,4846,4890,4934,4977,5020,5063,5105,5147,
 5189,5230,5272,5312,5353,5393,5433,5473,5512,5551,5590,5628,5666,5704,5741,
 5779,5816,5852,5889,5925,5961,5996,6031,6066,6101,6136,6170,6204,6237,6271,
 6304,6337,6369,6402,6434};

/****************************************************************************    
* Function: long Atan2Approx(long y, long x)
*
* Returns atan2(y/x) in the range +-pi scaled by 2^13.
*
* Input: y - input argument (opposite).
*        x - input argument (adjacent).
*
* Output: None.
*
* Return Value: The arctangent, radians, scaled by 2^13.
****************************************************************************/
long Atan2Approx(long y, long x)
{
    long angle;                                         /* The arctangent. */
    long index;                        /* Index into the arctangent array. */
    long swap;    /* Used to swap the arguments according to the quadrant. */

    int quadrant;                     /* The quadrant of the result (1-4). */
    int sign;                                   /* The sign of the result. */

    logical greaterthanpiby4;       /* Indicates result greater than pi/4. */

    /* First check for the invalid case of x and y equal to 0. */

    if(x==0 && y==0)
	return(0);

    sign = 1;                                                      /* +ve. */
    if(y<0)                           /* Check if the result is -ve., y<0. */
    {
	sign = -1;                                                 /* -ve. */
	y = -y;                        /* Put y in the 1st or 4th quadrant */
    }

    /* Check if the result will be in the 1st or 4th quadrant. */

    quadrant = 1;
    if(x<0)
    {
	quadrant = 4;
	x = -x;
    }

    /* Check if the result will be greater than pi/4. */

    greaterthanpiby4 = FALSE;
    if(y>x)                    
    {
	greaterthanpiby4 = TRUE;
	swap = x;
	x = y;
	y = swap;
    }

    index = (y*128L)/x;              /* Get the index into the atan array. */
    angle = (long)atan2array[index];

    if(greaterthanpiby4)          /* Put the result in the right quadrant. */
	angle = 12868L - angle;

    if(quadrant==4)
	angle = 25735L - angle;

    return((long)sign*angle);
}

/****************************************************************************    
* Function: long Atan2Approx(long y, long x)
*
* Returns atan(y/x) in the range +-pi/2 scaled by 2^13.
*
* Input: y - input argument (opposite).
*        x - input argument (adjacent).
*
* Output: None.
*
* Return Value: The arctangent, radians, scaled by 2^13.
****************************************************************************/
long AtanApprox(long y, long x)
{
    if(x<0)                   /* Map x and y to the 1st and 2nd quadrants. */
    {
	x = -x;
	y = -y;
    }

    return(Atan2Approx(y,x));
}

/****************************************************************************
 * The following array contains sin(x) scaled by (2^15 - 1).