倒立振り子の微分方程式

/****************************/
/* 倒立振り子の微分方程式   */
/*      coded by Y.Suganuma */
/****************************/
import java.io.*;

class Test {

	static double a[][] = new double [2][2], b, k1;

	static void main(String args[]) throws IOException
	{
		double end_t = 3.0, g = 9.8, h = 0.01, I, k, l, m, pi, r, time, ut, uti,
               x[], dx[], wk[][];
					// 領域の確保
		x  = new double [2];
		dx = new double [2];
		wk = new double [4][2];
					// 微分方程式の係数の設定
		pi   = 4.0 * Math.atan(1.0);
		ut   = pi / 180.0;
		uti  = 1.0 / ut;
		m    = 10.0;
		l    = 1.0;
		r    = 0.5 * l;
		I    = l * l / 12.0;
		k    = m;
		k1   = 2.0 * m * r * g;

		a[0][0] = 0.0;
		a[0][1] = 1.0;
		a[1][0] = m * r * g / (I + m * r * r);
		a[1][1] = -k / (I + m * r * r);
		b       = -1.0 / (I + m * r * r);
					// 初期設定
		time = 0.0;
		x[0] = 10.0;
		x[1] = 0.0;
						// ラジアン単位に変換
		x[0] *= ut;
		x[1] *= ut;
					// 微分方程式を解く
		System.out.println(time + " " + x[0]*uti);
		Kansu kn = new Kansu(0);
		while (time < end_t) {
			time = App.rkg(time, h, x, dx, wk, 2, kn);
			System.out.println(time + " " + x[0]*uti);
		}
	}
}

/****************************/
/* 倒立振り子の運動方程式   */
/*      time : 時間         */
/*      x    : 位置と速度   */
/*      dx   : xの微分      */
/****************************/
class Kansu {
	private int sw;
					// コンストラクタ
	Kansu (int s) {sw = s;}
					// void型関数
	void snx(double time, double x[], double dx[])
	{
		switch (sw) {
			case 0:
				dx[0] = Test.a[0][0] * x[0] + Test.a[0][1] * x[1];
				dx[1] = Test.a[1][0] * Math.sin(x[0]) + Test.a[1][1] * x[1] +
                        Test.b * Test.k1 * x[0];
				break;
		}
	}
}

/************************
/* 科学技術系算用の手法 */
/************************/
class App {

	/*************************************************/
	/* ルンゲ・クッタ法  dx/dt=f(t,x)                */
	/*      time : 現在の時間                        */
	/*      h : 時間刻み幅                           */
	/*      x : 現在の状態                           */
	/*      dx : 微係数（f(t,x):snxで計算する）      */
	/*      g : 作業域（g[4][n]）                    */
	/*      n : 微分方程式の次数                     */
	/*      kn : 微係数を計算するクラスオブジェクト  */
	/*      return : time+h                          */
	/*************************************************/
	static double rkg(double time, double h, double x[], double dx[],
                      double g[][], int n, Kansu kn)
	{
		int i1;
		double h2;

		h2 = 0.5 * h;

		kn.snx(time, x, dx);
		for (i1 = 0; i1 < n; i1++)
			g[0][i1] = h * dx[i1];

		time += h2;
		for (i1 = 0; i1 < n; i1++)
			g[1][i1] = x[i1] + 0.5 * g[0][i1];
		kn.snx(time, g[1], dx);
		for (i1 = 0; i1 < n; i1++)
			g[1][i1] = h * dx[i1];

		for (i1 = 0; i1 < n; i1++)
			g[2][i1] = x[i1] + 0.5 * g[1][i1];
		kn.snx(time, g[2], dx);
		for (i1 = 0; i1 < n; i1++)
			g[2][i1] = h * dx[i1];

		time += h2;
		for (i1 = 0; i1 < n; i1++)
			g[3][i1] = x[i1] + g[2][i1];
		kn.snx(time, g[3], dx);
		for (i1 = 0; i1 < n; i1++)
			g[3][i1] = h * dx[i1];

		for (i1 = 0; i1 < n; i1++)
			x[i1] = x[i1] + (g[0][i1] + 2.0 * g[1][i1] + 2.0 * g[2][i1] + g[3][i1]) / 6.0;

		return time;
	}
}