/*----------------------------------------------------------------------------------
2重振り子の方程式とヤコビ行列（線形化したハイパーカオス方程式）
double pendulum equations and the Jacobian (linearized double pendulum equations)
solve_double_pendulum_eqs.c
----------------------------------------------------------------------------------*/
#include "double_pendulum_lyap.h"
void double_pendulum_eqs( double v[DIM], double dfu[DIM][DIM], double dvdt[DIM], double df_k[DIM][DIM] )
{
int i;
int j;
int k;
double d2;
double th1;
double th2;
double w1;
double w2;
double delta;
double temp;
double temp2;
double temp_w1;
double temp_w2;
double cos_delta;
double sin_delta;
double cos2_delta;
double sin2_delta;
double u;
double jac[5];
double df[DIM][DIM];
th1 = v[0]; // d theta1/dt
th2 = v[1]; // d theta2/dt
w1 = v[2]; // d omega1/dt
w2 = v[3]; // d omega2/dt
delta = th1 - th2;
cos_delta = cos( delta );
sin_delta = sin( delta );
cos2_delta = cos_delta * cos_delta;
sin2_delta = sin_delta * sin_delta;
u = (M1 + M2)/(double)M2;
temp_w1 = G * ( cos_delta * sin( th2 ) - u * sin( th1 ) ) - ( L1 * w1 * w1 * cos_delta + L2 * w2 * w2 ) * sin_delta;
temp_w2 = G * u * ( cos_delta * sin( th1 ) - sin( th2 ) ) + ( u * L1 * w1 * w1 + L2 * w2 * w2 * cos_delta ) * sin_delta;
temp = u - cos2_delta;
temp2 = temp * temp;
/*------------------------------------------------------------------*/
// 2重振り子の方程式
// double_pendulum equations
dvdt[0] = w1;
dvdt[1] = w2;
dvdt[2] = temp_w1/(double)( L1 * temp );
dvdt[3] = temp_w2/(double)( L2 * temp );
/*------------------------------------------------------------------*/
// ヤコビ行列
// Jacobian matrix
df[0][0] = 0.0;
df[0][1] = 0.0;
df[0][2] = 1.0;
df[0][3] = 0.0;
df[1][0] = 0.0;
df[1][1] = 0.0;
df[1][2] = 0.0;
df[1][3] = 1.0;
/*----------------------------------------------------------------------------------------*/
jac[0] = L1 * w1 * w1 * sin2_delta + G * (-sin( th2 ) * sin_delta - u * cos( th1 ) )
- cos_delta * ( L1 * w1 * w1 * cos_delta + L2 * w2 * w2 );
jac[1] = 2.0 * cos_delta * sin_delta * ( G * ( sin( th2 ) * cos_delta - u * sin( th1 ) )
- ( L1 * w1 * w1 * cos_delta + L2 * w2 *w2 ) * sin_delta );
d2 = jac[0]/(double)(L1 * temp) - jac[1]/(double)(L1 * temp2);
df[2][0] = d2;
jac[0] = -1.0 * L1 * w1 * w1 * sin2_delta + G * ( cos( th2 ) * cos_delta + sin( th2 ) * sin_delta )
+ cos_delta * ( L1 * w1 * w1 * cos_delta + L2 * w2 * w2 );
jac[1] = -2.0 * cos_delta * sin_delta * ( G * ( sin( th2 ) * cos_delta - u * sin( th1 ) )
- ( L1 * w1 * w1 * cos_delta + L2 * w2 * w2 ) * sin_delta );
d2 = jac[0]/(double)(L1 * temp) - jac[1]/(double)(L1 * temp2);
df[2][1] = d2;
df[2][2] = ( -2.0 * w1 * cos_delta * sin_delta )/(double)temp;
df[2][3] = ( -2.0 * L2 * w2 * sin_delta )/(double)( L1 * temp );
/*----------------------------------------------------------------------------------------*/
jac[0] = -1.0 * L2 * w2 * w2 * sin2_delta + G * u * ( cos( th1 ) * cos_delta - sin( th1 ) * sin_delta )
+ cos_delta * ( L2 * w2 * w2 * cos_delta + L1 * u * w1 * w1 );
jac[1] = 2.0 * cos_delta * sin_delta * ( ( L2 * w2 * w2 * cos_delta + L1 * u * w1 * w1 ) * sin_delta
+ G * u * ( sin( th1 ) * cos_delta - sin( th2 ) ) );
d2 = jac[0]/(double)(L2 * temp) - jac[1]/(double)(L2 * temp2);
df[3][0] = d2;
jac[0] = L2 * w2 * w2 * sin2_delta + G * u * ( sin( th1 ) * sin_delta - cos( th2 ) )
- cos_delta * ( L2 * w2 * w2 * cos_delta + L1 * u * w1 * w1 );
jac[1] = -2.0 * cos_delta * sin_delta * ( G * u * ( sin( th1 ) * cos_delta - sin( th2 ) )
+ ( L2 * w2 * w2 * cos_delta + L1 * u * w1 * w1 ) * sin_delta );
d2 = jac[0]/(double)(L2 * temp) - jac[1]/(double)(L2 * temp2);
df[3][1] = d2;
df[3][2] = ( 2.0 * L1 * u * w1 * sin_delta )/(double)( L2 * temp );
df[3][3] = ( 2.0 * w2 * cos_delta * sin_delta )/(double)temp;
/*
｜ df[0][0] df[0][1] df[0][2] df[0][3] ｜
｜ ｜
｜ df[1][0] df[1][1] df[1][2] df[1][3] ｜
df = ｜ ｜
｜ df[2][0] df[2][1] df[2][2] df[2][3] ｜
｜ ｜
｜ df[3][0] df[3][1] df[3][2] df[3][3] ｜
*/
// ヤコビ行列を用いて微小変位ベクトルを計算
// calculate the micro-displacement vectors using the Jacobian matrix
for ( i = 0; i < DIM; i++ ) {
for ( j = 0; j < DIM; j++ ) {
d2 = 0.0;
for ( k = 0; k < DIM; k++ ) {
d2 += df[i][k] * dfu[k][j];
}
df_k[i][j] = d2;
}
}
}
/*-----------------------------------------------------------------------------------------*/
void solve_double_pendulum_eqs_using_RK4( double v[DIM], double u[DIM][DIM] )
{
int i;
int j;
double vv[DIM];
double v_k1[DIM], v_k2[DIM], v_k3[DIM], v_k4[DIM];
double dfu[DIM][DIM];
double df_k1[DIM][DIM], df_k2[DIM][DIM], df_k3[DIM][DIM], df_k4[DIM][DIM];
/*******************************************************/
for (i = 0; i < DIM; i++) {
vv[i] = v[i];
for (j = 0; j < DIM; j++) {
dfu[i][j] = u[i][j];
}
}
/*******************************************************/
// vvとdfuを渡し、v_k1とdf_k1の計算値を得る。
// pasin_delta vv and dfu, and get the calculated values of v_k1 and df_k1
double_pendulum_eqs( vv, dfu, v_k1, df_k1 );
for (i = 0; i < DIM; i++) {
vv[i] = v[i] + DELTA_T * v_k1[i]/(double)2.0;
}
for (i = 0; i < DIM; i++) {
for (j = 0; j < DIM; j++) {
dfu[i][j] = u[i][j] + DELTA_T * df_k1[i][j]/(double)2.0;
}
}
/*******************************************************/
double_pendulum_eqs( vv, dfu, v_k2, df_k2 );
for (i = 0; i < DIM; i++) {
vv[i] = v[i] + DELTA_T * v_k2[i]/(double)2.0;
}
for (i = 0; i < DIM; i++) {
for (j = 0; j < DIM; j++) {
dfu[i][j] = u[i][j] + DELTA_T * df_k2[i][j]/(double)2.0;
}
}
/*******************************************************/
double_pendulum_eqs( vv, dfu, v_k3, df_k3 );
for (i = 0; i < DIM; i++) {
vv[i] = v[i] + DELTA_T * v_k3[i];
}
for (i = 0; i < DIM; i++) {
for (j = 0; j < DIM; j++) {
dfu[i][j] = u[i][j] + DELTA_T * df_k3[i][j];
}
}
/*******************************************************/
double_pendulum_eqs( vv, dfu, v_k4, df_k4 );
for (i = 0; i < DIM; i++) {
v[i] += DELTA_T * (v_k1[i] + 2.0 * v_k2[i] + 2.0 * v_k3[i] + v_k4[i])/(double)6.0;
}
for (i = 0; i < DIM; i++) {
for (j = 0; j < DIM; j++) {
u[i][j] += DELTA_T * (df_k1[i][j] + 2.0 * df_k2[i][j] + 2.0 * df_k3[i][j] + df_k4[i][j])/(double)6.0;
}
}
/*******************************************************/
}