倒立振り子の微分方程式

/****************************/
/* 倒立振り子の微分方程式   */
/*      coded by Y.Suganuma */
/****************************/
#include <stdio.h>
#include <math.h>

void link(double, double *, double *);
double rkg(double, double, double *, double *, double **, int,
           void (*)(double,double *,double *));

double a[2][2], b, k1;

int main(void)
{
	double end_t = 3.0, g = 9.8, h = 0.01, I, k, l, m, pi, r, time, ut, uti,
           x[2], dx[2], **wk;
	int i1;
					// 領域の確保
	wk = new double * [4];
	for (i1 = 0; i1 < 4; i1++)
		wk[i1] = new double [2];
					// 微分方程式の係数の設定
	pi   = 4.0 * 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;
					// 微分方程式を解く
	printf("%f %f\n", time, x[0]*uti);
	while (time < end_t) {
		time = rkg(time, h, x, dx, wk, 2, link);
		printf("%f %f\n", time, x[0]*uti);
	}
					// 領域の解法
	for (i1 = 0; i1 < 4; i1++)
		delete [] wk[i1];
	delete [] wk;

	return 0;
}

/****************************/
/* 倒立振り子の運動方程式   */
/*      time : 時間         */
/*      x    : 位置と速度   */
/*      dx   : xの微分      */
/****************************/
void link(double time, double *x, double *dx)
{
	dx[0] = a[0][0] * x[0] + a[0][1] * x[1];
	dx[1] = a[1][0] * sin(x[0]) + a[1][1] * x[1] + b * k1 * x[0];
}

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

	h2 = 0.5 * h;

	(*sub)(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];
	(*sub)(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];
	(*sub)(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];
	(*sub)(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;
}