SFML을 이용한 이중 진자 시뮬레이션 Double Pendulum

이경헌·2021년 10월 28일
0

이중 진자(double pendulum)는 하나의 진자의 끝에 다른 진자가 연결된 것으로, 간단한 구조로 이루어져 있지만 해석적 해가 존재하지 않고 chaotic하다는 성질이 있습니다.

SFML 라이브러리를 이용하여 그 형태를 시뮬레이션 해보겠습니다.

이중 진자의 운동방정식

이중 진자를 구성하는 두 진자의 질량과 실의 길이는 모두 같다고 가정하며, 이를 각각 mm, ll로 하겠습니다. 이 때 각 진자의 각변위(angular displacement) θ1\theta_1, θ2\theta_2일반화 운동량 p1p_1, p2p_2에 대해 다음이 성립합니다.

θ1˙=6ml22p13cos(θ1θ2)p2169cos2(θ1θ2)θ2˙=6ml28p23cos(θ1θ2)p1169cos2(θ1θ2)p1˙=12ml2(θ1˙θ2˙sin(θ1θ2)+3glsinθ1)p2˙=12ml2(θ1˙θ2˙sin(θ1θ2)+glsinθ1)\begin{aligned} \dot{\theta_1}&=\frac{6}{ml^2}\frac{2p_1-3\cos(\theta_1-\theta_2)p_2}{16-9\cos^2(\theta_1-\theta_2)} \\ \dot{\theta_2}&=\frac{6}{ml^2}\frac{8p_2-3\cos(\theta_1-\theta_2)p_1}{16-9\cos^2(\theta_1-\theta_2)} \\ \dot{p_1}&=-\frac12ml^2\left(\dot{\theta_1}\dot{\theta_2}\sin(\theta_1-\theta_2)+3\frac gl\sin{\theta_1}\right) \\ \dot{p_2}&=-\frac12ml^2\left(-\dot{\theta_1}\dot{\theta_2}\sin(\theta_1-\theta_2)+\frac gl\sin{\theta_1}\right) \end{aligned}

따라서 네 개의 변수에 해당하는 spontaneous ODE system의 해를 구하면 각 진자의 위치를 얻을 수 있습니다.

Runge-Kutta 방법을 이용한 수치해석

일변수 미분방정식

아쉽게도 위 ODEs에 대한 해석적 해는 존재하지 않습니다(즉, θ1\theta_1θ2\theta_2에 대한 닫힌 형식의 해가 존재하지 않습니다). 이를 룽게-쿠타 방법을 이용해 풀어봅시다.

다음과 같이 정의된 초기값 문제가 있습니다.

y=f(t,y), y(t0)=y0y'=f(t, y),\ y(t_0)=y_0

RK4(4차 룽게-쿠타 방법)은 다음의 공식을 이용해 시간 증분 hh이 지난 경우의 yy를 구할 수 있습니다.

yn+1=yn+16h(k1+2k2+2k3+k4)tn+1=tn+h(1)\begin{aligned} y_{n+1}&=y_n+\frac16 h(k_1+2k_2+2k_3+k_4) \\ t_{n+1}&=t_n+h \end{aligned} \tag{1}

이 때 ki, i=1,2,3,4k_i,\ i=1, 2, 3, 4는 다음으로 주어집니다.

k1=f(tn,yn)k2=f(tn+12h,yn+12hk1)k3=f(tn+12h,yn+12hk2)k4=f(tn+h,yn+hk3)(2)\begin{aligned} k_1&=f(t_n,y_n) \\ k_2&=f(t_n+\frac12 h, y_n+\frac12hk_1) \\ k_3&=f(t_n+\frac12 h, y_n+\frac12hk_2) \\ k_4&=f(t_n+h, y_n+hk_3) \end{aligned} \tag{2}

다변수 미분방정식

그렇다면 이를 ODEs에 대해 적용할 수 있을까요?

네! 그렇습니다.

yy를 실벡터(real vector)로, ff가 벡터 함수라 생각하고, (y+c)j=yj+c(y+c)_j=y_j+c라 하면 위 식 (2)는 잘 정의됩니다.

RK4의 누적오류

RK4 방법은 4차 해법입니다. 이는 각 단계에서의 오류는 tt의 증분 hh에 대해 O(h5)O(h^5), 총 누적 오류는 O(h4)O(h^4)가 됩니다.

C++에서 RK4 구현

