Double Pendulum

main.cpp

#include <cstdio>
#include <cstdlib>
#include <cmath>

#include <unistd.h>

#include <vector>

const double length1 = 1;
const double length2 = 1;
const double mass1 = 1;
const double mass2 = 1;
const double gravity = 9.8;
const double dt = 1.0/100;


#if true

bool update_fixed_point(const double* const x, double* const x_)
{

  static double x_middle[4];
  for(int d=0; d<4; ++d) {
    x_middle[d] = 0.5 * ( x[d] + x_[d] );
  }
  const double delta_theta = x_middle[0] - x_middle[1];
  const double cos_delta = cos(delta_theta);
  const double sin_delta = sin(delta_theta);
  const double sin_2delta = 2 * cos_delta * sin_delta;

  const double m1pm2sin2delta = ( mass1 + mass2 * sin_delta * sin_delta);

  const double Dtheta1 = x_middle[2];
  const double Dtheta2 = x_middle[3];
  const double sin1 = sin(x_middle[0]);
  const double sin2 = sin(x_middle[1]);

  bool stop = true;
  const double epsilon_th = 1e-12;
  
  const double x_0_new = x[0] + dt * x_middle[2];
  if(fabs(x_[0]- x_0_new) > epsilon_th ) stop = false;
  x_[0] = x_0_new;

  const double x_1_new = x[1] + dt * x_middle[3];
  if(fabs(x_[1]- x_1_new) > epsilon_th ) stop = false;
  x_[1] = x_1_new;

  x_[2] = x[2] + dt * ( - mass1 * gravity * sin1 - mass2 * ( gravity * sin1 + length2 * Dtheta2 * Dtheta2 * sin_delta + ( length1 * Dtheta1 * Dtheta1 * sin_delta - gravity * sin2 ) * cos_delta ) ) / ( length1 * m1pm2sin2delta );
    x_[3] = x[3] + dt * ( (mass1 + mass2) * ( length1 * Dtheta1 * Dtheta1 * sin_delta - gravity * sin2 + gravity * sin1 * cos_delta ) + mass2 * length2 * Dtheta2 * Dtheta2 * cos_delta * sin_delta ) / ( length2 * m1pm2sin2delta );

  return stop;
}

double energy(const double* const x)
{
  const double delta_theta = x[0] - x[1];
  const double cos_delta = cos(delta_theta);
  const double sin_delta = sin(delta_theta);
  const double sin_2delta = 2 * cos_delta * sin_delta;

  const double Dtheta1 = x[2];
  const double Dtheta2 = x[3];
  const double cos1 = cos(x[0]);
  const double cos2 = cos(x[1]);


  const double K = 0.5 * ( mass1 + mass2 ) * length1 * length1 * Dtheta1 * Dtheta1 + 0.5 * mass2 * length2 * length2 * Dtheta2 * Dtheta2 + mass2 * length1 * length2 * Dtheta1 * Dtheta2 * cos_delta; 
  //const double U = ( mass1 + mass2) * gravity * length1 * (1-cos1) + mass2 * gravity * length2 * ( 1 - cos2);
  const double U = ( mass1 + mass2) * gravity * length1 * (-cos1) + mass2 * gravity * length2 * ( - cos2);

  return K + U;
}


#else

bool update_fixed_point(const double* const x, double* const x_)
{

  static double x_middle[4];
  for(int d=0; d<4; ++d) {
    x_middle[d] = 0.5 * ( x[d] + x_[d] );
  }
  const double delta_theta = x_middle[0] - x_middle[1];
  const double cos_delta = cos(delta_theta);
  const double sin_delta = sin(delta_theta);
  const double sin_2delta = 2 * cos_delta * sin_delta;

  const double l1l2m1pm2sin2delta = length1 * length2 * ( mass1 + mass2 * sin_delta * sin_delta);
  const double l1l2m1pm2sin2delta_inv = 1.0/l1l2m1pm2sin2delta;

  const double p1 = x_middle[2];
  const double p2 = x_middle[3];
  const double sin1 = sin(x_middle[0]);
  const double sin2 = sin(x_middle[1]);
  const double c1 = p1 * p2 * sin_delta * l1l2m1pm2sin2delta_inv;
  const double c2 = (length2 * length2 * mass2 * p1 * p1 + length1 * length1 * ( mass1 + mass2 ) * p2 * p2 - 2 * length1 * length2 * mass2 * p1 * p2 * cos_delta) * sin_2delta * 0.5 * l1l2m1pm2sin2delta_inv * l1l2m1pm2sin2delta_inv;

  bool stop = true;
  const double epsilon_th = 1e-10;
  
  //x_[0] = x[0] +
  const double x_0_new = x[0] + dt * (length2 * p1 - length1 * p2 * cos_delta ) * l1l2m1pm2sin2delta_inv / length1;
  if(fabs(x_[0]- x_0_new) > epsilon_th ) stop = false;

  x_[0] = x_0_new;

  const double x_1_new = x[1] + dt * (length1 * (mass1 + mass2) * p2 - length2 * mass2 * p1 * cos_delta) * l1l2m1pm2sin2delta_inv / ( length2 * mass2 );
  if(fabs(x_[1]- x_1_new) > epsilon_th ) stop = false;

  x_[1] = x_1_new;

  x_[2] = x[2] + dt * ( - ( mass1 + mass2 ) * gravity * length1 * sin1 - c1 + c2);
  x_[3] = x[3] + dt * ( - mass2 * gravity * length2 * sin2 + c1 - c2 );

  return stop;
}

double energy(const double* const x)
{

  const double delta_theta = x[0] - x[1];
  const double cos_delta = cos(delta_theta);
  const double sin_delta = sin(delta_theta);
  // const double sin_2delta = 2 * cos_delta * sin_delta;

  const double p1 = x[2];
  const double p2 = x[3];
  const double cos1 = cos(x[0]);
  const double cos2 = cos(x[1]);

  const double K = ( length2*length2 * mass2 * p1*p1 + length1 * length1 * ( mass1 + mass2 ) * p2 * p2 - 2 * mass2 * length1 * length2 * p1 * p2 * cos_delta ) / ( 2 * length1 * length1 * length2 * length2 * mass2 * (mass1 + mass2 * sin_delta * sin_delta));
  const double U = - mass2 * gravity * length2 * cos2 - ( mass1 + mass2 ) * gravity * length1 * cos1;
  return K + U;
}

#endif

int main(const int argc, char* const argv[])
{

  double x_array[8] = {};
  double* x = &x_array[0];
  double* x_ = &x_array[4];
  x[0] = 3;
  x[2] = 1;
  double t = 0;

  for(int n=0; true; ++n) {
    int f;
    for(int g=0; g<1000; ++g) {
      for(f=0; true; ++f) {
        if(update_fixed_point(x, x_))
          break;
      }
      std::swap(x, x_);
      // if(x[0]>M_PI) x[0] -= 2*M_PI;
      // else if(x[0]<-M_PI) x[0] += 2*M_PI;

      // if(x[1]>M_PI) x[1] -= 2*M_PI;
      // else if(x[1]<-M_PI) x[1] += 2*M_PI;

      t += dt;
    }
    fprintf(stdout, "%g %g %g %g %g %g %d\n", t, x[0], x[1], x[2], x[3], energy(x), f);
  }

}