Create a gist now

Instantly share code, notes, and snippets.

anonymous /double pendulum Secret
Created Dec 15, 2015

What would you like to do?
# 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);
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment