/* 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;
}
/* find single root */
void CLPCAnal::Laguerre(COMPLEX *ap,int m,COMPLEX *r)
{
COMPLEX rlast;
int j,iter;
double err,abx;
COMPLEX sq,h,gp,gm,g2,g,bp,d,dx,f;
iter = 0;
do {
rlast = *r;
bp = ap[m];
err = CMag(bp);
f = CMake(0.0,0.0);
d = f;
abx = CMag(*r);
/* compute value of polynomial & derivatives */
for (j=m-1;j>=0;j--) {
f = CAdd(CMult(*r,f),d);
d = CAdd(CMult(*r,d),bp);
bp = CAdd(CMult(*r,bp),ap[j]);
err = CMag(bp)+abx*err;
}
/* if polynomial = zero then already at root */
err = err * ROUND_ERROR;
if (CMag(bp) > err) {
/* no, iterate using Laguerre's formula */
g = CDiv(d,bp);
g2 = CMult(g,g);
h = CSub(g2,CScale(CDiv(f,bp),2.0));
sq = CSqrt(CScale(CSub(CScale(h,m*1.0),g2),m-1.0));
gp = CAdd(g,sq);
gm = CSub(g,sq);
if (CMag(gp) < CMag(gm)) gp = gm;
dx = CDiv(CMake(m*1.0,0.0),gp);
*r = CSub(*r,dx);
}
iter++;
} while (!((iter==100) || (CMag(bp)<=err) || ((r->re == rlast.re) && (r->im == rlast.im))));
/* terminating condition for iteration */
}
void FftComplex (Cmplx *a, int size)
{
Cmplx t, w, wo;
real theta;
int i, j, k, n;
k = 0;
for (i = 0; i < size; i ++) {
if (i < k) {
t = a[i];
a[i] = a[k];
a[k] = t;
}
n = size / 2;
while (n >= 1 && k >= n) {
k -= n;
n /= 2;
}
k += n;
}
for (n = 1; n < size; n *= 2) {
theta = M_PI / n;
CSet (wo, cos (theta) - 1., sin (theta));
CSet (w, 1., 0.);
for (k = 0; k < n; k ++) {
for (i = k; i < size; i += 2 * n) {
j = i + n;
CMul (t, w, a[j]);
CSub (a[j], a[i], t);
CAdd (a[i], a[i], t);
}
CMul (t, w, wo);
CAdd (w, w, t);
}
}
}
