Kody źródłowe/Metoda Laguerre'a dla pierwiastków wielokrotnych

C

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <float.h>
#include <algorithm>

/* complex
mul: (a + bi)(c + di) = (ac - bd) + (bc + ad)i
div (a + bi)/(c + di) = ((ac + bd) + (bc - ad)i)/(c^2 + d^2)
*/
void hornerZ(double &resRe, double &resIm, double xRe, double xIm, int n, double *P)
{
	resIm = 0;
	if (n < 0)
	{
		resRe = 0;
		return;
	}
	resRe = P[n];
	for (int i = n - 1; i >= 0; i--)
	{
		double origResRe = resRe;
		resRe = (resRe*xRe - resIm*xIm);
		resIm = resIm*xRe + origResRe*xIm;
		resRe += P[i];
	}
}

void dhornerZ(double &resRe, double &resIm, double a, double b, int n, double *P)
{
	resIm = 0;
	if (n < 1)
	{
		resRe = 0;
		return;
	}
	resRe = P[n] * n;
	for (int i = n - 1; i >= 1; i--)
	{
		double origResRe = resRe;
		resRe = (resRe*a - resIm*b);
		resIm = resIm*a + origResRe*b;
		resRe += P[i] * i;
	}
}

void d2hornerZ(double &resRe, double &resIm, double a, double b, int n, double *P)
{
	resIm = 0;
	if (n < 2)
	{
		resRe = 0;
		return;
	}
	resRe = P[n] * n * (n - 1);
	for (int i = n - 1; i >= 2; i--)
	{
		double origResRe = resRe;
		resRe = (resRe*a - resIm*b);
		resIm = resIm*a + origResRe*b;
		resRe += P[i] * i *(i - 1);
	}
}

void dnthhornerZ(double &resRe, double &resIm, double a, double b, int n, double *P, int nth)
{
	resIm = 0;
	if (nth > n)
	{
		resRe = 0;
		return;
	}
	double factor = 1;
	for (int j = n; j > n - nth; j--)
		factor *= j;
	resRe = P[n] * factor;
	for (int i = n - 1; i >= nth; i--)
	{
		double origResRe = resRe;
		resRe = (resRe*a - resIm*b);
		resIm = resIm*a + origResRe*b;
		factor = 1;
		for (int j = i; j > i - nth; j--)
			factor *= j;
		resRe += P[i] * factor;
	}
}

double estimEps(double x, int n, double *P)
{
	if (n<0) return 0;
	double value = P[n];
	double esterror = 0;
	for (int i = n - 1; i >= 0; i--)
	{
		value *= x;
		double esterror1 = esterror*x;
		double esterror2 = 0.35* value;
		esterror = sqrt(esterror1*esterror1 + esterror2*esterror2);
		value += P[i];
		esterror = std::max(esterror, fabs(0.35*value));
	}
	return esterror;
}

double destimEps(double x, int n, double *P, int nth)
{
	if (nth > n) return 0;
	double factor = 1;
	for (int j = n; j > n - nth; j--)
		factor *= j;
	double value = P[n] * factor;
	double esterror = 0;
	for (int i = n - 1; i >= 0; i--)
	{
		value *= x;
		double esterror1 = esterror*x;
		double esterror2 = 0.35* value;
		esterror = sqrt(esterror1*esterror1 + esterror2*esterror2);
		factor = 1;
		for (int j = i; j > i - nth; j--)
			factor *= j;
		value += P[i] * factor;
		esterror = std::max(esterror, fabs(0.35*value));
	}
	return esterror;
}

int dLaguerreZ(double &resRe, double &resIm, double xRe, double xIm, int n, double *P, int nth, double baseEstError)
{
	int loopCnt = 0;
	if (n <= nth)
	{
		resRe = NAN;
		resIm = NAN;
		return loopCnt;
	}
	while (true)
	{
		double estError = destimEps(sqrt(xRe*xRe + xIm*xIm), n, P, nth);
		double hRe, hIm;
		dnthhornerZ(hRe, hIm, xRe, xIm, n, P, nth);
		double dhRe, dhIm;
		dnthhornerZ(dhRe, dhIm, xRe, xIm, n, P, nth + 1);
		if (sqrt(hRe*hRe + hIm*hIm) < DBL_EPSILON*sqrt(dhRe*dhRe + dhIm*dhIm + estError*estError))
		{
			double tryRe, tryIm;
			dLaguerreZ(tryRe, tryIm, xRe, xIm, n, P, nth + 1, estError);
			if (!isnan(tryRe))
			{
				xRe = tryRe;
				xIm = tryIm;
			}
			break;
		}
		double GRe, GIm;
		double denomh = hRe*hRe + hIm*hIm;
		GRe = (dhRe*hRe + dhIm*hIm) / denomh;
		GIm = (dhIm*hRe - dhRe*hIm) / denomh;
		double G2Re, G2Im;
		G2Re = GRe*GRe - GIm*GIm;
		G2Im = 2 * GRe * GIm;
		double d2hRe, d2hIm;
		dnthhornerZ(d2hRe, d2hIm, xRe, xIm, n, P, nth + 2);
		double GRe2, GIm2;
		GRe2 = (d2hRe*hRe + d2hIm*hIm) / denomh;
		GIm2 = (d2hIm*hRe - d2hRe*hIm) / denomh;
		double HRe, HIm;
		HRe = G2Re - GRe2;
		HIm = G2Im - GIm2;
		double vRe, vIm;
		vRe = (n - 1)*(n*HRe - G2Re);
		vIm = (n - 1)*(n*HIm - G2Im);
		double modv = sqrt(vRe*vRe + vIm*vIm);
		double sRe, sIm;
		sRe = sqrt((modv + vRe) / 2);
		sIm = sqrt((modv - vRe) / 2);
		if (vIm < 0) sIm = -sIm;
		double denom1Re, denom1Im;
		denom1Re = GRe + sRe;
		denom1Im = GIm + sIm;
		double denom2Re, denom2Im;
		denom2Re = GRe - sRe;
		denom2Im = GIm - sIm;
		double denomRe, denomIm;
		if (denom1Re*denom1Re + denom1Im*denom1Im > denom2Re*denom2Re + denom2Im*denom2Im)
		{
			denomRe = denom1Re;
			denomIm = denom1Im;
		}
		else
		{
			denomRe = denom2Re;
			denomIm = denom2Im;
		}
		double aRe, aIm;
		double denoma = denomRe*denomRe + denomIm*denomIm;
		aRe = n*denomRe / denoma;
		aIm = -n*denomIm / denoma;
		if (sqrt(aRe*aRe + aIm*aIm) < 2 * DBL_EPSILON*sqrt(xRe*xRe + xIm*xIm))
		{
			xRe -= 0.5*aRe;
			xIm -= 0.5*aIm;
			break;
		}
		xRe -= aRe;
		xIm -= aIm;
		loopCnt++;
		double baseRe, baseIm;
		dnthhornerZ(baseRe, baseIm, xRe, xIm, n, P, nth - 1);
		if (sqrt(baseRe*baseRe + baseIm*baseIm) >= 2*DBL_EPSILON*baseEstError)
		{
			resRe = NAN;
			resIm = NAN;
			return loopCnt;
		}
	}
	resRe = xRe;
	resIm = xIm;
	return loopCnt;
}