std::valarray에 대하여

C++ STL에는 std::valarray라는 실벡터와 유사한 클래스가 있습니다.

std::valarray는 값 배열의 표현과 조작을 위한 클래스입니다. 이는 개별 원소에 대한 수학 연산과 여러 가지 일반화된 아래첨자 연산, 슬라이싱, 그리고 간접 접근을 지원합니다.

다음과 같이 네 개의 원소를 갖는 세 valarray를 선언합니다.

std::valarray<double> vec1 { 1, 2, 3, 4 }, // (1, 2, 3, 4)
                      vec2 { 2, 3, 4, 5 }, // (2, 3, 4, 5)
                      vec3 { 3, 4, 5, 6 }; // (3, 4, 5, 6)

그렇다면 각 vec1, vec2, vec3은 4차원 실벡터라고 생각할 수 있습니다. 실제로, 다음과 같은 덧셈과 스칼라배를 지원합니다!

auto result = 2 * (vec1 + 3 * vec2) - vec3; // (11, 18, 25, 32)

따라서, valarray를 이용하면 위와 같은 RK4의 벡터 덧셈·스칼라배 연산을 쉽게 구현할 수 있겠습니다.

이중 진자 ODE 작성

std::valarray<float> doublePendulumODE(float t, std::valarray<float> y){
    float theta1 = y[0], theta2 = y[1], p1 = y[2], p2 = y[3];

    /*
        theta1_diff = 6 / (m * l^2) * (2 * p1 - 3 * cos(theta1 - theta2) * p2) / (16 - 9 * cos^2(theta1 - theta2))
        theta2_diff = 6 / (m * l^2) * (8 * p2 - 3 * cos(theta1 - theta2) * p1) / (16 - 9 * cos^2(theta1 - theta2))
        p1_diff = -m * l^2 / 2 * (theta1_diff * theta2_diff * sin(theta1 - theta2) + 3 * (g / l) * sin(theta1))
        p2_diff = -m * l^2 / 2 * (-theta1_diff * theta2_diff * sin(theta1 - theta2) + (g / l) * sin(theta2))
    */

    float a = 6.0f / (m * l * l), 
          b = -0.5f * m * l * l, 
          c = 3 * cos(theta1 - theta2), 
          d = sin(theta1 - theta2),
          e = c * c,
          theta1Diff = a * (2.0f * p1 - c * p2) / (16.0f - e),
          theta2Diff = a * (8.0f * p2 - c * p1) / (16.0f - e),
          f = theta1Diff * theta2Diff,
          p1Diff = b * (f * d + 3.0f * g / l * sin(theta1)),
          p2Diff = b * (-f * d + g / l * sin(theta2));

    return { theta1Diff, theta2Diff, p1Diff, p2Diff };
}

인자와 반환값에 들어가는 valarray(θ1,θ2,p1,p2)(\theta_1,\theta_2,p_1,p_2)의 구조입니다. 계산 성능 이득을 위해 일부 중복되는 부분을 문자로 치환하였습니다.

RK4 구현

t에 증분을 더해가며 y값을 점진적으로 변화하는 stepRK4 함수를 작성해봅시다. 인자로는 (이 미분방정식에서는 사용되지 않지만) ODE에서 사용할 t, 증분 dt, 해를 저장할 y, 그리고 계산할 ODE callback을 받습니다.

이후 내용은 이전 식 (1)과 같이 dy1, dy2, dy3, dy4를 계산해주면 됩니다.

void stepRK4(float dt, float &t, std::valarray<float> &y, std::valarray<float> (*ode)(float, std::valarray<float>)){
    std::valarray<float> dy1 { ode(t, y) * dt },
                         dy2 { ode(t + 0.5f * dt, y + 0.5f * dy1) * dt },
                         dy3 { ode(t + 0.5f * dt, y + 0.5f * dy2) * dt },
                         dy4 { ode(t + dt, y + dy3) * dt };

    y += (dy1 + 2.0f * dy2 + 2.0f * dy3 + dy4) / 6.0f;
    t += dt;
}

인자 t, y에 passing by reference 연산자(&)를 적용한 이유는 함수 내에서 해당 인자의 본래 값을 바꿀 수 있도록 해줍니다. 즉, stepRK4를 실행하는 것만으로 인자에 넣은 ty가 변하게 됩니다(t는 증분 dt가 추가되고, y는 해당 t에서의 미분방정식 해가 할당됩니다).

