/*
*
* NMEA library
* URL: http://nmea.sourceforge.net
* Author: Tim (xtimor@gmail.com)
* Licence: http://www.gnu.org/licenses/lgpl.html
* \$Id: gmath.h 17 2008-03-11 11:56:11Z xtimor \$
*
*/
#ifndef __NMEA_GMATH_H__
#define __NMEA_GMATH_H__
#include "info.h"
#define NMEA_PI                     (3.141592653589793)             /**< PI value */
#define NMEA_PI180                  (NMEA_PI / 180)                 /**< PI division by 180 */
#define NMEA_EARTHRADIUS_KM         (6378)                          /**< Earth's mean radius in km */   //地球平均半径km

#define NMEA_EARTHRADIUS_M          (NMEA_EARTHRADIUS_KM * 1000)    /**< Earth's mean radius in m */   //地球平均半径m
#define NMEA_EARTH_SEMIMAJORAXIS_M  (6378137.0)                     /**< Earth's semi-major axis in m according WGS84 */ //半长轴 //WGS宽带全球卫星通讯
#define NMEA_EARTH_SEMIMAJORAXIS_KM (NMEA_EARTHMAJORAXIS_KM / 1000) /**< Earth's semi-major axis in km according WGS 84 */
#define NMEA_EARTH_FLATTENING       (1 / 298.257223563)             /**< Earth's flattening according WGS 84 */   //平整度
#define NMEA_DOP_FACTOR             (5)                             /**< Factor for translating DOP to meters */  //DOP ::Dilution of Precision 精度因子
#ifdef  __cplusplus
extern "C" {
#endif
/*
* degree VS radian  //度与弧度的转换
*/
double nmea_degree2radian(double val);
double nmea_radian2degree(double val);
/*
* NDEG (NMEA degree)  //ndeg与度和弧度的转换
*/
double nmea_ndeg2degree(double val);
double nmea_degree2ndeg(double val);
double nmea_ndeg2radian(double val);
double nmea_radian2ndeg(double val);
/*
* DOP
*/
double nmea_calc_pdop(double hdop, double vdop);
double nmea_dop2meters(double dop);
double nmea_meters2dop(double meters);
/*
* positions work  //这个nmeaPOS就是nmeaINFO的一部分。easy
*/
void nmea_info2pos(const nmeaINFO *info, nmeaPOS *pos);
void nmea_pos2info(const nmeaPOS *pos, nmeaINFO *info);
double  nmea_distance(
const nmeaPOS *from_pos,
const nmeaPOS *to_pos
);    //距离
double  nmea_distance_ellipsoid(
const nmeaPOS *from_pos,
const nmeaPOS *to_pos,
double *from_azimuth,  //azimuth 方位
double *to_azimuth
);  //椭圆体距离
int     nmea_move_horz(
const nmeaPOS *start_pos,
nmeaPOS *end_pos,
double azimuth,
double distance
);  //水平移动
int     nmea_move_horz_ellipsoid(
const nmeaPOS *start_pos,
nmeaPOS *end_pos,
double azimuth,
double distance,
double *end_azimuth
);  //水平椭圆移动
#ifdef  __cplusplus
}
#endif
#endif /* __NMEA_GMATH_H__ */

/*
*
* NMEA library
* URL: http://nmea.sourceforge.net
* Author: Tim (xtimor@gmail.com)
* Licence: http://www.gnu.org/licenses/lgpl.html
* \$Id: gmath.c 17 2008-03-11 11:56:11Z xtimor \$
*
*/
/*! \file gmath.h */
#include "nmea/gmath.h"
#include <math.h>
#include <float.h>
/**
* \fn nmea_degree2radian
* \brief Convert degree to radian
*/
double nmea_degree2radian(double val)
{ return (val * NMEA_PI180); }  //高手都习惯使用宏，学习
/**
* \fn nmea_radian2degree
* \brief Convert radian to degree
*/
double nmea_radian2degree(double val)
{ return (val / NMEA_PI180); }
/**
* \brief Convert NDEG (NMEA degree) to fractional degree
*/
double nmea_ndeg2degree(double val)
{
double deg = ((int)(val / 100));
val = deg + (val - deg * 100) / 60;
return val;  //把他的小数部分除以60，只处理小数部分
}
/**
* \brief Convert fractional degree to NDEG (NMEA degree)
*/
double nmea_degree2ndeg(double val)
{
double int_part;
double fra_part;
fra_part = modf(val, &int_part);  //modf将double，分解成整数和小数部分
val = int_part * 100 + fra_part * 60;
return val;
}
/**
* \fn nmea_ndeg2radian
* \brief Convert NDEG (NMEA degree) to radian
*/
double nmea_ndeg2radian(double val)
{ return nmea_degree2radian(nmea_ndeg2degree(val)); }//函数嵌套
/**
* \fn nmea_radian2ndeg
* \brief Convert radian to NDEG (NMEA degree)
*/
double nmea_radian2ndeg(double val)
{ return nmea_degree2ndeg(nmea_radian2degree(val)); }
/**
* \brief Calculate PDOP (Position Dilution Of Precision) factor
*/
double nmea_calc_pdop(double hdop, double vdop)
{
return sqrt(pow(hdop, 2) + pow(vdop, 2));  //pow计算hdop的2次幂  //这个函数就是勾股定理
}
double nmea_dop2meters(double dop)
{ return (dop * NMEA_DOP_FACTOR); }
double nmea_meters2dop(double meters)
{ return (meters / NMEA_DOP_FACTOR); }
/**
* \brief Calculate distance between two points
* \return Distance in meters
*/
double nmea_distance(
const nmeaPOS *from_pos,    /**< From position in radians */
const nmeaPOS *to_pos       /**< To position in radians */
)
{
double dist = ((double)NMEA_EARTHRADIUS_M) * acos(
sin(to_pos->lat) * sin(from_pos->lat) +
cos(to_pos->lat) * cos(from_pos->lat) * cos(to_pos->lon - from_pos->lon)
);
return dist;
}  //数学公式，哇，好久没见过sin，cos，忘了
/**
* \brief Calculate distance between two points
* This function uses an algorithm for an oblate spheroid earth model.
* The algorithm is described here:   //下面函数的算法在下面这个链接可找到
* http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
* \return Distance in meters
*/
double nmea_distance_ellipsoid(
const nmeaPOS *from_pos,    /**< From position in radians */
const nmeaPOS *to_pos,      /**< To position in radians */
double *from_azimuth,       /**< (O) azimuth at "from" position in radians */
double *to_azimuth          /**< (O) azimuth at "to" position in radians */
)
{
/* All variables */
double f, a, b, sqr_a, sqr_b;
double L, phi1, phi2, U1, U2, sin_U1, sin_U2, cos_U1, cos_U2;
double sigma, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, sqr_cos_alpha, lambda, sin_lambda, cos_lambda, delta_lambda;
int remaining_steps;
double sqr_u, A, B, delta_sigma;
/* Check input */
NMEA_ASSERT(from_pos != 0);   //断言！！
NMEA_ASSERT(to_pos != 0);
if ((from_pos->lat == to_pos->lat) && (from_pos->lon == to_pos->lon))
{ /* Identical points */
if ( from_azimuth != 0 )
*from_azimuth = 0;
if ( to_azimuth != 0 )
*to_azimuth = 0;
return 0;
} /* Identical points */
/* Earth geometry */
f = NMEA_EARTH_FLATTENING;
a = NMEA_EARTH_SEMIMAJORAXIS_M;
b = (1 - f) * a;
sqr_a = a * a;
sqr_b = b * b;
/* Calculation */
L = to_pos->lon - from_pos->lon;
phi1 = from_pos->lat;
phi2 = to_pos->lat;
U1 = atan((1 - f) * tan(phi1));
U2 = atan((1 - f) * tan(phi2));
sin_U1 = sin(U1);
sin_U2 = sin(U2);
cos_U1 = cos(U1);
cos_U2 = cos(U2);
/* Initialize iteration */
sigma = 0;
sin_sigma = sin(sigma);
cos_sigma = cos(sigma);
cos_2_sigmam = 0;
sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
sqr_cos_alpha = 0;
lambda = L;
sin_lambda = sin(lambda);
cos_lambda = cos(lambda);
delta_lambda = lambda;
remaining_steps = 20;
while ((delta_lambda > 1e-12) && (remaining_steps > 0))
{ /* Iterate */
/* Variables */
double tmp1, tmp2, tan_sigma, sin_alpha, cos_alpha, C, lambda_prev;
/* Calculation */
tmp1 = cos_U2 * sin_lambda;
tmp2 = cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda;
sin_sigma = sqrt(tmp1 * tmp1 + tmp2 * tmp2);
cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lambda;
tan_sigma = sin_sigma / cos_sigma;
sin_alpha = cos_U1 * cos_U2 * sin_lambda / sin_sigma;
cos_alpha = cos(asin(sin_alpha));
sqr_cos_alpha = cos_alpha * cos_alpha;
cos_2_sigmam = cos_sigma - 2 * sin_U1 * sin_U2 / sqr_cos_alpha;
sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha));
lambda_prev = lambda;
sigma = asin(sin_sigma);
lambda = L +
(1 - C) * f * sin_alpha
* (sigma + C * sin_sigma * (cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam)));
delta_lambda = lambda_prev - lambda;
if ( delta_lambda < 0 ) delta_lambda = -delta_lambda;
sin_lambda = sin(lambda);
cos_lambda = cos(lambda);
remaining_steps--;
}  /* Iterate */
/* More calculation  */
sqr_u = sqr_cos_alpha * (sqr_a - sqr_b) / sqr_b;
A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u)));
B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u)));
delta_sigma = B * sin_sigma * (
cos_2_sigmam + B / 4 * (
cos_sigma * (-1 + 2 * sqr_cos_2_sigmam) -
B / 6 * cos_2_sigmam * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * sqr_cos_2_sigmam)
));
/* Calculate result */
if ( from_azimuth != 0 )
{
double tan_alpha_1 = cos_U2 * sin_lambda / (cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda);
*from_azimuth = atan(tan_alpha_1);
}
if ( to_azimuth != 0 )
{
double tan_alpha_2 = cos_U1 * sin_lambda / (-sin_U1 * cos_U2 + cos_U1 * sin_U2 * cos_lambda);
*to_azimuth = atan(tan_alpha_2);
}
return b * A * (sigma - delta_sigma);
//上述算法过于复杂，有心人可自己看看
/**
* \brief Horizontal move of point position
*/
int nmea_move_horz(
const nmeaPOS *start_pos,   /**< Start position in radians */
nmeaPOS *end_pos,           /**< Result position in radians */
double azimuth,             /**< Azimuth (degree) [0, 359] */
double distance             /**< Distance (km) */
)
{
nmeaPOS p1 = *start_pos;
int RetVal = 1;
distance /= NMEA_EARTHRADIUS_KM; /* Angular distance covered on earth's surface */
azimuth = nmea_degree2radian(azimuth);
end_pos->lat = asin(
sin(p1.lat) * cos(distance) + cos(p1.lat) * sin(distance) * cos(azimuth));
end_pos->lon = p1.lon + atan2(
sin(azimuth) * sin(distance) * cos(p1.lat), cos(distance) - sin(p1.lat) * sin(end_pos->lat));
if(NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon))
{
end_pos->lat = 0; end_pos->lon = 0;
RetVal = 0;
}
return RetVal;
}//都用到了数学方面sin  cos的知识。确实专业一点，这里不与讨论
/**
* \brief Horizontal move of point position
* This function uses an algorithm for an oblate spheroid earth model.
* The algorithm is described here:   //又是一个复杂的算法
* http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
*/
int nmea_move_horz_ellipsoid(
const nmeaPOS *start_pos,   /**< Start position in radians */
nmeaPOS *end_pos,           /**< (O) Result position in radians */
double azimuth,             /**< Azimuth in radians */
double distance,            /**< Distance (km) */
double *end_azimuth         /**< (O) Azimuth at end position in radians */
)
{
/* Variables */
double f, a, b, sqr_a, sqr_b;
double phi1, tan_U1, sin_U1, cos_U1, s, alpha1, sin_alpha1, cos_alpha1;
double tan_sigma1, sigma1, sin_alpha, cos_alpha, sqr_cos_alpha, sqr_u, A, B;
double sigma_initial, sigma, sigma_prev, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, delta_sigma;
int remaining_steps;
double tmp1, phi2, lambda, C, L;

/* Check input */
NMEA_ASSERT(start_pos != 0);
NMEA_ASSERT(end_pos != 0);

if (fabs(distance) < 1e-12)
{ /* No move */
*end_pos = *start_pos;
if ( end_azimuth != 0 ) *end_azimuth = azimuth;
return ! (NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon));
} /* No move */
/* Earth geometry */
f = NMEA_EARTH_FLATTENING;
a = NMEA_EARTH_SEMIMAJORAXIS_M;
b = (1 - f) * a;
sqr_a = a * a;
sqr_b = b * b;