int LaguerreMZ(double &resRe, double &resIm, double xRe, double xIm, int n, double *P)
{
	int loopCnt = 0;
	if (n <= 0)
	{
		resRe = NAN;
		resIm = NAN;
		return loopCnt;
	}
	while (true)
	{
		double estError = estimEps(sqrt(xRe*xRe + xIm*xIm), n, P);
		double hRe, hIm;
		hornerZ(hRe, hIm, xRe, xIm, n, P);
		double dhRe, dhIm;
		dhornerZ(dhRe, dhIm, xRe, xIm, n, P);
		if (sqrt(hRe*hRe + hIm*hIm) < DBL_EPSILON*sqrt(dhRe*dhRe + dhIm*dhIm + 1.4*estError*estError))
		{
			double tryRe, tryIm;
			dLaguerreZ(tryRe, tryIm, xRe, xIm, n, P, 1, estError);
			if (!isnan(tryRe))
			{
				xRe = tryRe;
				xIm = tryIm;
			}
			break;
		}
		double GRe, GIm;
		double denomh = hRe*hRe + hIm*hIm;
		GRe = (dhRe*hRe + dhIm*hIm) / denomh;
		GIm = (dhIm*hRe - dhRe*hIm) / denomh;
		double G2Re, G2Im;
		G2Re = GRe*GRe - GIm*GIm;
		G2Im = 2 * GRe * GIm;
		double d2hRe, d2hIm;
		d2hornerZ(d2hRe, d2hIm, xRe, xIm, n, P);
		double GRe2, GIm2;
		GRe2 = (d2hRe*hRe + d2hIm*hIm) / denomh;
		GIm2 = (d2hIm*hRe - d2hRe*hIm) / denomh;
		double HRe, HIm;
		HRe = G2Re - GRe2;
		HIm = G2Im - GIm2;
		double vRe, vIm;
		vRe = (n - 1)*(n*HRe - G2Re);
		vIm = (n - 1)*(n*HIm - G2Im);
		double modv = sqrt(vRe*vRe + vIm*vIm);
		double sRe, sIm;
		sRe = sqrt((modv + vRe) / 2);
		sIm = sqrt((modv - vRe) / 2);
		if (vIm < 0) sIm = -sIm;
		double denom1Re, denom1Im;
		denom1Re = GRe + sRe;
		denom1Im = GIm + sIm;
		double denom2Re, denom2Im;
		denom2Re = GRe - sRe;
		denom2Im = GIm - sIm;
		double denomRe, denomIm;
		if (denom1Re*denom1Re + denom1Im*denom1Im > denom2Re*denom2Re + denom2Im*denom2Im)
		{
			denomRe = denom1Re;
			denomIm = denom1Im;
		}
		else
		{
			denomRe = denom2Re;
			denomIm = denom2Im;
		}
		double aRe, aIm;
		double denoma = denomRe*denomRe + denomIm*denomIm;
		aRe = n*denomRe / denoma;
		aIm = -n*denomIm / denoma;
		if (sqrt(aRe*aRe + aIm*aIm) < 2 * DBL_EPSILON*sqrt(xRe*xRe + xIm*xIm))
		{
			xRe -= 0.5*aRe;
			xIm -= 0.5*aIm;
			break;
		}
		xRe -= aRe;
		xIm -= aIm;
		loopCnt++;
	}
	resRe = xRe;
	resIm = xIm;
	return loopCnt;
}

//const int degP = 6;
//double P[degP + 1] = { 36,-48,-3,17,2,-5,1 }; //(x^2 + 3·x + 3)(x-1)(x-2)(x-2)(x-3)
const int degP = 8;
double P[degP + 1] = {1,-8,28,-56,70,-56,28,-8,1};//(x - 1) ^ 8

int main()
{
	double a, b;
	LaguerreMZ(a,b, 0.99, 0, degP, P);
	printf("%.16f %.16f\n", a,b);
	return 0;
}