Create a gist now

Instantly share code, notes, and snippets.

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