/* Convert lla to utm (float).
* Note this conversion is not very accurate. If high accuracy needed use lla_of_utm_d.
* @param[out] utm position in m, alt is copied directly from lla
* @param[in] lla position in rad, alt in m
*/
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
{
// compute zone if not initialised
if (utm->zone == 0) {
utm->zone = UtmZoneOfLlaLonRad(lla->lon);
}
float lambda_c = LambdaOfUtmZone(utm->zone);
float ll = isometric_latitude_f(lla->lat , E);
float dl = lla->lon - lambda_c;
float phi_ = asinf(sinf(dl) / coshf(ll));
float ll_ = isometric_latitude_fast_f(phi_);
float lambda_ = atanf(sinhf(ll) / cosf(dl));
struct complex z_ = { lambda_, ll_ };
CScal(serie_coeff_proj_mercator[0], z_);
int8_t k;
for (k = 1; k < 3; k++) {
struct complex z = { lambda_, ll_ };
CScal(2.*k, z);
CSin(z);
CScal(serie_coeff_proj_mercator[k], z);
CAdd(z, z_);
}
CScal(N, z_);
utm->east = DELTA_EAST + z_.im;
utm->north = DELTA_NORTH + z_.re;
// copy alt above reference ellipsoid
utm->alt = lla->alt;
}
/* Convert utm to lla (float).
* Note this conversion is not very accurate. If high accuracy needed use lla_of_utm_d.
* @param[out] lla position in rad, alt is copied directly from utm
* @param[in] utm position in m, alt in m
*/
void lla_of_utm_f(struct LlaCoor_f *lla, struct UtmCoor_f *utm)
{
float scale = 1 / N / serie_coeff_proj_mercator[0];
float real = (utm->north - DELTA_NORTH) * scale;
float img = (utm->east - DELTA_EAST) * scale;
struct complex z = { real, img };
int8_t k;
for (k = 1; k < 2; k++) {
struct complex z_ = { real, img };
CScal(2.*k, z_);
CSin(z_);
CScal(serie_coeff_proj_mercator_inverse[k], z_);
CSub(z_, z);
}
float lambda_c = LambdaOfUtmZone(utm->zone);
lla->lon = lambda_c + atanf(sinhf(z.im) / cosf(z.re));
float phi_ = asinf(sinf(z.re) / coshf(z.im));
float il = isometric_latitude_fast_f(phi_);
lla->lat = inverse_isometric_latitude_f(il, E, 1e-8);
// copy alt above reference ellipsoid
lla->alt = utm->alt;
}
void utm_of_lla_f(struct UtmCoor_f* utm, struct LlaCoor_f* lla) {
float lambda_c = LambdaOfUtmZone(utm->zone);
float ll = isometric_latitude_f(lla->lat , E);
float dl = lla->lon - lambda_c;
float phi_ = asin(sin(dl) / cosh(ll));
float ll_ = isometric_latitude_fast_f(phi_);
float lambda_ = atan(sinh(ll) / cos(dl));
struct complex z_ = { lambda_, ll_ };
CScal(serie_coeff_proj_mercator[0], z_);
uint8_t k;
for(k = 1; k < 3; k++) {
struct complex z = { lambda_, ll_ };
CScal(2*k, z);
CSin(z);
CScal(serie_coeff_proj_mercator[k], z);
CAdd(z, z_);
}
CScal(N, z_);
utm->east = DELTA_EAST + z_.im;
utm->north = DELTA_NORTH + z_.re;
}
void latlong_utm_of(float phi, float lambda, uint8_t utm_zone) {
float lambda_c = LambdaOfUtmZone(utm_zone);
float ll = isometric_latitude(phi , E);
float dl = lambda - lambda_c;
float phi_ = asin(sin(dl) / cosh(ll));
float ll_ = isometric_latitude0(phi_);
float lambda_ = atan(sinh(ll) / cos(dl));
struct complex z_ = { lambda_, ll_ };
CScal(serie_coeff_proj_mercator[0], z_);
uint8_t k;
for(k = 1; k < 3; k++) {
struct complex z = { lambda_, ll_ };
CScal(2*k, z);
CSin(z);
CScal(serie_coeff_proj_mercator[k], z);
CAdd(z, z_);
}
CScal(N, z_);
latlong_utm_x = XS + z_.im;
latlong_utm_y = z_.re;
}
