int main(int argc, char** argv)
{
// allocate data
size_t uTimes = 1000;
size_t uSize = 1000000;
double *pD1 = new double[uSize],
*pD2 = new double[uSize];
// generate test data
for (size_t i = 0; i < uSize; ++i)
{
pD1[i] = i;
pD2[i] = -1.0 * i;
}
RunTest(uTimes, [&]()
{
return Templeat_Func(uSize, pD1, pD2, [](double d1, double d2){ return d1 + d2; });
}, "template + lambda");
RunTest(uTimes, [&]()
{
return Templeat_Func(uSize, pD1, pD2, CAdd());
}, "template + class");
RunTest(uTimes, [&]()
{
return Templeat_Func(uSize, pD1, pD2, fadd);
}, "template + function");
std::cout << std::endl;
RunTest(uTimes, [&]()
{
return STL_Func(uSize, pD1, pD2, [](double d1, double d2){ return d1 + d2; });
}, "std::function + lambda");
RunTest(uTimes, [&]()
{
return STL_Func(uSize, pD1, pD2, CAdd());
}, "std::function + class");
RunTest(uTimes, [&]()
{
return STL_Func(uSize, pD1, pD2, fadd);
}, "std::function + func");
std::cout << std::endl;
RunTest(uTimes, [&]()
{
return Pointer_Func(uSize, pD1, pD2, [](double d1, double d2){ return d1 + d2; });
}, "function ptr + lambda");
RunTest(uTimes, [&]()
{
return Pointer_Func(uSize, pD1, pD2, fadd);
}, "function ptr + func");
}
double CLPCAnal::Response(float *coeff, int n, double f)
{
COMPLEX omega[MAXORDER+1];
int i;
COMPLEX rnum,rden;
/* initialise polynomial values of complex frequency */
omega[0] = CMake(1.0,0.0);
omega[1] = CExp(CMake(0.0,2*M_PI*f));
for (i=2;i<=n;i++)
omega[i] = CMult(omega[i-1],omega[1]);
/* compute response of numerator */
rnum=omega[0];
/* compute response of denominator */
rden=omega[0];
for (i=1;i<=n;i++)
rden = CAdd(rden,CScale(omega[i],coeff[i]));
/* compute ratio */
if (CMag(rden)==0)
return(1.0E10); /* i.e. infinity */
else
return(CMag(CDiv(rnum,rden)));
}
/* 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;
}
/* 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 */
}
SEXP F21DaR(SEXP A, SEXP B, SEXP C, SEXP Z, SEXP Minit, SEXP Maxit) {
int n = LENGTH(Z);
double maxit = REAL(Maxit)[0];
double minit = REAL(Minit)[0];
double f, maxsum;
double a = REAL(A)[0];
Rcomplex b = COMPLEX(AS_COMPLEX(B))[0];
Rcomplex c = COMPLEX(AS_COMPLEX(C))[0];
Rcomplex *z = COMPLEX(Z);
double curra;
Rcomplex currc,currb,currsum,tres;
SEXP LRes, LNames, Res, Rel;
PROTECT (LRes = allocVector(VECSXP, 2));
PROTECT (LNames = allocVector(STRSXP, 2));
PROTECT (Res = allocVector(CPLXSXP, n));
PROTECT (Rel = allocVector(REALSXP, n));
Rcomplex *res = COMPLEX(Res);
double *rel = REAL(Rel);
for (int i=0; i<n; i++) {
curra = a; currb = b; currc = c; currsum.r = 1.; currsum.i = 0.;
tres = currsum; maxsum = 1.;
for (f = 1.; (f<minit)||((f<maxit)&&(StopCritD(currsum,tres)>DOUBLE_EPS)); f=f+1.) {
R_CheckUserInterrupt();
currsum = CMultR(currsum,curra);
currsum = CMult(currsum,currb);
currsum = CDiv(currsum,currc);
currsum = CMult(currsum,z[i]);
currsum = CDivR(currsum,f);
tres = CAdd(tres,currsum);
curra = curra+1.;
currb = CAdd1(currb);
currc = CAdd1(currc);
// Rprintf("%f: %g + %g i\n",f,currsum.r,currsum.i);
maxsum = fmax2(maxsum,Cabs2(currsum));
}
if (f>=maxit) {
// Rprintf("D:Appr: %f - Z: %f + %f i, Currsum; %f + %f i, Rel: %g\n",f,z[i].r,z[i].i,currsum.r,currsum.i,StopCritD(currsum,tres));
warning("approximation of hypergeometric function inexact");
}
res[i] = tres;
rel[i] = sqrt(Cabs2(res[i])/maxsum);
// Rprintf("Iterations: %f, Result: %g+%g i\n",f,res[i].r,res[i].i);
}
SET_VECTOR_ELT(LRes, 0, Res);
SET_STRING_ELT(LNames, 0, mkChar("value"));
SET_VECTOR_ELT(LRes, 1, Rel);
SET_STRING_ELT(LNames, 1, mkChar("rel"));
setAttrib(LRes, R_NamesSymbol, LNames);
UNPROTECT(4);
return(LRes);
}
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);
}
}
}
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;
}
/* find all roots */
void CLPCAnal::AllRoots(COMPLEX *ap,int m,COMPLEX *roots)
{
int k,j,i;
COMPLEX x,bp,c;
COMPLEX ad[MAXPOLY];
for (j=0;j<=m;j++) ad[j] = ap[j];
for (j=m;j>=1;j--) {
/* find root */
x = CMake(0.0,0.0);
Laguerre(ad,j,&x);
if (fabs(x.im) <= (IM_RANGE*fabs(x.re)))
x.im = 0.0;
roots[j] = x;
/* deflation */
bp = ad[j];
for (k=j-1;k>=0;k--) {
c = ad[k];
ad[k] = bp;
bp = CAdd(CMult(x,bp),c);
}
}
/* sort into increasing root.real */
for (j=2;j<=m;j++) {
x = roots[j];
i = j;
while ((i > 1) && (x.re < roots[i-1].re)) {
roots[i] = roots[i-1];
i = i - 1;
}
roots[i] = x;
}
}
