代数方程式（ベアストウ）

/************************************/
/* 代数方程式の解（ベアストウ法）   */
/*      例：(x+1)(x-2)(x-3)(x2+x+1) */
/*           =x5-3x4-2x3+3x2+7x+6=0 */
/*      coded by Y.Suganuma         */
/************************************/
#include <stdio.h>

int Bairstow(int, int, double, double, double,
             double *, double *, double *, double *, double *);

int main()
{
	double *a, *b, *c, *rl, *im, p0, q0, eps;
	int i1, ind, ct, n;
					// データの設定
	ct   = 1000;
	eps  = 1.0e-10;
	p0   = 0.0;
	q0   = 0.0;
	n    = 5;
	a    = new double [n+1];
	b    = new double [n+1];
	c    = new double [n+1];
	rl   = new double [n];
	im   = new double [n];

	a[0] = 1.0;
	a[1] = -3.0;
	a[2] = -2.0;
	a[3] = 3.0;
	a[4] = 7.0;
	a[5] = 6.0;
					// 計算
	ind = Bairstow(n, ct, eps, p0, q0, a, b, c, rl, im);
					// 出力
	if (ind > 0)
		printf("収束しませんでした！\n");
	else {
		for (i1 = 0; i1 < n; i1++)
			printf("   %f  i %f\n", rl[i1], im[i1]);
	}

	delete [] a;
	delete [] b;
	delete [] c;
	delete [] rl;
	delete [] im;

	return 0;
}

/*************************************************/
/* 実係数代数方程式の解（ベアストウ法）          */
/*      n : 次数                                 */
/*      ct : 最大繰り返し回数                    */
/*      eps : 収束判定条件                       */
/*      p0, q0 : x2+px+qにおけるp,qの初期値      */
/*      a : 係数（最高次から与え，値は変化する） */
/*      b,c : 作業域（(n+1)次の配列）            */
/*      rl, im : 結果の実部と虚部                */
/*      return : =0 : 正常                       */
/*               =1 : 収束せず                   */
/*      coded by Y.Suganuma                      */
/*************************************************/
#include <math.h>

int Bairstow(int n, int ct, double eps, double p0, double q0, 
             double *a, double *b, double *c, double *rl, double *im)
{
	double D, dp, dq, p1 = p0, p2 = 0.0, q1 = q0, q2 = 0.0;
	int i1, ind = 0, count = 0;
/*
          １次の場合
*/
	if (n == 1) {
		if (fabs(a[0]) < eps)
			ind = 1;
		else {
			rl[0] = -a[1] / a[0];
			im[0] = 0.0;
		}
	}
/*
          ２次の場合
*/
	else if (n == 2) {
					// １次式
		if (fabs(a[0]) < eps) {
			if (fabs(a[1]) < eps)
				ind = 1;
			else {
				rl[0] = -a[2] / a[1];
				im[0] = 0.0;
			}
		}
					// ２次式
		else {
			D = a[1] * a[1] - 4.0 * a[0] * a[2];
			if (D < 0.0) {   // 虚数
				D      = sqrt(-D);
				a[0]  *= 2.0;
				rl[0]  = -a[1] / a[0];
				rl[1]  = -a[1] / a[0];
				im[0]  = D / a[0];
				im[1]  = -im[0];
			}
			else {           // 実数
				D      = sqrt(D);
				a[0]   = 1.0 / (2.0 * a[0]);
				rl[0]  = a[0] * (-a[1] + D);
				rl[1]  = a[0] * (-a[1] - D);
				im[0]  = 0.0;
				im[1]  = 0.0;
			}
		}
	}
					// ３次以上の場合
	else {
						// 因数分解
		ind = 1;
		while (ind > 0 && count <= ct) {
			for (i1 = 0; i1 <= n; i1++) {
				if (i1 == 0)
					b[i1] = a[i1];
				else if (i1 == 1)
					b[i1] = a[i1] - p1 * b[i1-1];
				else
					b[i1] = a[i1] - p1 * b[i1-1] - q1 * b[i1-2];
			}
			for (i1 = 0; i1 <= n; i1++) {
				if (i1 == 0)
					c[i1] = b[i1];
				else if (i1 == 1)
					c[i1] = b[i1] - p1 * c[i1-1];
				else
					c[i1] = b[i1] - p1 * c[i1-1] - q1 * c[i1-2];
			}
			D = c[n-2] * c[n-2] - c[n-3] * (c[n-1] - b[n-1]);
			if (fabs(D) < eps)
				return ind;
			else {
				dp = (b[n-1] * c[n-2] - b[n] * c[n-3]) / D;
				dq = (b[n] * c[n-2] - b[n-1] * (c[n-1] - b[n-1])) / D;
				p2 = p1 + dp;
				q2 = q1 + dq;
				if (fabs(dp) < eps && fabs(dq) < eps)
					ind = 0;
				else {
					count++;
					p1 = p2;
					q1 = q2;
				}
			}
		}

		if (ind == 0) {
						// ２次方程式を解く
			D = p2 * p2 - 4.0 * q2;
			if (D < 0.0) {   // 虚数
				D      = sqrt(-D);
				rl[0]  = -0.5 * p2;
				rl[1]  = -0.5 * p2;
				im[0]  = 0.5 * D;
				im[1]  = -im[0];
			}
			else {           // 実数
				D      = sqrt(D);
				rl[0]  = 0.5 * (-p2 + D);
				rl[1]  = 0.5 * (-p2 - D);
				im[0]  = 0.0;
				im[1]  = 0.0;
			}
						// 残りの方程式を解く
			n -= 2;
			for (i1 = 0; i1 <= n; i1++)
				a[i1] = b[i1];
			ind = Bairstow(n, ct, eps, p0, q0, a, b, c, &rl[2], &im[2]);
		}
	}

	return ind;
}