조금 더 깔끔한 이중진자 시뮬레이션

https://youtu.be/k2oe_A52pk0

전보다 더 완벽해진 이중 진자 시뮬레이션

 

더 깔끔한 코드와 여러 개의 진자를 동시에 굴릴 수 있게 작성했음(영상에서는 하나밖에 안보이지만 진자를 추가하는 것 자체는 아주 쉬움). 저번에 짠 코드는 좀 많이 더럽긴 함.

 

원래는 그냥 SFML 기술 실증 프로젝트였는데 막상 손대다 보니 아예 중소규모 프로젝트가 되버림.

 

깃헙 주소: https://github.com/hydrogendeuteride/DoublePendulum_Continued

Visual Studio 깃은 처음 써봐서 아직 많이 미숙함.

그리고 파워셸 너무 불편함.

 

코드(클릭하면 나옴)

 

#ifndef _PENDULUM_H_
#define _PENDULUM_H_

#include <eigen3/Eigen/Dense>
#include <tuple>

typedef Eigen::Vector4d State;

constexpr double g = 9.81;

class Pendulum
{
private:
    double m1, m2, l1, l2;  //mass1, mass2, legth1, length2
    State state;            //theta, phi, generalized momentum, generalized momentum

    State RungeKutta4(const State& s, double timestep)
        //because Derivative is member function, you can`t put this function out of class 
        //in this class structure. 
    {
        State k1, k2, k3, k4;

        k1 = Derivative(s) * timestep;
        k2 = Derivative(k1 * 0.5 + s) * timestep;
        k3 = Derivative(k2 * 0.5 + s) * timestep;
        k4 = Derivative(k3 + s) * timestep;

        return (k1 + k2 * 2.0 + k3 * 2.0 + k4) / 6 + s;
    }

    State Derivative(const State& s)
    {
        State deriv;

        double u = m2 / m1;
        double dTheta = s(0) - s(1);

        double sinT = std::sin(dTheta);
        double cosT = std::cos(dTheta);

        double A1 = (s(2) * s(3) * sinT) / (m1 * std::pow(l1, 2) *
            (1 + u * sinT * sinT));

        double A2 = ((s(2) * s(2) * u - 2 * s(2) * s(3) * u * cosT +
            s(3) * s(3) * (1 + u)) * std::sin(2.0 * dTheta)) /
            (2 * m1 * l1 * l1 * (1 + (u * sinT * sinT)) * (1 + (u * sinT * sinT)));

        deriv(2) = -m1 * (1 + u) * g * l1 * std::sin(s(0)) - A1 + A2;
        deriv(3) = -m1 * u * g * l1 * std::sin(s(1)) + A1 - A2;

        deriv(0) = (s(2) - (s(3) * cosT)) /
            (m1 * l1 * l1 * (1 + u * sinT * sinT));
        deriv(1) = (s(3) * (1 + u) - s(2) * u * cosT) /
            (m1 * l1 * l1 * (1 + u * sinT * sinT));

        return deriv;
    }

public:
    Pendulum(double m1, double m2, double l1, double l2, State s)   //L2 is not working, L1 == L2
        : m1(m1), m2(m2), l1(l1), l2(l2), state(std::move(s))
    {};

    std::tuple<double, double> getTwoAngles()
    {
        return std::make_tuple(this->state(0), this->state(1));
    }

    void CalculateMove(double timestep)
    {
        State temp = RungeKutta4(this->state, timestep);
        this->state = temp;
    }
};

#endif // !_PENDULUM_H

pendulum.h

 

#ifndef _DRAWABLE_H_
#define _DRAWABLE_H_

#include <vector>
#include <SFML/Graphics.hpp>

namespace Object
{
    class Pendulum
    {

        /*
         * ------------------------
         *             |*
         *      theta  |  *
         *             |   *
         *                  Ball1
         *                       *
         *          phi                 *
         *                                     *
         *                                    Ball2
         */

    private:
        sf::CircleShape Ball_Up;
        sf::CircleShape Ball_Down;

        sf::Vertex Line1[2];
        sf::Vertex Line2[2];

        float BallSize, Length, PosX, PosY;

        sf::VertexArray curve;

