Instantly share code, notes, and snippets.

# anonymous/double pendulumSecret Created Dec 15, 2015

 # include # 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); } }