Add
This commit is contained in:
parent
54c1ad74ac
commit
f9e2024dac
|
|
@ -0,0 +1,76 @@
|
|||
//
|
||||
// Created by villuton on 24.03.25.
|
||||
//
|
||||
|
||||
#ifndef NMEA0183_MATH_H
|
||||
#define NMEA0183_MATH_H
|
||||
|
||||
/*
|
||||
* G math units
|
||||
*/
|
||||
#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 */
|
||||
#define NMEA_EARTHRADIUS_M (NMEA_EARTHRADIUS_KM * 1000) /**< Earth's mean radius in m */
|
||||
#define NMEA_EARTH_SEMIMAJORAXIS_M (6378137.0) /**< Earth's semi-major axis in m according WGS84 */
|
||||
#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 */
|
||||
|
||||
/*
|
||||
* degree VS radian
|
||||
*/
|
||||
double nmeaDegreeToRadian(double val);
|
||||
double nmeaRadianToDegree(double val);
|
||||
|
||||
/*
|
||||
* NDEG (NMEA degree)
|
||||
*/
|
||||
double nmeaNdegToDegree(double val);
|
||||
double nmeaDegreeToNdeg(double val);
|
||||
|
||||
double nmeaNdegToRadian(double val);
|
||||
double nmeaRadianToNdeg(double val);
|
||||
|
||||
/*
|
||||
* DOP
|
||||
*/
|
||||
double nmeaCalcPdop(double hdop, double vdop);
|
||||
double nmeaDopToMeters(double dop);
|
||||
double nmeaMetersToDop(double meters);
|
||||
|
||||
/*
|
||||
* positions work
|
||||
*/
|
||||
double nmeaCalcDistance(
|
||||
const double *from_pos_lat, // /**< From position latitude in radians */
|
||||
const double *from_pos_lon, // /**< From position longitude in radians */
|
||||
const double *to_pos_lat, // /**< To position latitude in radians */
|
||||
const double *to_pos_lon // /**< To position longitude in radians */
|
||||
);
|
||||
double nmeaCalcDistanceEllipsoid(
|
||||
const double *from_pos_lat, // /**< From position latitude in radians */
|
||||
const double *from_pos_lon, // /**< From position longitude in radians */
|
||||
const double *to_pos_lat, // /**< To position latitude in radians */
|
||||
const double *to_pos_lon, // /**< To position longitude in radians */
|
||||
double *from_azimuth, // /**< (O) azimuth at "from" position in radians */
|
||||
double *to_azimuth // /**< (O) azimuth at "to" position in radians */
|
||||
);
|
||||
int nmeaMoveHorz(
|
||||
const double *start_pos_lat, // /**< Start position latitude in radians */
|
||||
const double *start_pos_lon, // /**< Start position longitude in radians */
|
||||
double *end_pos_lat, // /**< Result position latitude in radians */
|
||||
double *end_pos_lon, // /**< Result position longitude in radians */
|
||||
double azimuth, // /**< Azimuth (degree) [0, 359] */
|
||||
double distance // /**< Distance (km) */
|
||||
);
|
||||
int nmeaMoveHorzEllipsoid(
|
||||
const double *start_pos_lat, // /**< Start position latitude in radians */
|
||||
const double *start_pos_lon, // /**< Start position longitude in radians */
|
||||
double *end_pos_lat, // /**< (O) Result position latitude in radians */
|
||||
double *end_pos_lon, // /**< (O) Result position longitude in radians */
|
||||
double azimuth, // /**< Azimuth in radians */
|
||||
double distance, // /**< Distance (km) */
|
||||
double *end_azimuth // /**< (O) Azimuth at end position in radians */
|
||||
);
|
||||
#endif //NMEA0183_MATH_H
|
||||
|
|
@ -0,0 +1,34 @@
|
|||
//
|
||||
// Created by villuton on 26.03.25.
|
||||
//
|
||||
|
||||
#ifndef NMEA0183_SENTENCE_H
|
||||
#define NMEA0183_SENTENCE_H
|
||||
|
||||
/**
|
||||
* Sentence location data in fractional NDEG and Cardinal directions
|
||||
*/
|
||||
typedef struct {
|
||||
double lat; /**< Latitude in NDEG - [degree][min].[sec/60] */
|
||||
char ns; /**< [N]orth or [S]outh */
|
||||
double lon; /**< Longitude in NDEG - [degree][min].[sec/60] */
|
||||
char ew; /**< [E]ast or [W]est */
|
||||
}tNmeaLocation;
|
||||
|
||||
/**
|
||||
* Position data in fractional degrees or radians
|
||||
*/
|
||||
typedef struct {
|
||||
double lat; /**< Latitude */
|
||||
double lon; /**< Longitude */
|
||||
}tNmeaPositionDouble;
|
||||
|
||||
/**
|
||||
* Position data in fractional degrees or radians
|
||||
*/
|
||||
typedef struct {
|
||||
float lat; /**< Latitude */
|
||||
float lon; /**< Longitude */
|
||||
}tNmeaPositionFloat;
|
||||
|
||||
#endif //NMEA0183_SENTENCE_H
|
||||
|
|
@ -0,0 +1,448 @@
|
|||
//
|
||||
// Created by villuton on 24.03.25.
|
||||
//
|
||||
#include "Nmea0183_Math.h"
|
||||
#include "Nmea0183Parser_Private.h"
|
||||
#include <math.h>
|
||||
|
||||
/**
|
||||
* \struct tNmeaMathPos
|
||||
* \brief Position data in fractional degrees or radians
|
||||
*/
|
||||
typedef struct {
|
||||
double lat;
|
||||
double lon;
|
||||
}tNmeaMathPos;
|
||||
|
||||
/**
|
||||
* \fn nmeaDegreeToRadian
|
||||
* \brief Convert degree to radian
|
||||
*/
|
||||
double nmeaDegreeToRadian(double val)
|
||||
{ return (val * NMEA_PI180); }
|
||||
|
||||
/**
|
||||
* \fn nmeaRadianToDegree
|
||||
* \brief Convert radian to degree
|
||||
*/
|
||||
double nmeaRadianToDegree(double val)
|
||||
{ return (val / NMEA_PI180); }
|
||||
|
||||
/**
|
||||
* \fn nmeaNdegToDegree
|
||||
* \brief Convert NDEG (NMEA degree) to fractional degree
|
||||
*/
|
||||
double nmeaNdegToDegree(double val)
|
||||
{
|
||||
double deg = ((int)(val / 100));
|
||||
val = deg + (val - deg * 100) / 60;
|
||||
return val;
|
||||
}
|
||||
|
||||
/**
|
||||
* \fn nmeaDegreeToNdeg
|
||||
* \brief Convert fractional degree to NDEG (NMEA degree)
|
||||
*/
|
||||
double nmeaDegreeToNdeg(double val)
|
||||
{
|
||||
double int_part;
|
||||
double fra_part;
|
||||
fra_part = modf(val, &int_part);
|
||||
val = int_part * 100 + fra_part * 60;
|
||||
return val;
|
||||
}
|
||||
|
||||
/**
|
||||
* \fn nmeaNdegToRadian
|
||||
* \brief Convert NDEG (NMEA degree) to radian
|
||||
*/
|
||||
double nmeaNdegToRadian(double val)
|
||||
{ return nmeaDegreeToRadian(nmeaNdegToDegree(val)); }
|
||||
|
||||
/**
|
||||
* \fn nmeaRadianToNdeg
|
||||
* \brief Convert radian to NDEG (NMEA degree)
|
||||
*/
|
||||
double nmeaRadianToNdeg(double val)
|
||||
{ return nmeaDegreeToNdeg(nmeaRadianToDegree(val)); }
|
||||
|
||||
/**
|
||||
* \fn nmeaCalcPdop
|
||||
* \brief Calculate PDOP (Position Dilution Of Precision) factor
|
||||
*/
|
||||
double nmeaCalcPdop(double hdop, double vdop)
|
||||
{
|
||||
return sqrt(pow(hdop, 2) + pow(vdop, 2));
|
||||
}
|
||||
|
||||
/**
|
||||
* \fn nmeaDopToMeters
|
||||
* \brief Calculate DOP to meters
|
||||
*/
|
||||
double nmeaDopToMeters(double dop)
|
||||
{ return (dop * NMEA_DOP_FACTOR); }
|
||||
|
||||
/**
|
||||
* \fn nmeaMetersToDop
|
||||
* \brief Calculate meters to DOP
|
||||
*/
|
||||
double nmeaMetersToDop(double meters)
|
||||
{ return (meters / NMEA_DOP_FACTOR); }
|
||||
|
||||
/**
|
||||
* \fn nmeaCalcDistance
|
||||
* \brief Calculate distance between two points
|
||||
* @param from_pos_lat < From position latitude in radians
|
||||
* @param from_pos_lon < From position longitude in radians
|
||||
* @param to_pos_lat < To position latitude in radians
|
||||
* @param to_pos_lon < To position longitude in radians
|
||||
* @return Distance in meters
|
||||
*/
|
||||
double nmeaCalcDistance(
|
||||
const double *from_pos_lat, // /**< From position latitude in radians */
|
||||
const double *from_pos_lon, // /**< From position longitude in radians */
|
||||
const double *to_pos_lat, // /**< To position latitude in radians */
|
||||
const double *to_pos_lon // /**< To position longitude in radians */
|
||||
)
|
||||
{
|
||||
tNmeaMathPos from_pos = {
|
||||
.lat = *from_pos_lat,
|
||||
.lon = *from_pos_lon
|
||||
};
|
||||
tNmeaMathPos to_pos = {
|
||||
.lat = *to_pos_lat,
|
||||
.lon = *to_pos_lon
|
||||
};
|
||||
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;
|
||||
}
|
||||
|
||||
/**
|
||||
* \fn nmeaCalcDistanceEllipsoid
|
||||
* \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
|
||||
* @param from_pos_lat < From position latitude in radians
|
||||
* @param from_pos_lon < From position longitude in radians
|
||||
* @param to_pos_lat < To position latitude in radians
|
||||
* @param to_pos_lon < To position longitude in radians
|
||||
* @param from_azimuth < (O) azimuth at "from" position in radians
|
||||
* @param to_azimuth < (O) azimuth at "to" position in radians
|
||||
* @return Distance in meters
|
||||
*/
|
||||
double nmeaCalcDistanceEllipsoid(
|
||||
const double *from_pos_lat, // /**< From position latitude in radians */
|
||||
const double *from_pos_lon, // /**< From position longitude in radians */
|
||||
const double *to_pos_lat, // /**< To position latitude in radians */
|
||||
const double *to_pos_lon, // /**< To position longitude in radians */
|
||||
double *from_azimuth, // /**< (O) azimuth at "from" position in radians */
|
||||
double *to_azimuth // /**< (O) azimuth at "to" position in radians */
|
||||
)
|
||||
{
|
||||
/* All variables */
|
||||
tNmeaMathPos from_pos = {
|
||||
.lat = *from_pos_lat,
|
||||
.lon = *from_pos_lon
|
||||
};
|
||||
tNmeaMathPos to_pos = {
|
||||
.lat = *to_pos_lat,
|
||||
.lon = *to_pos_lon
|
||||
};
|
||||
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_lat != NULL)
|
||||
NMEA_ASSERT(from_pos_lon != NULL)
|
||||
|
||||
NMEA_ASSERT(to_pos_lat != NULL)
|
||||
NMEA_ASSERT(to_pos_lon != NULL)
|
||||
|
||||
|
||||
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, 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;
|
||||
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);
|
||||
}
|
||||
|
||||
/**
|
||||
* \fn nmeaMoveHorz
|
||||
* \brief Horizontal move of point position
|
||||
* @param start_pos_lat < Start position latitude in radians
|
||||
* @param start_pos_lon < Start position longitude in radians
|
||||
* @param end_pos_lat < Result position latitude in radians
|
||||
* @param end_pos_lon < Result position longitude in radians
|
||||
* @param azimuth < Azimuth (degree) [0, 359]
|
||||
* @param distance < Distance (km)
|
||||
* @return
|
||||
*/
|
||||
int nmeaMoveHorz(
|
||||
const double *start_pos_lat, // /**< Start position latitude in radians */
|
||||
const double *start_pos_lon, // /**< Start position longitude in radians */
|
||||
double *end_pos_lat, // /**< Result position latitude in radians */
|
||||
double *end_pos_lon, // /**< Result position longitude in radians */
|
||||
double azimuth, // /**< Azimuth (degree) [0, 359] */
|
||||
double distance // /**< Distance (km) */
|
||||
)
|
||||
{
|
||||
tNmeaMathPos p1 = {
|
||||
.lat = *start_pos_lat,
|
||||
.lon = *start_pos_lon
|
||||
};
|
||||
int RetVal = 1;
|
||||
|
||||
distance /= NMEA_EARTHRADIUS_KM; /* Angular distance covered on earth's surface */
|
||||
azimuth = nmeaDegreeToRadian(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(isnan(*end_pos_lat) || isnan(*end_pos_lon))
|
||||
{
|
||||
*end_pos_lat = 0; *end_pos_lon = 0;
|
||||
RetVal = 0;
|
||||
}
|
||||
|
||||
return RetVal;
|
||||
}
|
||||
|
||||
/**
|
||||
*
|
||||
* \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
|
||||
* @param start_pos_lat < Start position latitude in radians
|
||||
* @param start_pos_lon < Start position longitude in radians
|
||||
* @param end_pos_lat < (O) Result position latitude in radians
|
||||
* @param end_pos_lon < (O) Result position longitude in radians
|
||||
* @param azimuth < Azimuth in radians
|
||||
* @param distance < Distance (km)
|
||||
* @param end_azimuth < (O) Azimuth at end position in radians
|
||||
* @return
|
||||
*/
|
||||
int nmeaMoveHorzEllipsoid(
|
||||
const double *start_pos_lat, // /**< Start position latitude in radians */
|
||||
const double *start_pos_lon, // /**< Start position longitude in radians */
|
||||
double *end_pos_lat, // /**< (O) Result position latitude in radians */
|
||||
double *end_pos_lon, // /**< (O) Result position longitude 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 sigma1, sin_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_lat != NULL)
|
||||
NMEA_ASSERT(start_pos_lon != NULL)
|
||||
NMEA_ASSERT(end_pos_lat != NULL)
|
||||
NMEA_ASSERT(end_pos_lon != NULL)
|
||||
|
||||
if (fabs(distance) < 1e-12)
|
||||
{ /* No move */
|
||||
*end_pos_lat = *start_pos_lat;
|
||||
*end_pos_lon = *start_pos_lon;
|
||||
if ( end_azimuth != 0 ) *end_azimuth = azimuth;
|
||||
return ! (isnan(*end_pos_lat) || 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);
|
||||
sigma1 = atan2(tan_U1, cos_alpha1);
|
||||
sin_alpha = cos_U1 * sin_alpha1;
|
||||
sqr_cos_alpha = 1 - sin_alpha * sin_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;
|
||||
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 ! (isnan(*end_pos_lat) || isnan(*end_pos_lon));
|
||||
}
|
||||
|
||||
/** todo перонести в другой модуль */
|
||||
|
||||
///**
|
||||
// * \brief Convert position from INFO to radians position
|
||||
// */
|
||||
//void nmeaInfoToPos(const nmeaINFO *info, const nmeaPOS *pos)
|
||||
//{
|
||||
// pos->lat = nmeaNdegToRadian(info->lat);
|
||||
// pos->lon = nmeaNdegToRadian(info->lon);
|
||||
//}
|
||||
//
|
||||
///**
|
||||
// * \brief Convert radians position to INFOs position
|
||||
// */
|
||||
//void nmeaPosToInfo(const nmeaPOS *pos, const nmeaINFO *info)
|
||||
//{
|
||||
// info->lat = nmeaRadianToNdeg(pos->lat);
|
||||
// info->lon = nmeaRadianToNdeg(pos->lon);
|
||||
//}
|
||||
Loading…
Reference in New Issue