/* Calculation */
phi1 = start_pos->lat;
tan_U1 = (1 - f) * tan(phi1);
cos_U1 = 1 / sqrt(1 + tan_U1 * tan_U1);
sin_U1 = tan_U1 * cos_U1;
s = distance;
alpha1 = azimuth;
sin_alpha1 = sin(alpha1);
cos_alpha1 = cos(alpha1);
tan_sigma1 = tan_U1 / cos_alpha1;
sigma1 = atan2(tan_U1, cos_alpha1);
sin_alpha = cos_U1 * sin_alpha1;
sqr_cos_alpha = 1 - sin_alpha * sin_alpha;
cos_alpha = sqrt(sqr_cos_alpha);
sqr_u = sqr_cos_alpha * (sqr_a - sqr_b) / sqr_b;
A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u)));
B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u)));

/* Initialize iteration */
sigma_initial = s / (b * A);
sigma = sigma_initial;
sin_sigma = sin(sigma);
cos_sigma = cos(sigma);
cos_2_sigmam = cos(2 * sigma1 + sigma);
sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
delta_sigma = 0;
sigma_prev = 2 * NMEA_PI;
remaining_steps = 20;
while ((fabs(sigma - sigma_prev) > 1e-12) && (remaining_steps > 0))
{ /* Iterate */
cos_2_sigmam = cos(2 * sigma1 + sigma);
sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
sin_sigma = sin(sigma);
cos_sigma = cos(sigma);
delta_sigma = B * sin_sigma * (
cos_2_sigmam + B / 4 * (
cos_sigma * (-1 + 2 * sqr_cos_2_sigmam) -
B / 6 * cos_2_sigmam * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * sqr_cos_2_sigmam)
));
sigma_prev = sigma;
sigma = sigma_initial + delta_sigma;
remaining_steps --;
} /* Iterate */

