double pendulum

# include <Siv3D.hpp>
# define THETA1 Pi
# define THETA2 Pi+0.01
# define M1 10.0
# define M2 30.0
# define L1 100
# define L2 90
# define G  9.80665
# define DT 0.00002
# define V1 0.0
# define V2 0.0

# define C Cos(theta1 - theta2)
# define S Sin(theta1 - theta2)
# define M (m1 + m2)

const double m1 = M1;
const double m2 = M2;
const double l1 = L1;
const double l2 = L2;
const double g = G;
const double dt = DT;

double F1(double theta1, double theta2, double v1, double v2, double t){
	double tmp[3];

	tmp[0] = g * (Sin(theta2) * C - M * Sin(theta1) / m2);
	tmp[1] = (l1 * v1 * v1 * C + l2 * v2 * v2) * S;
	tmp[2] = l1 * (M / m2 - C * C);

	return (tmp[0] - tmp[1]) / tmp[2];
}

double F2(double theta1, double theta2, double v1, double v2, double t){
	double tmp[3];

	tmp[0] = g * M / m2 * (C * Sin(theta1) - Sin(theta2));
	tmp[1] = S * (M / m2 * l1 * v1 * v1 + l2 * v2 * v2 * C);
	tmp[2] = l2 * (M / m2 - C * C);

	return (tmp[0] + tmp[1]) / tmp[2];
}

double Runge_Kutta1(double theta1, double theta2, double v1, double v2, double t){
	double k[4];

	k[0] = F1(theta1, theta2, v1, v2, t);
	k[1] = F1(theta1, theta2, v1 + dt * k[0] / 2.0, v2 + dt * k[0] / 2.0, t + dt / 2.0);
	k[2] = F1(theta1, theta2, v1 + dt * k[1] / 2.0, v2 + dt * k[1] / 2.0, t + dt / 2.0);
	k[3] = F1(theta1, theta2, v1 + dt * k[2], v2 + dt * k[2], t + dt);

	return dt * (k[0] + 2 * k[1] + 2 * k[2] + k[3]) / 6.0;
}

double Runge_Kutta2(double theta1, double theta2, double v1, double v2, double t){
	double k[4];

	k[0] = F2(theta1, theta2, v1, v2, t);
	k[1] = F2(theta1, theta2, v1 + dt * k[0] / 2.0, v2 + dt * k[0] / 2.0, t + dt / 2.0);
	k[2] = F2(theta1, theta2, v1 + dt * k[1] / 2.0, v2 + dt * k[1] / 2.0, t + dt / 2.0);
	k[3] = F2(theta1, theta2, v1 + dt * k[2], v2 + dt * k[2], t + dt);

	return dt * (k[0] + 2 * k[1] + 2 * k[2] + k[3]) / 6.0;;
}

void Main()
{

	double theta1 = THETA1;
	double theta2 = THETA2;
	double v1 = V1;
	double v2 = V2;
	double t = 0.0;

	double i = 0, E = 0;

	Vec2 pos1, pos2;

	const Font font(10);
	Image image(Window::Size(), Palette::Black);
	DynamicTexture texture;

	while (System::Update())
	{

		for (i = 0; i < 0.14; i += dt){

			pos1 = Circular(l1, theta1 - Pi) + Window::Center();
			pos2 = pos1 + Circular(l2, theta2 - Pi);

			v1 += Runge_Kutta1(theta1, theta2, v1, v2, t);
			v2 += Runge_Kutta2(theta1, theta2, v1, v2, t);

			theta1 += v1 * dt;
			theta2 += v2 * dt;

			t += dt;

			Point(pos1.x, pos1.y).overwrite(image, Palette::White);
			Point(pos2.x, pos2.y).overwrite(image, Palette::Lightgreen);
		}

		E = M * l1 * l1 * v1 * v1 / 2.0 + m2 * l2 * l2 *v2 * v2 / 2.0 + m2 * l1 * l2 * v1 * v2 * Cos(Pi - (theta1 - theta2))

			+ M * g * l1 * (1 - Cos(theta1)) - m2 * g * l2 * (1 - Cos(theta2));

		texture.fill(image);
		texture.draw();

		Line(pos1, pos2).draw(5, Palette::Cyan);
		Line(Window::Center(), pos1).draw(5, Palette::Yellowgreen);

		Circle(pos1, M1).draw(Palette::Orange);
		Circle(pos2, M2).draw(Palette::Chartreuse);
		Circle(Window::Center(), 5).draw();

		font(L"v1\t", v1).draw();
		font(L"v2\t", v2).draw(180, 0);
		font(L"θ1\t", theta1).draw(0, 15);
		font(L"θ2\t", theta2).draw(180, 15);
		font(Profiler::FPS(), L"fps").draw(0, 30);
		font(L"E = ", E).draw(0, 45);
	}
}