multiple_pendulum.cpp

#include<iostream>
#include<cstdio>
#include<cmath>
#include<vector>
#include "Eigen/Dense"
#include "Eigen/LU"

#define OUTPUT_ENERGY false
#define OUTPUT_A_T false
using namespace std;
using namespace Eigen;
const int INF = 1 << 29;
const double EPS = 1e-9;

typedef long long ll;


const int N = 5; //振り子の数
const double TERM = 40.0; // 実行する時間
const double init_l = 0.4;// 各棒の長さ
const double init_m = 0.0001; //各質点の質量
const double rad_init = 10.0;

double dt = 0.001; //微小時間
double t = 0.0; // 
const double pi = 3.141592653589793;
const double g = 9.8; //重力定数
double l[101];
double m[101];
VectorXd pvec(N);
VectorXd theta(N);
VectorXd thetad(N);
VectorXd w(N);
MatrixXd R(N,N);

double _m_sum[102];
struct Point {
    double x;
    double y;
};

double toRad(double angle){
    return angle / 180.0 *pi;
}

double difsin(int i, int j){
    double p = theta(i);
    double q = theta(j);
    return sin(p-q);
}
double difcos(int i, int j ){
    double p = theta(i);
    double q = theta(j);
    return cos(p-q);
}
double range_mass(int a, int b){
    return _m_sum[b+1] - _m_sum[a];
}

void init(){
    _m_sum[0] = 0.0;
    for (int i=0; i<N; i++){
        m[i] = init_m;
        if (i==N-1) m[i] = init_m*1000.0;
        l[i] = init_l;
        _m_sum[i+1] = _m_sum[i]+m[i];
        theta(i) = toRad(rad_init);
        thetad(i) = 0.0;
        pvec(i) = 0.0;
    }
}

void makeR(){
    for (int i = 0; i<N; i++){
        for (int j = 0; j<N; j++){
            if (j<i)
                R(i,j) = range_mass(i,N-1) * l[i] * l[j] * difcos(i,j);
            else
                R(i,j) = range_mass(j,N-1) * l[i] * l[j] * difcos(i,j);
        }
    }
}

void makeW(){
    for (int i = 0 ; i< N; i++){
        double s = 0.0;
        for (int j = 0; j<N; j++){
            if (i==j ) s+= range_mass(j,N-1) * g * l[i] * sin(theta(i));
            else if(i>j)
                s += range_mass(i,N-1) * l[i] * l[j] * thetad(i) * thetad(j) * difsin(i,j);
            else
                s += range_mass(j,N-1) * l[i] * l[j] * thetad(i) * thetad(j) * difsin(i,j);
        }
        w(i) = -s;
    }
}

vector<Point> _pos(){
    vector<Point> v;
    for(int i=0; i<N; i++){
        Point p;
        p.x = l[i] * sin(theta(i));
        p.y = l[i] * cos(theta(i));
        if (i!=0){
            p.x += v[i-1].x;
            p.y += v[i-1].y;
        }
        v.push_back(p);
    }
    return v;
}

void printResult(){
    vector<Point> v = _pos();

    printf("%lf", t);
    for(int i = 0; i<N;i++)
        printf("\t%lf\t%lf", v[i].x, v[i].y);
    printf("\n");

}

double sumEnergy(){
    double T = 0.0;
    double U = 0.0;

    for(int i = 0; i<N; i++){
        for (int j = 0; j<=i; j++){
            T += 0.5*m[i]*l[j]*l[j]*thetad(j)*thetad(j);
            for (int k = j+1; k<=i; k++){
                T+=m[i]*l[j]*l[k]*thetad(j)*thetad(k)*difcos(j,k);
            }
            U -= m[i] * g * l[j]*cos(theta(j));
        }
    }
    return T+U;

}

int main(int argc, char * argv[])
{
    init();
    double first = -100;
    
    makeW();
    int idx = 0;
    double bef_x = 10000000.0;
    double first_x;
    int state = 1;
    double ini_angle;
    double last_period = 0.0;
    for ( t = 0.0f; t < TERM; t+=dt){
        t += dt;
        makeW();
        pvec = pvec + w*dt;
        makeR();
        thetad = R.inverse()*pvec;
        theta = theta + thetad*dt;

        if (OUTPUT_A_T){
            // calculate Amp and T
            vector<Point> v = _pos();
            if (bef_x > 1000000){
                first_x = v[N-1].x;
                bef_x = first_x;
                ini_angle = theta(N-1);
            }
            else if (state*(bef_x - v[N-1].x) < 0) {
                double amp = (double)N*init_l * (ini_angle - theta(N-1))*state;
                printf("%lf\t%lf\n", t-last_period, amp);
                // initialization for the next half of period
                last_period = t;
                first_x = v[N-1].x;
                bef_x = first_x;
                state *= -1;
                ini_angle = theta(N-1);
            }
            bef_x = v[N-1].x;
        }
        else if (OUTPUT_ENERGY)
        {
            double sum_en = sumEnergy();
            if(first < -50) first = sum_en;
            if (idx++%100 == 0) printf("%lf\t%lf\n", t,sum_en-first);
            if (sum_en > 10000.0) break;
        }
        else
            printResult();

    }

    return 0;
}