/* Calculate result */
tmp1 = (sin_U1 * sin_sigma - cos_U1 * cos_sigma * cos_alpha1);
phi2 = atan2(
sin_U1 * cos_sigma + cos_U1 * sin_sigma * cos_alpha1,
(1 - f) * sqrt(sin_alpha * sin_alpha + tmp1 * tmp1)
);
lambda = atan2(
sin_sigma * sin_alpha1,
cos_U1 * cos_sigma - sin_U1 * sin_sigma * cos_alpha1
);
C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha));
L = lambda -
(1 - C) * f * sin_alpha * (
sigma + C * sin_sigma *
(cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam))
);

/* Result */
end_pos->lon = start_pos->lon + L;
end_pos->lat = phi2;
if ( end_azimuth != 0 )
{
*end_azimuth = atan2(
sin_alpha, -sin_U1 * sin_sigma + cos_U1 * cos_sigma * cos_alpha1
);
}
return ! (NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon));
}
/**
* \brief Convert position from INFO to radians position
*/
void nmea_info2pos(const nmeaINFO *info, nmeaPOS *pos)
{
pos->lat = nmea_ndeg2radian(info->lat);
pos->lon = nmea_ndeg2radian(info->lon);
}
/**
* \brief Convert radians position to INFOs position
*/
void nmea_pos2info(const nmeaPOS *pos, nmeaINFO *info)
{
info->lat = nmea_radian2ndeg(pos->lat);
info->lon = nmea_radian2ndeg(pos->lon);
}