SFML 렌더링

위치 계산

각 진자의 추의 위치는 다음과 같이 계산됩니다.

x1=lsin(θ1)y1=lcos(θ1)x2=x1+lsin(θ2)x2=y1+lcos(θ2)\begin{aligned} x_1&=l\sin(\theta_1) \\ y_1&=l\cos(\theta_1) \\ x_2&=x_1+l\sin(\theta_2) \\ x_2&=y_1+l\cos(\theta_2) \end{aligned}

프로그램은 800x600 해상도의 창에서 렌더링하므로 진자의 중심을 (400, 300)으로, 진자의 길이를 120이라 하면 계산되는 x1, y1, x2, y2는 다음과 같습니다.

float x1 = 400.0f + 120.0f * sin(y[0]),
      y1 = 300.0f + 120.0f * cos(y[0]),
      x2 = x1 + 120.0f * sin(y[1]),
      y2 = y1 + 120.0f * cos(y[1]);

궤적(trajectory) 그리기

이중 진자의 아래쪽 추의 궤적을 그릴 수 있습니다. sf::VertexArray에 인덱스를 늘려가면서 아래쪽 추의 위치를 할당하면 이는 아래쪽 추의 꼬리처럼 만들 수 있습니다.

trajectory[trajectoryIndex++].position = joint2[1].position;
if (trajectoryIndex == 1000){
    trajectoryIndex = 0;
}

FPS 측정

sf::RenderWindowsetFramerateLimit(unsigned int) 메서드를 이용해 렌더링 창의 FPS를 조절할 수 있으며, while 문을 돌면서 걸린 시간을 바탕으로 FPS를 측정할 수도 있습니다. 다만, FPS는 대개 각각의 프레임마다 급변하는 경우가 많기 때문에 이를 100회 측정한 평균을 내는 것이 더 합리적일수도 있습니다.

elapsedTime100 += elapsed;
elapsedTicks100++;
if (elapsedTicks100 == 100){
    fpsMeter.setString("FPS: " + std::to_string(static_cast<int>(100.0f / elapsedTime100)));
    elapsedTicks100 = 0;
    elapsedTime100 = 0.0f;
}

전체 코드

#include <valarray>
#include <SFML/Graphics.hpp>
#include <sstream>
#include <iomanip>

constexpr float PI = 3.141592f;
float m, l, g;


std::valarray<float> doublePendulumODE(float t, std::valarray<float> y){
    float theta1 = y[0], theta2 = y[1], p1 = y[2], p2 = y[3];

    /*
        theta1_diff = 6 / (m * l^2) * (2 * p1 - 3 * cos(theta1 - theta2) * p2) / (16 - 9 * cos^2(theta1 - theta2))
        theta2_diff = 6 / (m * l^2) * (8 * p2 - 3 * cos(theta1 - theta2) * p1) / (16 - 9 * cos^2(theta1 - theta2))
        p1_diff = -m * l^2 / 2 * (theta1_diff * theta2_diff * sin(theta1 - theta2) + 3 * (g / l) * sin(theta1))
        p2_diff = -m * l^2 / 2 * (-theta1_diff * theta2_diff * sin(theta1 - theta2) + (g / l) * sin(theta2))
    */

    float a = 6.0f / (m * l * l), 
          b = -0.5f * m * l * l, 
          c = 3 * cos(theta1 - theta2), 
          d = sin(theta1 - theta2),
          e = c * c,
          theta1Diff = a * (2.0f * p1 - c * p2) / (16.0f - e),
          theta2Diff = a * (8.0f * p2 - c * p1) / (16.0f - e),
          f = theta1Diff * theta2Diff,
          p1Diff = b * (f * d + 3.0f * g / l * sin(theta1)),
          p2Diff = b * (-f * d + g / l * sin(theta2));

    return { theta1Diff, theta2Diff, p1Diff, p2Diff };
}

void stepRK4(float dt, float &t, std::valarray<float> &y, std::valarray<float> (*ode)(float, std::valarray<float>)){
    std::valarray<float> dy1 { ode(t, y) * dt },
                         dy2 { ode(t + 0.5f * dt, y + 0.5f * dy1) * dt },
                         dy3 { ode(t + 0.5f * dt, y + 0.5f * dy2) * dt },
                         dy4 { ode(t + dt, y + dy3) * dt };

    y += (dy1 + 2.0f * dy2 + 2.0f * dy3 + dy4) / 6.0f;
    t += dt;
}

