laguer.cpp

Visual C++ 常用数值算法集 源代码

void value(double x1[3],double dx[3],double gm[3],double gp[3],
		   double sq[3],double g2[3],double h[3],double f[3],
		   double d[3],double b[3],double zero[3])
{
	int i;
	for(i=1;i<=3;i++)
	{
	 x1[i]=0.0;
	 dx[i]=0.0;
	 gm[i]=0.0;
 	 gp[i]=0.0;
	 sq[i]=0.0;
	 g2[i]=0.0;
	 h[i]=0.0;
	 f[i]=0.0;
	 d[i]=0.0;
	 b[i]=0.0;
	 zero[i]=0.0;
	}
}

double cabs(double a1,double a2)
{
	double x,y,t;
    x = fabs(a1);
    y = fabs(a2);
    if (x == 0)
        t = y;
    else if (y == 0)
        t = x;
    else if (x > y)
        t = x * sqrt(1 + sqrt(y / x));
    else
        t = y * sqrt(1 + sqrt(x / y));
    return t;
}

double cdiv1(double a1,double a2 ,double b1,double b2)
{
	double r,den,t;
    if( fabs(b1) >= fabs(b2))
	{
        r = b2 / b1;
        den = b1 + r * b2;
        t = (a1 + a2 * r) / den;
	}
    else
	{
        r = b1 / b2;
        den = b2 + r * b1;
        t = (a1 * r + a2) / den;
    }
	return t;
}

double cdiv2(double a1,double a2,double b1,double b2)
{
	double r,den,t;
    if (fabs(b1) >= fabs(b2))
	{
        r = b2 / b1;
        den = b1 + r * b2;
        t = (a2 - a1 * r) / den;
	}
    else
	{
        r = b1 / b2;
        den = b2 + r * b1;
        t = (a2 * r - a1) / den;
	}
	return t;
}

double csqr1(double x,double y)
{
	double tt,u,r,w,v;

    if ((x == 0) &&( y == 0))
        u = 0.0;
    else
	{
        if (fabs(x) >= fabs(y))
            w = sqrt(fabs(x)) * sqrt(0.5 * (1 + sqrt(1 + sqrt(fabs(y / x)))));
        else
		{
            r = fabs(x / y);
            w = sqrt(fabs(y)) * sqrt(0.5 * (r + sqrt(1 + sqrt(r))));
		}
        
        if (x >= 0)
		{
            u = w;
            v = y / (2 * u);
		}
        else
		{
            if( y >= 0)
                v = w;
            else
                v = -w;
        u = y / (2 * v);
        }
    }
    tt = u;
	return tt;
}

double csqr2(double x,double y)
{
	double ttt,u,r,w,v;
    if ((x = 0) && (y = 0))
        v = 0;
    else
	{
        if (fabs(x) >= fabs(y))
            w = sqrt(fabs(x)) * sqrt(0.5 * (1 + sqrt(1 + sqrt(fabs(y / x)))));
        else
		{
            r = fabs(x / y);
            w = sqrt(fabs(y)) * sqrt(0.5 * (r + sqrt(1 + sqrt(r))));
        }
        if (x >= 0)
		{
            u = w;
            v = y / (2 * u);
		}
        else
		{
            if (y >= 0 )
				v = w;
			else
				v = -w;
        u = y / (2 * v);
        }
    }
    ttt = v;
	return ttt;
}

void laguer(double a[3][6],int m,double x[3],double eps,int polish)
{
	int maxit,iter,j;
	double epss,dxold,erq,abx,dum,dum1,dum2,cdx,tttt;
    double  zero[3],b[3],d[3],f[3],g[3],h[3];
    double  g2[3],sq[3],gp[3],gm[3],dx[3],x1[3];
    zero[1] = 0.0;
    zero[2] = 0.0;
    epss = 0.000000006;
    maxit = 100;
    dxold = cabs(x[1],x[2]);
    for (iter = 1; iter<=maxit; iter)
	{
        b[1] = a[1][m + 1];
        b[2] = a[2][m + 1];
        erq = cabs(b[1],x[2]);
        d[1] = zero[1];
        d[2] = zero[2];
        f[1] = zero[1];
        f[2] = zero[2];
        abx = cabs(x[1],x[2]);
        for (j = m; j>=1; j--)
		{
            dum = x[1] * f[1] - x[2] * f[2] + d[1];
            f[2] = x[2] * f[1] + x[1] * f[2] + d[2];
            f[1] = dum;
            dum = x[1] * d[1] - x[2] * d[2] + b[1];
            d[2] = x[2] * d[1] + x[1] * d[2] + b[2];
            d[1] = dum;
            dum = x[1] * b[1] - x[2] * b[2] + a[1][j];
            b[2] = x[2] * b[1] + x[1] * b[2] + a[2][j];
            b[1] = dum;
            erq = cabs(b[1],b[2]) + abx * erq;
        }
        erq = epss * erq;
		tttt=cabs(b[1],b[2]);
        if (cabs(b[1],b[2]) <= erq)
		{
           value(x1,dx,gm,gp,sq,g2,h,f,d,b,zero);
           return;
		}
        else
		{
            g[1] = cdiv1(d[1],d[2],b[1],b[2]);
            g[2] = cdiv2(d[1],d[2],b[1],b[2]);
            g2[1] = g[1] * g[1] - g[2] * g[2];
            g2[2] = 2 * g[1] * g[2];
            h[1] = g2[1] - 2 * cdiv1(f[1],f[2],b[1],b[2]);
            h[2] = g2[2] - 2 * cdiv2(f[1],f[2],b[1],b[2]);
            dum1 = (m - 1) * (m * h[1] - g2[1]);
            dum2 = (m - 1) * (m * h[2] - g2[2]);
            sq[1] = csqr1(dum1,dum2);
            sq[2] = csqr2(dum1,dum2);
            gp[1] = g[1] + sq[1];
            gp[2] = g[2] + sq[2];
            gm[1] = g[1] - sq[1];
            gm[2] = g[2] - sq[2];
            if (cabs(gp[1],gp[2]) < cabs(gm[1],gm[2]))
			{
                gp[1] = gm[1];
                gp[2] = gm[2];
            }
            dx[1] = cdiv1(m,0,gp[1],gp[2]);
            dx[2] = cdiv2(m,0,gp[1],gp[2]);
        }
        x1[1] = x[1] - dx[1];
        x1[2] = x[2] - dx[2];
        if ((x[1] == x1[1]) && (x[2] == x1[2]))
		{
            value(x1,dx,gm,gp,sq,g2,h,f,d,b,zero);
                return;
        }
        x[1] = x1[1];
        x[2] = x1[2];
        cdx = cabs(dx[1],dx[2]);
        dxold = cdx;
        if (! polish)
		{
            if (cdx <= eps * cabs(x[1],x[2]))
			{
                value(x1,dx,gm,gp,sq,g2,h,f,d,b,zero);
                return;
            }
        }
    }
    cout<< "too many iterations"<<endl;
}