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)));
}
/* 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);
}
int Hqr2 (int n, int low, int high, double **h, double *wr, double *wi, double **z)
{
int i, j, k, l, m, na, en, notlas, mp2, itn, its, enm2, twoRoots;
double norm, p, q, r, s, t, w, x, y, ra, sa, vi, vr, zz, tst1, tst2;
/* store roots isolated by Balanc and compute matrix norm */
norm = 0.0;
k = 0;
for (i = 0; i < n; i++)
{
for (j = k; j < n; j++)
norm += fabs(h[i][j]);
k = i;
if ((i < low) || (i > high))
{
wr[i] = h[i][i];
wi[i] = 0.0;
}
}
en = high;
t = 0.0;
itn = n * 30;
/* search for next eigenvalues */
while (en >= low)
{
its = 0;
na = en - 1;
enm2 = na - 1;
twoRoots = FALSE;
/* look for single small sub-diagonal element */
for (;;)
{
for (l = en; l > low; l--)
{
s = fabs(h[l-1][l-1]) + fabs(h[l][l]);
if (s == 0.0)
s = norm;
tst1 = s;
tst2 = tst1 + fabs(h[l][l-1]);
if (tst2 == tst1)
break;
}
/* form shift */
x = h[en][en];
if (l == en)
break;
y = h[na][na];
w = h[en][na] * h[na][en];
if (l == na)
{
twoRoots = TRUE;
break;
}
if (itn == 0)
{
/* set error -- all eigenvalues have not converged after 30*n iterations */
return en;
}
if ((its == 10) || (its == 20))
{
/* form exceptional shift */
t += x;
for (i = low; i <= en; i++)
h[i][i] -= x;
s = fabs(h[en][na]) + fabs(h[na][enm2]);
x = s * (double) 0.75;
y = x;
w = s * (double)-0.4375 * s;
}
its++;
--itn;
/* look for two consecutive small sub-diagonal elements */
for (m = enm2; m >= l; m--)
{
zz = h[m][m];
r = x - zz;
s = y - zz;
p = (r * s - w) / h[m+1][m] + h[m][m+1];
q = h[m+1][m+1] - zz - r - s;
r = h[m+2][m+1];
s = fabs(p) + fabs(q) + fabs(r);
p /= s;
q /= s;
r /= s;
if (m == l)
break;
tst1 = fabs(p) * (fabs(h[m-1][m-1]) + fabs(zz) + fabs(h[m+1][m+1]));
tst2 = tst1 + fabs(h[m][m-1]) * (fabs(q) + fabs(r));
if (tst2 == tst1)
break;
}
mp2 = m + 2;
for (i = mp2; i <= en; i++)
{
h[i][i-2] = 0.0;
if (i != mp2)
h[i][i-3] = 0.0;
}
/* double qr step involving rows l to en and columns m to en */
for (k = m; k <= na; k++)
{
notlas = (k != na);
if (k != m)
{
p = h[k][k-1];
q = h[k+1][k-1];
r = 0.0;
if (notlas)
r = h[k+2][k-1];
x = fabs(p) + fabs(q) + fabs(r);
if (x == 0.0)
continue;
p /= x;
q /= x;
r /= x;
}
s = D_sign(sqrt(p*p + q*q + r*r), p);
if (k != m)
h[k][k-1] = -s * x;
else if (l != m)
h[k][k-1] = -h[k][k-1];
p += s;
x = p / s;
y = q / s;
zz = r / s;
q /= p;
r /= p;
if (!notlas)
{
/* row modification */
for (j = k; j < n; j++)
{
p = h[k][j] + q * h[k+1][j];
h[k][j] -= p * x;
h[k+1][j] -= p * y;
}
j = MIN(en, k + 3);
/* column modification */
for (i = 0; i <= j; i++)
{
p = x * h[i][k] + y * h[i][k+1];
h[i][k] -= p;
h[i][k+1] -= p * q;
}
/* accumulate transformations */
for (i = low; i <= high; i++)
{
p = x * z[i][k] + y * z[i][k+1];
z[i][k] -= p;
z[i][k+1] -= p * q;
}
}
else
{
/* row modification */
for (j = k; j < n; j++)
{
p = h[k][j] + q * h[k+1][j] + r * h[k+2][j];
h[k][j] -= p * x;
h[k+1][j] -= p * y;
h[k+2][j] -= p * zz;
}
j = MIN(en, k + 3);
/* column modification */
for (i = 0; i <= j; i++)
{
p = x * h[i][k] + y * h[i][k+1] + zz * h[i][k+2];
h[i][k] -= p;
h[i][k+1] -= p * q;
h[i][k+2] -= p * r;
}
/* accumulate transformations */
for (i = low; i <= high; i++)
{
p = x * z[i][k] + y * z[i][k+1] + zz * z[i][k+2];
z[i][k] -= p;
z[i][k+1] -= p * q;
z[i][k+2] -= p * r;
}
}
}
}
if (twoRoots)
{
/* two roots found */
p = (y - x) / (double) 2.0;
q = p * p + w;
zz = sqrt(fabs(q));
h[en][en] = x + t;
x = h[en][en];
h[na][na] = y + t;
/* DLS 28aug96: Changed "0.0" to "-1e-12" below. Roundoff errors can cause this value
to dip ever-so-slightly below zero even when eigenvalue is not complex.
*/
if (q >= -1e-12)
{
/* real pair */
zz = p + D_sign(zz, p);
wr[na] = x + zz;
wr[en] = wr[na];
if (zz != 0.0)
wr[en] = x - w/zz;
wi[na] = 0.0;
wi[en] = 0.0;
x = h[en][na];
s = fabs(x) + fabs(zz);
p = x / s;
q = zz / s;
r = sqrt(p*p + q*q);
p /= r;
q /= r;
/* row modification */
for (j = na; j < n; j++)
{
zz = h[na][j];
h[na][j] = q * zz + p * h[en][j];
h[en][j] = q * h[en][j] - p * zz;
}
/* column modification */
for (i = 0; i <= en; i++)
{
zz = h[i][na];
h[i][na] = q * zz + p * h[i][en];
h[i][en] = q * h[i][en] - p * zz;
}
/* accumulate transformations */
for (i = low; i <= high; i++)
{
zz = z[i][na];
z[i][na] = q * zz + p * z[i][en];
z[i][en] = q * z[i][en] - p * zz;
}
}
else
{
/* complex pair */
wr[na] = x + p;
wr[en] = x + p;
wi[na] = zz;
wi[en] = -zz;
}
en = enm2;
}
else
{
/* one root found */
h[en][en] = x + t;
wr[en] = h[en][en];
wi[en] = 0.0;
en = na;
}
}
/* All roots found. Backsubstitute to find vectors of upper triangular form */
if (norm == 0.0)
return 0;
for (en = n - 1; en >= 0; en--)
{
p = wr[en];
q = wi[en];
na = en - 1;
/* DLS 28aug96: Changed "0.0" to -1e-12 below (see comment above) */
if (q < -1e-12)
{
/* complex vector */
m = na;
/* last vector component chosen imaginary so that eigenvector matrix is triangular */
if (fabs(h[en][na]) > fabs(h[na][en]))
{
h[na][na] = q / h[en][na];
h[na][en] = -(h[en][en] - p) / h[en][na];
}
else
CDiv(0.0, -h[na][en], h[na][na] - p, q, &h[na][na], &h[na][en]);
h[en][na] = 0.0;
h[en][en] = 1.0;
enm2 = na - 1;
if (enm2 >= 0)
{
for (i = enm2; i >= 0; i--)
{
w = h[i][i] - p;
ra = 0.0;
sa = 0.0;
for (j = m; j <= en; j++)
{
ra += h[i][j] * h[j][na];
sa += h[i][j] * h[j][en];
}
if (wi[i] < 0.0)
{
zz = w;
r = ra;
s = sa;
}
else
{
m = i;
if (wi[i] == 0.0)
CDiv(-ra, -sa, w, q, &h[i][na], &h[i][en]);
else
{
/* solve complex equations */
x = h[i][i+1];
y = h[i+1][i];
vr = (wr[i] - p) * (wr[i] - p) + wi[i] * wi[i] - q * q;
vi = (wr[i] - p) * (double)2.0 * q;
if ((vr == 0.0) && (vi == 0.0))
{
tst1 = norm * (fabs(w) + fabs(q) + fabs(x) + fabs(y) + fabs(zz));
vr = tst1;
do {
vr *= (double) 0.01;
tst2 = tst1 + vr;
}
while (tst2 > tst1);
}
CDiv(x * r - zz * ra + q * sa, x * s - zz * sa - q * ra, vr, vi, &h[i][na], &h[i][en]);
if (fabs(x) > fabs(zz) + fabs(q))
{
h[i+1][na] = (-ra - w * h[i][na] + q * h[i][en]) / x;
h[i+1][en] = (-sa - w * h[i][en] - q * h[i][na]) / x;
}
else
CDiv(-r - y * h[i][na], -s - y * h[i][en], zz, q, &h[i+1][na], &h[i+1][en]);
}
/* overflow control */
tst1 = fabs(h[i][na]);
tst2 = fabs(h[i][en]);
t = MAX(tst1, tst2);
if (t != 0.0)
{
tst1 = t;
tst2 = tst1 + ONE_POINT_ZERO / tst1;
if (tst2 <= tst1)
{
for (j = i; j <= en; j++)
{
h[j][na] /= t;
h[j][en] /= t;
}
}
}
}
}
}
/* end complex vector */
}
else if (q == 0.0)
{
/* real vector */
m = en;
h[en][en] = 1.0;
if (na >= 0)
{
for (i = na; i >= 0; i--)
{
w = h[i][i] - p;
r = 0.0;
for (j = m; j <= en; j++)
r += h[i][j] * h[j][en];
if (wi[i] < 0.0)
{
zz = w;
s = r;
continue;
}
else
{
m = i;
if (wi[i] == 0.0)
{
t = w;
if (t == 0.0)
{
tst1 = norm;
t = tst1;
do {
t *= (double) 0.01;
tst2 = norm + t;
}
while (tst2 > tst1);
}
h[i][en] = -r / t;
}
else
{
/* solve real equations */
x = h[i][i+1];
y = h[i+1][i];
q = (wr[i] - p) * (wr[i] - p) + wi[i] * wi[i];
t = (x * s - zz * r) / q;
h[i][en] = t;
if (fabs(x) > fabs(zz))
h[i+1][en] = (-r - w * t) / x;
else
h[i+1][en] = (-s - y * t) / zz;
}
/* overflow control */
t = fabs(h[i][en]);
if (t != 0.0)
{
tst1 = t;
tst2 = tst1 + ONE_POINT_ZERO / tst1;
if (tst2 <= tst1)
{
for (j = i; j <= en; j++)
h[j][en] /= t;
}
}
}
}
}
/* end real vector */
}
}
/* end back substitution */
/* vectors of isolated roots */
for (i = 0; i < n; i++)
{
if ((i < low) || (i > high))
{
for (j = i; j < n; j++)
z[i][j] = h[i][j];
}
}
/* multiply by transformation matrix to give vectors of original full matrix */
for (j = n - 1; j >= low; j--)
{
m = MIN(j, high);
for (i = low; i <= high; i++)
{
zz = 0.0;
for (k = low; k <= m; k++)
zz += z[i][k] * h[k][j];
z[i][j] = zz;
}
}
return 0;
}