int main(){
    sf::Clock clock;
    float t = 0.0f; // elapsed seconds from simulation begin

    // FIELDS FOR FPS CHECK
    float elapsedTime100 = 0.0f;
    int elapsedTicks100 = 0;

    // OBJECTS
    sf::CircleShape obj1 { 5.0f }, obj2 { 5.0f }; // objects
    obj1.setFillColor(sf::Color::White);
    obj1.setOrigin(5.0f, 5.0f);
    obj2.setFillColor(sf::Color::White);
    obj2.setOrigin(5.0f, 5.0f);
    
    auto whiteZeroVertex = sf::Vertex({ 0.0f, 0.0f });
    whiteZeroVertex.color = sf::Color::White;
    sf::Vertex joint1[] = { sf::Vertex {{ 400.0f, 300.0f }}, sf::Vertex {{ 0.0f, 0.0f }} }, // joints
               joint2[] = { sf::Vertex {{ 0.0f, 0.0f }}, sf::Vertex {{ 0.0f, 0.0f }} };
    
    sf::VertexArray trajectory { sf::Points, 1000 }; // trajectory
    size_t trajectoryIndex = 0;
    for (int i = 0; i < 1000; ++i){
        trajectory[i].color = sf::Color::White;
    }

    sf::Font font;
    if (!font.loadFromFile("/System/Library/Fonts/Menlo.ttc")){
        return 1;
    }
    sf::Text fpsMeter { "", font, 24U }; // fps
    fpsMeter.setFillColor(sf::Color::White);
    fpsMeter.setPosition(20.0f, 20.0f);

    sf::Text inspectMeter { "", font, 24U }; // meter
    inspectMeter.setFillColor(sf::Color::White);
    inspectMeter.setPosition(20.0f, 80.0f);

    // SIMULATION PARAMETERS
    m = 1.0f; // mass
    l = 1.0f; // length of each pendulum
    g = 9.8f; // gravitational acceleration
    std::valarray<float> y { PI / 6.0f, PI / 3.0f, 0.0f, 0.0f }; // initial condition: theta1, theta2, p1, p2

    sf::RenderWindow window { { 800, 600 }, "Double Pendulum" };
    window.setFramerateLimit(144);
    while (window.isOpen()){
        sf::Event event;
        while (window.pollEvent(event)){
            if (event.type == sf::Event::Closed){
                window.close();
            }
        }

        // RK4 CALCULATION
        float elapsed = clock.restart().asSeconds();
        stepRK4(elapsed, t, y, doublePendulumODE);
        float x1 = 400.0f + 120.0f * sin(y[0]),
              y1 = 300.0f + 120.0f * cos(y[0]),
              x2 = x1 + 120.0f * sin(y[1]),
              y2 = y1 + 120.0f * cos(y[1]);

        // SET OBJECT LAYOUT
        obj1.setPosition(sf::Vector2f { x1, y1 });
        obj2.setPosition(sf::Vector2f { x2, y2 });
        joint1[1].position = obj1.getPosition();
        joint2[0].position = joint1[1].position;
        joint2[1].position = obj2.getPosition();
        trajectory[trajectoryIndex++].position = joint2[1].position;
        if (trajectoryIndex == 1000){
            trajectoryIndex = 0;
        }

        // FPS
        elapsedTime100 += elapsed;
        elapsedTicks100++;
        if (elapsedTicks100 == 100){
            fpsMeter.setString("FPS: " + std::to_string(static_cast<int>(100.0f / elapsedTime100)));
            elapsedTicks100 = 0;
            elapsedTime100 = 0.0f;
        }

        // INSPECTION
        std::ostringstream oss;
        oss << "t = " << std::setprecision(4) << t << "\ntheta1 = " << y[0] << "\ntheta2 = " << y[1];
        inspectMeter.setString(oss.str());

        // DRAW 
        window.clear();

        window.draw(obj1),
        window.draw(obj2);
        window.draw(joint1, 2, sf::Lines);
        window.draw(joint2, 2, sf::Lines);
        window.draw(trajectory);
        window.draw(fpsMeter);
        window.draw(inspectMeter);

        window.display();
    }

    return EXIT_SUCCESS;
}

profile
Undergraduate student in Korea University. Major in electrical engineering and computer science.

0개의 댓글