非線形方程式（ニュートン法，多次元）

/***************************************/
/* 多次元ニュートン法                  */
/* （３点(0.5,1.0),(0.0,1.5),(0.5,2.0) */
/*   を通る円の中心と半径）            */
/*      coded by Y.Suganuma            */
/***************************************/
#include <stdio.h>

int newton(void(*)(double *, double *), int(*)(double *, double **, double),
           double *, double, double, int, double **, int, double *);
void snx(double *, double *);
int dsnx(double *, double **, double);
int gauss(double **, int, int, double);

int main()
{
	double eps1, eps2, x[3], x0[3], **w;
	int i1, max, ind;

	eps1  = 1.0e-7;
	eps2  = 1.0e-10;
	max   = 20;
	x0[0] = 0.0;
	x0[1] = 0.0;
	x0[2] = 2.0;
	w     = new double * [3];
	for (i1 = 0; i1 < 3; i1++)
		w[i1] = new double [6];

	ind = newton(snx, dsnx, x0, eps1, eps2, max, w, 3, x);

	printf("ind = %d\n", ind);
	printf("x");
	for (i1 = 0; i1 < 3; i1++)
		printf(" %f", x[i1]);
	printf("\nf ");
	snx(x, x0);
	for (i1 = 0; i1 < 3; i1++)
		printf(" %f", x0[i1]);
	printf("\n");

	for (i1 = 0; i1 < 3; i1++)
		delete [] w[i1];
	delete [] w;

	return 0;
}

/*****************************************************/
/* Newton法による非線形方程式(f(x)=0)の解            */
/*      fn : f(x)を計算する関数名                    */
/*      dfn : f(x)の微分を計算する関数名             */
/*      x0 : 初期値                                  */
/*      eps1 : 終了条件１（｜x(k+1)-x(k)｜＜eps1）   */
/*      eps2 : 終了条件２（｜f(x(k))｜＜eps2）       */
/*      max : 最大試行回数                           */
/*      w : f(x)の微分の逆行列を入れる (n x 2n)      */
/*      n : xの次数                                  */
/*      x : 解                                       */
/*      return : 実際の試行回数                      */
/*            （負の時は解を得ることができなかった） */
/*****************************************************/
#include <math.h>

int newton(void(*f)(double *, double *), int(*df)(double *, double **, double),
           double *x0, double eps1, double eps2, int max, double **w,
           int n, double *x)
{
	double *g, *x1;
	int i1, i2, ind = 0, sw, sw1;

	g  = new double [n];
	x1 = new double [n];
	sw = 0;
	for (i1 = 0; i1 < n; i1++) {
		x1[i1] = x0[i1];
		x[i1]  = x1[i1];
	}

	while (sw == 0 && ind >= 0) {

		sw = 1;
		ind++;
		(*f)(x1, g);

		sw1 = 0;
		for (i1 = 0; i1 < n && sw1 == 0; i1++) {
			if (fabs(g[i1]) > eps2)
				sw1 = 1;
		}
		if (sw1 > 0) {
			if (ind <= max) {
				sw1 = (*df)(x1, w, eps2);
				if (sw1 == 0) {
					for (i1 = 0; i1 < n; i1++) {
						x[i1] = x1[i1];
						for (i2 = 0; i2 < n; i2++)
							x[i1] -= w[i1][i2+n] * g[i2];
					}
					sw1 = 0;
					for (i1 = 0; i1 < n && sw1 == 0; i1++) {
						if (fabs(x[i1]-x1[i1]) > eps1 && fabs(x[i1]-x1[i1]) > eps1*fabs(x[i1]))
							sw1 = 1;
					}
					if (sw1 > 0) {
						for (i1 = 0; i1 < n; i1++)
							x1[i1] = x[i1];
						sw = 0;
					}
				}
				else
					ind = -1;
			}
			else
				ind = -1;
		}
	}

	delete [] g;
	delete [] x1;

	return ind;
}

/************************/
/* 関数値（f(x)）の計算 */
/************************/
void snx(double *x, double *y)
{
	y[0] = (0.5 - x[0]) * (0.5 - x[0]) + (1.0 - x[1]) * (1.0 - x[1]) - x[2] * x[2];
	y[1] = (0.0 - x[0]) * (0.0 - x[0]) + (1.5 - x[1]) * (1.5 - x[1]) - x[2] * x[2];
	y[2] = (0.5 - x[0]) * (0.5 - x[0]) + (2.0 - x[1]) * (2.0 - x[1]) - x[2] * x[2];
}

/*****************************************/
/* 関数の微分の計算                      */
/*      x : 変数値                       */
/*      w : 微分の逆行列（後半）         */
/*      eps : 逆行列の存在を判定する規準 */
/*      return : =0 : 正常               */
/*               =1 : 逆行列が存在しない */
/*****************************************/
int dsnx(double *x, double **w, double eps)
{
	int i1, i2, sw;

	for (i1 = 0; i1 < 3; i1++) {
		for (i2 = 0; i2 < 3; i2++)
			w[i1][i2+3] = 0.0;
		w[i1][i1+3] = 1.0;
	}

	w[0][0] = -2.0 * (0.5 - x[0]);
	w[0][1] = -2.0 * (1.0 - x[1]);
	w[0][2] = -2.0 * x[2];
	w[1][0] = -2.0 * (0.0 - x[0]);
	w[1][1] = -2.0 * (1.5 - x[1]);
	w[1][2] = -2.0 * x[2];
	w[2][0] = -2.0 * (0.5 - x[0]);
	w[2][1] = -2.0 * (2.0 - x[1]);
	w[2][2] = -2.0 * x[2];

	sw = gauss(w, 3, 3, eps);

	return sw;
}

/*******************************************************/
/* 線形連立方程式を解く（逆行列を求める）              */
/*      w : 方程式の左辺及び右辺                       */
/*      n : 方程式の数                                 */
/*      m : 方程式の右辺の列の数                       */
/*      eps : 逆行列の存在を判定する規準               */
/*      return : =0 : 正常                             */
/*               =1 : 逆行列が存在しない               */
/*******************************************************/
#include <math.h>

int gauss(double **w, int n, int m, double eps)
{
	double y1, y2;
	int ind = 0, nm, m1, m2, i1, i2, i3;

	nm = n + m;

	for (i1 = 0; i1 < n && ind == 0; i1++) {

		y1 = .0;
		m1 = i1 + 1;
		m2 = 0;
					// ピボット要素の選択
		for (i2 = i1; i2 < n; i2++) {
			y2 = fabs(w[i2][i1]);
			if (y1 < y2) {
				y1 = y2;
				m2 = i2;
			}
		}
					// 逆行列が存在しない
		if (y1 < eps)
			ind = 1;
					// 逆行列が存在する
		else {
						// 行の入れ替え
			for (i2 = i1; i2 < nm; i2++) {
				y1        = w[i1][i2];
				w[i1][i2] = w[m2][i2];
				w[m2][i2] = y1;
			}
						// 掃き出し操作
			y1 = 1.0 / w[i1][i1];

			for (i2 = m1; i2 < nm; i2++)
				w[i1][i2] *= y1;

			for (i2 = 0; i2 < n; i2++) {
				if (i2 != i1) {
					for (i3 = m1; i3 < nm; i3++)
						w[i2][i3] -= w[i2][i1] * w[i1][i3];
				}
			}
		}
	}

	return(ind);
}