    public:
        Pendulum(float BallSize, float Length, float PosX, float PosY, double Theta1, double Theta2, sf::Color Color)
            : BallSize(BallSize), Length(Length), PosX(PosX), PosY(PosY)
        {
            Ball_Up.setRadius(this->BallSize);
            Ball_Down.setRadius(this->BallSize);

            Ball_Up.setFillColor(Color);
            Ball_Down.setFillColor(Color);


            float x_ref_up, x_ref_down, y_ref_up, y_ref_down;

            x_ref_up = static_cast<float>(std::sin(Theta1)) * this->Length;
            y_ref_up = static_cast<float>(std::cos(Theta1)) * this->Length;

            x_ref_down = static_cast<float>(std::sin(Theta2)) * this->Length;
            y_ref_down = static_cast<float>(std::cos(Theta2)) * this->Length;

            Line1[0] = sf::Vertex(
                sf::Vector2f(this->PosX, this->PosY));
            Line1[1] = sf::Vertex(
                sf::Vector2f(x_ref_up + this->PosX, y_ref_up + this->PosY));


            Ball_Up.setPosition(
                sf::Vector2f(x_ref_up + this->PosX - BallSize,
                    y_ref_up + this->PosY - BallSize));


            Line2[0] = sf::Vertex(sf::Vector2f(x_ref_up + this->PosX,
                y_ref_up + this->PosY));
            Line2[1] = sf::Vertex(
                sf::Vector2f(x_ref_up + x_ref_down + this->PosX,
                    y_ref_up + y_ref_down + this->PosY));

            Ball_Down.setPosition(
                sf::Vector2f(x_ref_up + x_ref_down + this->PosX - BallSize,
                    y_ref_up + y_ref_down + this->PosY - BallSize));
        }

        void Move(double Theta1, double Theta2)
        {
            float x_ref_up, x_ref_down, y_ref_up, y_ref_down;

            x_ref_up = static_cast<float>(std::sin(Theta1)) * this->Length;
            y_ref_up = static_cast<float>(std::cos(Theta1)) * this->Length;

            x_ref_down = static_cast<float>(std::sin(Theta2)) * this->Length;
            y_ref_down = static_cast<float>(std::cos(Theta2)) * this->Length;

            Line1[0] = sf::Vertex(
                sf::Vector2f(this->PosX, this->PosY));
            Line1[1] = sf::Vertex(
                sf::Vector2f(x_ref_up + this->PosX, y_ref_up + this->PosY));


            Ball_Up.setPosition(
                sf::Vector2f(x_ref_up + this->PosX - BallSize,
                    y_ref_up + this->PosY - BallSize));


            Line2[0] = sf::Vertex(sf::Vector2f(x_ref_up + this->PosX,
                y_ref_up + this->PosY));
            Line2[1] = sf::Vertex(
                sf::Vector2f(x_ref_up + x_ref_down + this->PosX,
                    y_ref_up + y_ref_down + this->PosY));

            Ball_Down.setPosition(
                sf::Vector2f(x_ref_up + x_ref_down + this->PosX - BallSize,
                    y_ref_up + y_ref_down + this->PosY - BallSize));

            curve.append(sf::Vertex(
                sf::Vector2f(x_ref_up + x_ref_down + this->PosX - BallSize,
                    y_ref_up + y_ref_down + this->PosY - BallSize)));
        }

        void Draw(sf::RenderWindow* window)
        {
            window->draw(Line1, 2, sf::Lines);
            window->draw(Line2, 2, sf::Lines);

            window->draw(Ball_Up);
            window->draw(Ball_Down);

            window->draw(curve);
        }
    };
}

#endif // !_DRAWABLE_H_

drawable.h

#ifdef _WIN64
#define _USE_MATH_DEFINES
#endif

#include <iostream>
#include <cmath>
#include "Pendulum.h"
#include "Drawable.h"

constexpr double dt = 0.008;

int main()
{
    sf::RenderWindow window (sf::VideoMode (1920, 1200), "Double Pendulum");
    window.setVerticalSyncEnabled(true);

    auto deg2rad = [](double deg) { return deg * (M_PI / 180.0); };

    double deg1 =110.0;
    double deg2 = 110.0;

    Object::Pendulum pendulum(50, 300, 960, 540,
        deg2rad(deg1), deg2rad(deg2), sf::Color::Cyan);

    State s(deg2rad(deg1), deg2rad(deg2), 0.0, 0.0);
    Pendulum p(1.0, 1.0, 1.0, 1.0, s);

    sf::Clock clock;
    float fps, currentTime;

    int record = 0;

    while (window.isOpen ())
    {
        sf::Event event;
        while (window.pollEvent (event))
        {
            if (event.type == sf::Event::Closed)
                window.close ();
        }

        window.clear ();
        
        if (record > 1200)
        {
            p.CalculateMove(dt);
            pendulum.Move(std::get<0>(p.getTwoAngles()), std::get<1>(p.getTwoAngles()));
        }

        record++;

        pendulum.Draw (&window);

        window.display ();

        currentTime = clock.restart().asSeconds();
        fps = 1.0f / (currentTime);
        
        std::cout << fps << "\n";
    }

    return 0;
}

main.cc

 

코드 컴파일은  Visual Studio 2022로 했고, Eigen 라이브러리와 SFML 라이브러리가 필요함.

 

리눅스는 안돌려봐서 모름.