static void Show(double t, PhaseVector q)
{
double y = Math.Sin(q.q2);
double x = Math.Cos(q.q1) * y;
y = Math.Sin(q.q1) * y;
double z = -Math.Cos(q.q2);
Console.Write("{0},{1},{2},{3}\n",
t, x, y, z);
}
static void Main(string[] args)
{
const double dt = 0.01;
const double t_end = 10;
const int DISPLAY_INTERVAL = 5;
const double q1 = 0;
const double q2 = Math.PI / 2;
const double p1 = 0.1;
const double p2 = 0;
PhaseVector.Simulate(
0, t_end, dt, DISPLAY_INTERVAL,
new PhaseVector(q1, q2, p1, p2),
F, Show
);
}
static PhaseVector F(PhaseVector q)
{
const double M = 0.1;
const double G = 10;
double s2 = Math.Sin(q.q2);
double c2 = Math.Cos(q.q2);
return(new PhaseVector(
q.p1 / (M * s2),
q.p2 / M,
0,
(q.p1 * q.p1 * c2)
/ (M * s2 * s2 * s2)
- M * G * s2
));
}
/// <summary>
/// 微分方程式
/// (d/dt)q = f(q)
/// の解を数値計算で求める。
/// </summary>
/// <param name="t0">時刻の初期値</param>
/// <param name="t1">時刻の最終値</param>
/// <param name="dt">時刻の刻み幅</param>
/// <param name="display_interval">結果出力の間隔</param>
/// <param name="initial">q の初期値</param>
/// <param name="f">f</param>
/// <param name="cb">結果出力用のコールバック関数</param>
/// <remarks>
/// 4次のルンゲクッタ法で計算。
/// </remarks>
public static void Simulate(
double t0, double t1, double dt, int display_interval,
PhaseVector initial,
PhaseFunc f, Callback cb)
{
PhaseVector q = initial;
int n = 1;
for (double t = t0; t < t1; t += dt, ++n)
{
PhaseVector k1 = dt * f(q);
PhaseVector k2 = dt * f(q + k1 / 2);
PhaseVector k3 = dt * f(q + k2 / 2);
PhaseVector k4 = dt * f(q + k3);
q = q + (k1 + 2 * (k2 + k3) + k4) / 6;
if (n > display_interval)
{
cb(t, q);
n = 1;
}
}
}
/// <summary>
/// 微分方程式
/// (d/dt)q = f(q)
/// の解を数値計算で求める。
/// </summary>
/// <param name="t0">時刻の初期値</param>
/// <param name="t1">時刻の最終値</param>
/// <param name="dt">時刻の刻み幅</param>
/// <param name="display_interval">結果出力の間隔</param>
/// <param name="initial">q の初期値</param>
/// <param name="f">f</param>
/// <param name="cb">結果出力用のコールバック関数</param>
/// <remarks>
/// 4次のルンゲクッタ法で計算。
/// </remarks>
public static void Simulate(
double t0, double t1, double dt, int display_interval,
PhaseVector initial,
PhaseFunc f, Callback cb)
{
PhaseVector q = initial;
int n = 1;
for (double t = t0; t < t1; t += dt, ++n)
{
PhaseVector k1 = dt * f(q);
PhaseVector k2 = dt * f(q + k1 / 2);
PhaseVector k3 = dt * f(q + k2 / 2);
PhaseVector k4 = dt * f(q + k3);
q = q + (k1 + 2 * (k2 + k3) + k4) / 6;
if (n > display_interval)
{
cb(t, q);
n = 1;
}
}
}
static void Show(double t, PhaseVector q)
{
double y = Math.Sin(q.q2);
double x = Math.Cos(q.q1) * y;
y = Math.Sin(q.q1) * y;
double z = -Math.Cos(q.q2);
Console.Write("{0},{1},{2},{3}\n",
t, x, y, z);
}
static PhaseVector F(PhaseVector q)
{
const double M = 0.1;
const double G = 10;
double s2 = Math.Sin(q.q2);
double c2 = Math.Cos(q.q2);
return new PhaseVector(
q.p1 / (M * s2),
q.p2 / M,
0,
(q.p1 * q.p1 * c2)
/ (M * s2 * s2 * s2)
- M * G * s2
);
}
